-
-# PythonRobotics
-
-
-
-[](https://ci.appveyor.com/project/AtsushiSakai/pythonrobotics)
-[](https://codecov.io/gh/AtsushiSakai/PythonRobotics)
-
Python codes for robotics algorithm.
@@ -74,8 +65,30 @@ Python codes for robotics algorithm.
* [1Password](#1password)
* [Authors](#authors)
+# How to use it with repository: Ardupilot, AirSim and Unreal 4.7
+
+git clone https://github.com/ss48/PythonRobotics_AirSim_Unreal.git
+1. Install the AirSim and Unreal 4.7
+2. Copy the settings.json file in this repository into your Documents/AirSim folder.
+3. Run the Blocks.uproject located in the Unreal folder of Airsim.
+
+
+4. Install the following:
+
+sudo pip install shapely
+
+sudo pip install dronekit
+
+pip install dronekit-sitl
+
+
+5. Run the ./libraries/SITL/examples/Airsim/follow-1-copter.sh in the Terminal
+6. Download the Takeoff_arm.py in this repository. Run the script to make the drones go into Guided mode, Arm, and Takeoff, and make sure MavProxy for both drones is running.
+
+
# What is this?
+
This is a Python code collection of robotics algorithms.
Features:
diff --git a/Takeoff_arm.py b/Takeoff_arm.py
new file mode 100644
index 0000000000..ee3a5659db
--- /dev/null
+++ b/Takeoff_arm.py
@@ -0,0 +1,70 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+© Copyright 2015-2016, 3D Robotics.
+guided_set_speed_yaw.py: (Copter Only)
+This example shows how to move/direct Copter and send commands in GUIDED mode using DroneKit Python.
+Example documentation: http://python.dronekit.io/examples/guided-set-speed-yaw-demo.html
+"""
+from __future__ import print_function
+import numpy as np
+from dronekit import connect, VehicleMode, LocationGlobalRelative
+from pymavlink import mavutil
+import time
+import math
+import threading
+import matplotlib.pyplot as plt
+import argparse
+from mpl_toolkits.mplot3d import Axes3D
+
+# Set up option parsing to get connection string
+parser = argparse.ArgumentParser(description='Control Copter and send commands in GUIDED mode')
+parser.add_argument('--connect1', help="Vehicle 1 connection target string.")
+parser.add_argument('--connect2', help="Vehicle 2 connection target string.")
+args = parser.parse_args()
+
+connection_string1 = args.connect1 if args.connect1 else 'udpin:127.0.0.1:14552'
+connection_string2 = args.connect2 if args.connect2 else 'udpin:127.0.0.1:14553'
+
+# Connect to the Vehicles
+print('Connecting to vehicle on: %s' % connection_string1)
+vehicle1 = connect(connection_string1, wait_ready=True)
+
+print('Connecting to vehicle on: %s' % connection_string2)
+vehicle2 = connect(connection_string2, wait_ready=True)
+
+# Function to arm and takeoff
+def arm_and_takeoff(vehicle, aTargetAltitude):
+ """
+ Arms vehicle and fly to aTargetAltitude.
+ """
+ # time.sleep(1)
+
+ print("Arming motors")
+ vehicle.mode = VehicleMode("GUIDED")
+ time.sleep(3)
+ vehicle.armed = True
+
+
+ print("Taking off!")
+ vehicle.simple_takeoff(aTargetAltitude)
+ # while True:
+ print(f" Altitude: {vehicle.location.global_relative_frame.alt}")
+ # if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95:
+ print("Reached target altitude")
+ # break
+ time.sleep(3)
+
+
+
+# Arm and take off to altitude of 5 meters for both vehicles
+arm_and_takeoff(vehicle1, 5)
+time.sleep(10) # Wait for some time to ensure vehicle1 has taken off
+arm_and_takeoff(vehicle2, 5)
+time.sleep(15)
+print("Both vehicles are in the air.")
+current_value1 = vehicle1.parameters['WPNAV_SPEED_UP']
+current_value2 = vehicle2.parameters['WPNAV_SPEED_UP']
+print(f"WPNAV_SPEED_UP parameter set to: {current_value1} for vehicle 1")
+print(f"WPNAV_SPEED_UP parameter set to: {current_value2} for vehicle 2")
+
diff --git a/Trav_SalMan.py b/Trav_SalMan.py
new file mode 100644
index 0000000000..1a6ea6429e
--- /dev/null
+++ b/Trav_SalMan.py
@@ -0,0 +1,195 @@
+import copy
+import matplotlib.pyplot as plt
+import networkx as nx
+import numpy as np
+import numpy.random as rnd
+import tsplib95
+import tsplib95.distances as distances
+
+from alns import ALNS
+from alns.accept import HillClimbing
+from alns.select import RouletteWheel
+from alns.stop import MaxRuntime
+
+SEED = 7654
+DATA = tsplib95.load('/home/dell/ALNS/examples/data/xqf131.tsp')
+CITIES = list(DATA.node_coords.keys())
+
+COORDS = DATA.node_coords.values()
+DIST = np.empty((len(COORDS) + 1, len(COORDS) + 1))
+
+for row, coord1 in enumerate(COORDS, 1):
+ for col, coord2 in enumerate(COORDS, 1):
+ DIST[row, col] = distances.euclidean(coord1, coord2)
+
+SOLUTION = tsplib95.load('/home/dell/ALNS/examples/data/xqf131.opt.tour')
+OPTIMAL = DATA.trace_tours(SOLUTION.tours)[0]
+
+print(f"Total optimal tour length is {OPTIMAL}.")
+
+
+def draw_graph(graph, only_nodes=False, ax=None, title=None):
+ """
+ Helper method for drawing TSP (tour) graphs.
+ """
+ if ax is None:
+ fig, ax = plt.subplots(figsize=(12, 6))
+
+ if only_nodes:
+ nx.draw_networkx_nodes(graph, DATA.node_coords, node_size=25, ax=ax)
+ else:
+ nx.draw_networkx(graph, DATA.node_coords, node_size=25, with_labels=False, ax=ax)
+
+ if title:
+ ax.set_title(title)
+
+
+class TspState:
+ """
+ Solution class for the TSP problem. It has two data members, nodes, and edges.
+ nodes is a list of IDs. The edges data member, then, is a mapping from each node
+ to their only outgoing node.
+ """
+
+ def __init__(self, nodes, edges):
+ self.nodes = nodes
+ self.edges = edges
+
+ def objective(self):
+ """
+ The objective function is simply the sum of all individual edge lengths,
+ using the rounded Euclidean norm.
+ """
+ return sum(DIST[node, self.edges[node]] for node in self.nodes)
+
+ def to_graph(self):
+ """
+ NetworkX helper method.
+ """
+ graph = nx.Graph()
+
+ for node in self.nodes:
+ graph.add_node(node, pos=DATA.node_coords[node])
+
+ for node_from, node_to in self.edges.items():
+ graph.add_edge(node_from, node_to)
+
+ return graph
+
+
+DEGREE_OF_DESTRUCTION = 0.1
+
+
+def edges_to_remove(state):
+ return int(len(state.edges) * DEGREE_OF_DESTRUCTION)
+
+
+def worst_removal(current, rnd_state):
+ """
+ Worst removal iteratively removes the 'worst' edges, that is,
+ those edges that have the largest distance.
+ """
+ destroyed = copy.deepcopy(current)
+
+ worst_edges = sorted(destroyed.nodes,
+ key=lambda node: DIST[node, destroyed.edges[node]])
+
+ for idx in range(edges_to_remove(current)):
+ del destroyed.edges[worst_edges[-(idx + 1)]]
+
+ return destroyed
+
+
+def path_removal(current, rnd_state):
+ """
+ Removes an entire consecutive sub-path, that is, a series of
+ contiguous edges.
+ """
+ destroyed = copy.deepcopy(current)
+
+ node_idx = rnd_state.choice(len(destroyed.nodes))
+ node = destroyed.nodes[node_idx]
+
+ for _ in range(edges_to_remove(current)):
+ node = destroyed.edges.pop(node)
+
+ return destroyed
+
+
+def random_removal(current, rnd_state):
+ """
+ Random removal iteratively removes random edges.
+ """
+ destroyed = copy.deepcopy(current)
+
+ for idx in rnd_state.choice(len(destroyed.nodes),
+ edges_to_remove(current),
+ replace=False):
+ del destroyed.edges[destroyed.nodes[idx]]
+
+ return destroyed
+
+
+def would_form_subcycle(from_node, to_node, state):
+ """
+ Ensures the proposed solution would not result in a cycle smaller
+ than the entire set of nodes.
+ """
+ for step in range(1, len(state.nodes)):
+ if to_node not in state.edges:
+ return False
+
+ to_node = state.edges[to_node]
+
+ if from_node == to_node and step != len(state.nodes) - 1:
+ return True
+
+ return False
+
+
+def greedy_repair(current, rnd_state):
+ """
+ Greedily repairs a tour, stitching up nodes that are not departed
+ with those not visited.
+ """
+ visited = set(current.edges.values())
+
+ shuffled_idcs = rnd_state.permutation(len(current.nodes))
+ nodes = [current.nodes[idx] for idx in shuffled_idcs]
+
+ while len(current.edges) != len(current.nodes):
+ node = next(node for node in nodes
+ if node not in current.edges)
+
+ unvisited = {other for other in current.nodes
+ if other != node
+ if other not in visited
+ if not would_form_subcycle(node, other, current)}
+
+ nearest = min(unvisited,
+ key=lambda other: DIST[node, other])
+
+ current.edges[node] = nearest
+ visited.add(nearest)
+
+ return current
+
+
+# Initialize random state and initial solution
+random_state = rnd.RandomState(SEED)
+state = TspState(CITIES, {})
+
+# Initial greedy repair solution
+init_sol = greedy_repair(state, random_state)
+print(f"Initial solution objective is {init_sol.objective()}.")
+
+# Create subplots
+fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(16, 6))
+
+# Plot both graphs
+draw_graph(init_sol.to_graph(), ax=ax1, title="Initial Solution")
+draw_graph(DATA.get_graph(), only_nodes=True, ax=ax2, title="Original Graph with Nodes Only")
+
+plt.tight_layout()
+plt.show()
+
diff --git a/Trav_SalMan1.py b/Trav_SalMan1.py
new file mode 100644
index 0000000000..88c0f16344
--- /dev/null
+++ b/Trav_SalMan1.py
@@ -0,0 +1,160 @@
+import copy
+import matplotlib.pyplot as plt
+import networkx as nx
+import numpy as np
+import numpy.random as rnd
+import tsplib95
+import tsplib95.distances as distances
+
+from alns import ALNS
+from alns.accept import HillClimbing
+from alns.select import RouletteWheel
+from alns.stop import MaxRuntime
+
+# Set seed for reproducibility
+SEED = 7654
+
+# Load TSP data
+DATA = tsplib95.load('/home/dell/ALNS/examples/data/xqf131.tsp')
+CITIES = list(DATA.node_coords.keys())
+
+# Precompute distance matrix
+COORDS = DATA.node_coords.values()
+DIST = np.empty((len(COORDS) + 1, len(COORDS) + 1))
+for row, coord1 in enumerate(COORDS, 1):
+ for col, coord2 in enumerate(COORDS, 1):
+ DIST[row, col] = distances.euclidean(coord1, coord2)
+
+# Load optimal solution for comparison
+SOLUTION = tsplib95.load('/home/dell/ALNS/examples/data/xqf131.opt.tour')
+OPTIMAL = DATA.trace_tours(SOLUTION.tours)[0]
+print(f"Total optimal tour length is {OPTIMAL}.")
+
+# Helper function to draw TSP graphs
+def draw_graph(graph, only_nodes=False, ax=None):
+ if ax is None:
+ fig, ax = plt.subplots(figsize=(12, 6))
+
+ pos = DATA.node_coords
+ if only_nodes:
+ nodes_to_draw = [node for node in graph.nodes if node in pos]
+ nx.draw_networkx_nodes(graph, pos, nodelist=nodes_to_draw, node_size=25, ax=ax)
+ else:
+ nx.draw_networkx(graph, pos, with_labels=False, ax=ax)
+
+
+# Solution class for TSP problem
+class TspState:
+ def __init__(self, nodes, edges):
+ self.nodes = nodes
+ self.edges = edges
+
+ def objective(self):
+ return sum(DIST[node, self.edges[node]] for node in self.nodes)
+
+ def to_graph(self):
+ graph = nx.Graph()
+ for node in self.nodes:
+ graph.add_node(node, pos=DATA.node_coords[node])
+ for node_from, node_to in self.edges.items():
+ graph.add_edge(node_from, node_to)
+ return graph
+
+# Operators for ALNS
+DEGREE_OF_DESTRUCTION = 0.1
+
+def edges_to_remove(state):
+ return int(len(state.edges) * DEGREE_OF_DESTRUCTION)
+
+def worst_removal(current, rnd_state):
+ destroyed = copy.deepcopy(current)
+ worst_edges = sorted(destroyed.nodes, key=lambda node: DIST[node, destroyed.edges[node]])
+ for idx in range(edges_to_remove(current)):
+ del destroyed.edges[worst_edges[-(idx + 1)]]
+ return destroyed
+
+def path_removal(current, rnd_state):
+ destroyed = copy.deepcopy(current)
+ node_idx = rnd_state.choice(len(destroyed.nodes))
+ node = destroyed.nodes[node_idx]
+ for _ in range(edges_to_remove(current)):
+ node = destroyed.edges.pop(node)
+ return destroyed
+
+def random_removal(current, rnd_state):
+ destroyed = copy.deepcopy(current)
+ for idx in rnd_state.choice(len(destroyed.nodes), edges_to_remove(current), replace=False):
+ del destroyed.edges[destroyed.nodes[idx]]
+ return destroyed
+
+def would_form_subcycle(from_node, to_node, state):
+ for step in range(1, len(state.nodes)):
+ if to_node not in state.edges:
+ return False
+ to_node = state.edges[to_node]
+ if from_node == to_node and step != len(state.nodes) - 1:
+ return True
+ return False
+
+def greedy_repair(current, rnd_state):
+ visited = set(current.edges.values())
+ shuffled_idcs = rnd_state.permutation(len(current.nodes))
+ nodes = [current.nodes[idx] for idx in shuffled_idcs]
+ while len(current.edges) != len(current.nodes):
+ node = next(node for node in nodes if node not in current.edges)
+ unvisited = {other for other in current.nodes if other != node
+ if other not in visited
+ if not would_form_subcycle(node, other, current)}
+ nearest = min(unvisited, key=lambda other: DIST[node, other])
+ current.edges[node] = nearest
+ visited.add(nearest)
+ return current
+
+# Initialize random state
+random_state = rnd.RandomState(SEED)
+state = TspState(CITIES, {})
+
+# Initial solution using greedy repair
+init_sol = greedy_repair(state, random_state)
+print(f"Initial solution objective is {init_sol.objective()}.")
+
+# ALNS initialization and parameters
+alns = ALNS(random_state)
+alns.add_destroy_operator(random_removal)
+alns.add_destroy_operator(path_removal)
+alns.add_destroy_operator(worst_removal)
+alns.add_repair_operator(greedy_repair)
+
+# ALNS parameters and execution
+select = RouletteWheel([3, 2, 1, 0.5], 0.8, 3, 1)
+accept = HillClimbing()
+stop = MaxRuntime(60)
+
+# Run ALNS on the initial solution
+result = alns.iterate(init_sol, select, accept, stop)
+solution = result.best_state
+objective = solution.objective()
+pct_diff = 100 * (objective - OPTIMAL) / OPTIMAL
+
+print(f"Best heuristic objective is {objective}.")
+print(f"This is {pct_diff:.1f}% worse than the optimal solution, which is {OPTIMAL}.")
+
+# Plotting all graphs side by side
+fig, axs = plt.subplots(1, 3, figsize=(18, 6))
+
+# Plot the initial solution
+draw_graph(init_sol.to_graph(), ax=axs[0])
+axs[0].set_title('Initial Solution')
+
+# Plot the ALNS objectives
+result.plot_objectives(ax=axs[1], lw=2)
+axs[1].set_title('ALNS Objectives')
+
+# Plot the optimal solution
+draw_graph(SOLUTION.get_graph(), only_nodes=True, ax=axs[2])
+axs[2].set_title('Optimal Solution')
+
+# Adjust layout and display the plots
+plt.tight_layout()
+plt.show()
+
diff --git a/Trav_SalMan1_hospitals.py b/Trav_SalMan1_hospitals.py
new file mode 100644
index 0000000000..74ff0bc153
--- /dev/null
+++ b/Trav_SalMan1_hospitals.py
@@ -0,0 +1,231 @@
+import copy
+import matplotlib.pyplot as plt
+import networkx as nx
+import numpy as np
+import numpy.random as rnd
+import tsplib95
+import tsplib95.distances as distances
+from math import radians, sin, cos, sqrt, atan2
+
+from alns import ALNS
+from alns.accept import HillClimbing
+from alns.select import RouletteWheel
+from alns.stop import MaxRuntime
+
+# Set seed for reproducibility
+SEED = 7654
+
+# Load TSP data
+DATA = tsplib95.load('/home/dell/ALNS/Destinations.tsp')
+CITIES = list(DATA.node_coords.keys())
+
+# Load fences data
+def load_fences(filename):
+ with open(filename, 'r') as file:
+ lines = file.readlines()
+ fences = []
+ read_fences = False
+ for line in lines:
+ if line.strip() == "EDGE_COORD_SECTION":
+ read_fences = True
+ continue
+ if read_fences:
+ if line.strip() == "EOF":
+ break
+ parts = list(map(float, line.strip().split()))
+ if len(parts) == 4:
+ fences.append(parts)
+ return fences
+
+FENCES = load_fences('/home/dell/ALNS/Fences.tsp')
+
+def intersects_fence(coord1, coord2, fence):
+ # Function to check if a line segment from coord1 to coord2 intersects a fence
+ x1, y1 = coord1
+ x2, y2 = coord2
+ fx1, fy1, fx2, fy2 = fence
+
+ def ccw(A, B, C):
+ return (C[1] - A[1]) * (B[0] - A[0]) > (B[1] - A[1]) * (C[0] - A[0])
+
+ return ccw((x1, y1), (fx1, fy1), (fx2, fy2)) != ccw((x2, y2), (fx1, fy1), (fx2, fy2)) and \
+ ccw((x1, y1), (x2, y2), (fx1, fy1)) != ccw((x1, y1), (x2, y2), (fx2, fy2))
+
+def path_valid(coord1, coord2):
+ return not any(intersects_fence(coord1, coord2, fence) for fence in FENCES)
+
+def haversine_distance(coord1, coord2):
+ # Convert latitude and longitude from degrees to radians
+ lat1, lon1 = map(radians, coord1)
+ lat2, lon2 = map(radians, coord2)
+
+ # Haversine formula
+ dlat = lat2 - lat1
+ dlon = lon2 - lon1
+ a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2
+ c = 2 * atan2(sqrt(a), sqrt(1 - a))
+ radius_of_earth_km = 6371.0 # Radius of the Earth in kilometers
+ distance = radius_of_earth_km * c
+
+ return distance
+
+# Precompute distance matrix with fences
+COORDS = list(DATA.node_coords.values())
+DIST = np.empty((len(COORDS) + 1, len(COORDS) + 1))
+for row, coord1 in enumerate(COORDS, 1):
+ for col, coord2 in enumerate(COORDS, 1):
+ if row == col:
+ DIST[row, col] = 0
+ else:
+ dist = haversine_distance(coord1, coord2)
+ if not path_valid(coord1, coord2):
+ dist = np.inf # Use a very large number to indicate an impassable path
+ DIST[row, col] = dist
+
+# Helper function to draw TSP graphs
+def draw_graph(graph, only_nodes=False, ax=None):
+ if ax is None:
+ fig, ax = plt.subplots(figsize=(12, 6))
+
+ pos = DATA.node_coords
+ if only_nodes:
+ nodes_to_draw = [node for node in graph.nodes if node in pos]
+ nx.draw_networkx_nodes(graph, pos, nodelist=nodes_to_draw, node_size=25, ax=ax)
+ else:
+ nx.draw_networkx(graph, pos, with_labels=False, ax=ax)
+
+ # Draw fences
+ for fence in FENCES:
+ x_values = [fence[0], fence[2]]
+ y_values = [fence[1], fence[3]]
+ ax.plot(x_values, y_values, 'r-')
+
+# Solution class for TSP problem
+class TspState:
+ def __init__(self, nodes, edges):
+ self.nodes = nodes
+ self.edges = edges
+
+ def objective(self):
+ return sum(DIST[node, self.edges[node]] for node in self.nodes)
+
+ def to_graph(self):
+ graph = nx.Graph()
+ for node in self.nodes:
+ graph.add_node(node, pos=DATA.node_coords[node])
+ for node_from, node_to in self.edges.items():
+ graph.add_edge(node_from, node_to)
+ return graph
+
+# Operators for ALNS
+DEGREE_OF_DESTRUCTION = 0.1
+
+def edges_to_remove(state):
+ return int(len(state.edges) * DEGREE_OF_DESTRUCTION)
+
+def worst_removal(current, rnd_state):
+ destroyed = copy.deepcopy(current)
+ worst_edges = sorted(destroyed.nodes, key=lambda node: DIST[node, destroyed.edges[node]])
+ for idx in range(edges_to_remove(current)):
+ del destroyed.edges[worst_edges[-(idx + 1)]]
+ return destroyed
+
+def path_removal(current, rnd_state):
+ destroyed = copy.deepcopy(current)
+ node_idx = rnd_state.choice(len(destroyed.nodes))
+ node = destroyed.nodes[node_idx]
+ for _ in range(edges_to_remove(current)):
+ node = destroyed.edges.pop(node)
+ return destroyed
+
+def random_removal(current, rnd_state):
+ destroyed = copy.deepcopy(current)
+ for idx in rnd_state.choice(len(destroyed.nodes), edges_to_remove(current), replace=False):
+ del destroyed.edges[destroyed.nodes[idx]]
+ return destroyed
+
+def would_form_subcycle(from_node, to_node, state):
+ for step in range(1, len(state.nodes)):
+ if to_node not in state.edges:
+ return False
+ to_node = state.edges[to_node]
+ if from_node == to_node and step != len(state.nodes) - 1:
+ return True
+ return False
+
+def greedy_repair(current, rnd_state):
+ visited = set(current.edges.values())
+ shuffled_idcs = rnd_state.permutation(len(current.nodes))
+ nodes = [current.nodes[idx] for idx in shuffled_idcs]
+ while len(current.edges) != len(current.nodes):
+ node = next(node for node in nodes if node not in current.edges)
+ unvisited = {other for other in current.nodes if other != node
+ if other not in visited
+ if not would_form_subcycle(node, other, current)
+ if path_valid(DATA.node_coords[node], DATA.node_coords[other])}
+ if not unvisited:
+ # If no valid unvisited nodes remain, we need to backtrack
+ continue
+ nearest = min(unvisited, key=lambda other: DIST[node, other])
+ current.edges[node] = nearest
+ visited.add(nearest)
+ return current
+
+# Initialize random state
+random_state = rnd.RandomState(SEED)
+state = TspState(CITIES, {})
+
+# Initial solution using greedy repair
+init_sol = greedy_repair(state, random_state)
+initial_objective = init_sol.objective()
+print(f"Initial solution objective (distance): {initial_objective:.2f} km")
+
+# ALNS initialization and parameters
+alns = ALNS(random_state)
+alns.add_destroy_operator(random_removal)
+alns.add_destroy_operator(path_removal)
+alns.add_destroy_operator(worst_removal)
+alns.add_repair_operator(greedy_repair)
+
+# ALNS parameters and execution
+select = RouletteWheel([3, 2, 1, 0.5], 0.8, 3, 1)
+accept = HillClimbing()
+stop = MaxRuntime(600) # Increased runtime for better chances of finding improvements
+
+# Run ALNS on the initial solution
+result = alns.iterate(init_sol, select, accept, stop)
+solution = result.best_state
+solution_objective = solution.objective()
+
+print(f"Best solution objective (distance) avoiding fences: {solution_objective:.2f} km")
+
+# Check if the best solution is different from the initial solution
+if initial_objective == solution_objective:
+ print("ALNS did not find a better solution.")
+
+# Plotting the initial solution, ALNS objectives, and the best solution
+fig, axs = plt.subplots(1, 3, figsize=(24, 8))
+
+# Plot the initial solution
+draw_graph(init_sol.to_graph(), ax=axs[0])
+axs[0].set_title('Initial Solution')
+
+# Plot the ALNS objectives
+result.plot_objectives(ax=axs[1], lw=2)
+axs[1].set_title('ALNS Objectives')
+
+# Plot the best solution found by ALNS
+draw_graph(solution.to_graph(), ax=axs[2])
+axs[2].set_title('Best Solution Found by ALNS')
+
+# Draw fences on all plots for clarity
+for ax in axs:
+ for fence in FENCES:
+ x_values = [fence[0], fence[2]]
+ y_values = [fence[1], fence[3]]
+ ax.plot(x_values, y_values, 'r-')
+
+# Adjust layout and display the plots
+plt.tight_layout()
+plt.show()
+
diff --git a/convert_KML_to_TSP.py b/convert_KML_to_TSP.py
new file mode 100644
index 0000000000..a3676eaefc
--- /dev/null
+++ b/convert_KML_to_TSP.py
@@ -0,0 +1,38 @@
+import xml.etree.ElementTree as ET
+
+def parse_kml(kml_file):
+ tree = ET.parse(kml_file)
+ root = tree.getroot()
+ namespace = {'kml': 'http://www.opengis.net/kml/2.2'}
+ coordinates = []
+
+ for placemark in root.findall('.//kml:Placemark', namespace):
+ coords = placemark.find('.//kml:coordinates', namespace).text.strip()
+ for coord in coords.split():
+ lon, lat, _ = map(float, coord.split(','))
+ coordinates.append((lat, lon))
+
+ return coordinates
+
+def convert_to_tsp(coordinates, tsp_file):
+ with open(tsp_file, 'w') as f:
+ f.write(f"NAME: example\n")
+ f.write(f"TYPE: TSP\n")
+ f.write(f"DIMENSION: {len(coordinates)}\n")
+ f.write(f"EDGE_WEIGHT_TYPE: EUC_2D\n")
+ f.write(f"NODE_COORD_SECTION\n")
+
+ for i, (lat, lon) in enumerate(coordinates, start=1):
+ f.write(f"{i} {lat} {lon}\n")
+
+ f.write(f"EOF\n")
+
+# Main execution
+kml_file = '/home/dell/ALNS/Destinations.kml'
+tsp_file = '/home/dell/ALNS/Destinations.tsp'
+
+coordinates = parse_kml(kml_file)
+convert_to_tsp(coordinates, tsp_file)
+
+print(f"Converted {len(coordinates)} points from {kml_file} to {tsp_file}")
+
diff --git a/mavproxy_kml.py b/mavproxy_kml.py
new file mode 100644
index 0000000000..583cbb2114
--- /dev/null
+++ b/mavproxy_kml.py
@@ -0,0 +1,30 @@
+#!/usr/bin/env python3
+import pexpect
+import time
+
+# Define the MAVProxy command
+mavproxy_cmd = "mavproxy.py --master=udpin:127.0.0.1:14550 --console --map --out=udpout:127.0.0.1:14552"
+
+# Start MAVProxy
+child = pexpect.spawn(mavproxy_cmd)
+
+# Wait for MAVProxy to load
+child.expect("Received", timeout=60)
+
+# Load the kmlread module
+child.sendline("module load kmlread")
+
+# Wait for the module to load
+time.sleep(2)
+
+# Load the KML file
+child.sendline("kml load /home/dell/Essex_Hospitals_NoFlyZones.kml")
+
+# Give some time for the KML to load and process
+time.sleep(5)
+
+# Optionally, you can add more commands here, e.g., to toggle visibility or create fences
+
+# Interact with MAVProxy console
+child.interact()
+
diff --git a/settings.json b/settings.json
new file mode 100644
index 0000000000..fd1383b806
--- /dev/null
+++ b/settings.json
@@ -0,0 +1,31 @@
+{
+ "SettingsVersion": 1.2,
+ "SimMode": "Multirotor",
+ "OriginGeopoint": {
+ "Latitude": 51.77458,
+ "Longitude": 0.46677,
+ "Altitude": 56
+ },
+ "Vehicles": {
+ "Copter1": {
+ "VehicleType": "ArduCopter",
+ "UseSerial": false,
+ "LocalHostIp": "127.0.0.1",
+ "UdpIp": "127.0.0.1",
+ "UdpPort": 9003,
+ "ControlPort": 9002
+ },
+ "Copter2": {
+ "VehicleType": "ArduCopter",
+ "UseSerial": false,
+ "LocalHostIp": "127.0.0.1",
+ "UdpIp": "127.0.0.1",
+ "UdpPort": 9013,
+ "ControlPort": 9012,
+ "X": 0,
+ "Y": 3,
+ "Z": 0
+ }
+ }
+}
+