def generateHTMLFile():
html_path = simulation_config["Relative Path"] + simulation_config[
"HTML File"]
with open(html_path, 'w') as fh:
fh.write("<!DOCTYPE html>\n")
fh.write("<html>\n")
fh.write("<body>\n\n")
fh.write(
"<canvas id=\"myCanvas\" width=1000 height=500 style=\"border:1px solid #d3d3d3;\">\n"
)
fh.write(
"Your browser does not support the HTML5 canvas tag.</canvas>\n\n")
fh.write(
"<button onclick=\"gridOn()\" id=\"gridButton\">Grid On</button>\n\n"
)
fh.write("<script src=\"" + simulation_config["Config File"] +
"\"></script>\n")
fh.write("<script src=\"" + simulation_config["Start Point File"] +
"\"></script>\n")
fh.write("<script src=\"" + simulation_config["Sensor File"] +
"\"></script>\n")
fh.write("<script src=\"" + simulation_config["Path File"] +
"\"></script>\n")
fh.write("<script src=\"" + simulation_config["Obstacle Path File"] +
"\"></script>\n")
fh.write("<script src=\"" + simulation_config["Intersection File"] +
"\"></script>\n")
fh.write("<script src=\"" + simulation_config["Grid File"] +
"\"></script>\n")
fh.write("<script src=\"" + simulation_config["JS Code"] +
"\"></script>\n\n")
fh.write("</body>\n")
fh.write("</html>\n")
fh.close()
logger.info('Generated HTML File at ' + html_path)
def generateStartPointFile(start_point):
start_point_path = simulation_config["Relative Path"] + simulation_config[
"Start Point File"]
with open(start_point_path, 'w') as fh:
fh.write("var start_point = " + "[" + str(start_point[0]) + "," +
str(start_point[1]) + ",0" + "]")
fh.close()
logger.info('Generated Start Point File at ' + start_point_path)
def generateSensorPointsFile(sensor_positions):
sensors_point_path = simulation_config[
"Relative Path"] + simulation_config["Sensor File"]
with open(sensors_point_path, 'w') as fh:
fh.write("var sensor_points = " + "[")
cnt = len(sensor_positions)
last_space = ", "
for point in sensor_positions:
if cnt == 1:
last_space = ""
fh.write("[" + str(point[0]) + "," + str(point[1]) + "," +
str(point[2]) + "]" + last_space)
cnt -= 1
fh.write("]")
fh.close()
logger.info('Generated Sensor Point File at ' + sensors_point_path)
def generatePathFile(path):
path_point_path = simulation_config["Relative Path"] + simulation_config[
"Path File"]
with open(path_point_path, 'w') as fh:
first_line = "var path_points = " + "["
second_line = "var path_actions = " + "["
cnt = len(path)
last_space = ", "
for action, point in path:
if cnt == 1:
last_space = ""
first_line += "[" + str(point[0]) + "," + str(
point[1]) + "]" + last_space
second_line += "[\"" + action + "\"]" + last_space
cnt -= 1
fh.write(first_line + "]\n")
fh.write(second_line + "]\n")
fh.close()
logger.info('Generated Path File at ' + path_point_path)
def pathPlanningCall(start_point, sensor_coords_with_radius, sensor_coords,
obstacle_bboxes):
# If we work in meters, our points are all relative to 0,0 so start_point should be updated
if global_config["Point Format"] == PointFormats.Meters:
start_point = [0, 0]
dist = 0
# Calculate path with traveling salesman
# Regular traveling salesman
# path = list(pathPlanning.travelingSalesman(start_point, sensor_coords))
# Adapted traveling salesman
# path = list(pathPlanning.advancedTravelingSalesman(start_point, sensor_coords_with_radius))
# Obstacle traveling salesman
# path, dist = pathPlanning.obstacleTravelingSalesman(start_point, sensor_coords_with_radius, obstacle_bboxes)
# Obstacle traveling salesman with dynamic algorithm
# path, dist = pathPlanning.obstacleTravelingSalesman(start_point, sensor_coords_with_radius, obstacle_bboxes, TravelingSalesmanTypes.HeuristicDynamic)
# path, dist = pathPlanning.obstacleTravelingSalesman(start_point, sensor_coords_with_radius, obstacle_bboxes, TravelingSalesmanTypes.BruteForceDynamic)
# Dynamic optimized traveling salesman : take straight line if no obstacles. Otherwise calculate route using grid and dijkstra
# path, dist = pathPlanning.obstacleTravelingSalesman(start_point, sensor_coords_with_radius, obstacle_bboxes, TravelingSalesmanTypes.HeuristicOptimizedDynamic)
path, dist = pathPlanning.obstacleTravelingSalesman(
start_point, sensor_coords_with_radius, obstacle_bboxes,
TravelingSalesmanTypes.BruteForceOptimizedDynamic)
logger.info("Total dist is: " + str(dist))
return path, dist
def createWaypointFile(start_point, point_list):
column_names = waypoint_config["Columns"]
columns = column_names.split(' ')
first_row = waypoint_config["First Row"].split(' ')
first_row.append(waypoint_config["Version"])
if global_config["Point Format"] == PointFormats.Meters:
# If we are in meter format we need to transform all points for the mission planner
# Compared to start point which we receive already in long/lat
# We reverse the point because we get it as x,y and want result as y,x because of long/lat
point_list = [[point[0],
metersToLongLat(start_point, point[1][::-1])]
for point in point_list]
file = waypoint_config["Relative Path"] + waypoint_config["File Name"]
with open(file, 'w') as fh:
writer = csv.writer(fh, delimiter='\t')
writer.writerow([' '.join(first_row)])
fh.close()
def createWaypointRowMap(data_vec):
data_map = {
'<CURRENT WP>': [data_vec[0]],
'<COORD FRAME>': [data_vec[1]],
'<COMMAND>': [data_vec[2]],
'<PARAM1>': [data_vec[3]],
'<PARAM2>': [data_vec[4]],
'<PARAM3>': [data_vec[5]],
'<PARAM4>': [data_vec[6]],
'<PARAM5/X/LONGITUDE>': [data_vec[7]],
'<PARAM6/Y/LATITUDE>': [data_vec[8]],
'<PARAM7/Z/ALTITUDE>': [data_vec[9]],
'<AUTOCONTINUE>': [data_vec[10]],
}
return data_map
def appendToWaypointRowMap(map, data_vec):
map['<CURRENT WP>'].append(data_vec[0])
map['<COORD FRAME>'].append(data_vec[1])
map['<COMMAND>'].append(data_vec[2])
map['<PARAM1>'].append(data_vec[3])
map['<PARAM2>'].append(data_vec[4])
map['<PARAM3>'].append(data_vec[5])
map['<PARAM4>'].append(data_vec[6])
map['<PARAM5/X/LONGITUDE>'].append(data_vec[7])
map['<PARAM6/Y/LATITUDE>'].append(data_vec[8])
map['<PARAM7/Z/ALTITUDE>'].append(data_vec[9])
map['<AUTOCONTINUE>'].append(data_vec[10])
return map
# Add first point - point of HOME
first_point = [
1, 0, waypoint_config["Commands"]["WAYPOINT"], 0, 0, 0, 0,
start_point[0], start_point[1], waypoint_config["Default Alt"], 1
]
data = createWaypointRowMap(first_point)
# Build other points according to path plan
if waypoint_config["Default Path Plan"] == path_plans_config[
"Follow Points"]:
for action, point in point_list:
action_cmd = waypoint_config["Commands"]["WAYPOINT"]
first_arg = 0
if action == "stop":
action_cmd = waypoint_config["Commands"]["LOITER_TIME"]
first_arg = waypoint_config["Default Loiter Time"]
point_data = [
0, waypoint_config["Default Coord Frame"], action_cmd,
first_arg, 0, 0, 0, point[0], point[1],
waypoint_config["Default Alt"], 1
]
data = appendToWaypointRowMap(data, point_data)
# Add last point - return to launch
last_point = [
0, waypoint_config["Default Coord Frame"],
waypoint_config["Commands"]["RETURN_TO_LAUNCH"], 0, 0, 0, 0, 0, 0,
0, 1
]
data = appendToWaypointRowMap(data, last_point)
else:
logger.error("No plan by name " +
waypoint_config["Default Path Plan"] + " exists")
exit(1)
df = pd.DataFrame(data)
# Write all to csv
df.to_csv(file, sep='\t', header=False, mode='a')
logger.info('Generated Waypoint File at ' + file)