Allow setting UAV ending position

.gitignore

@@ -1,4 +1,5 @@
 .idea
+.vscode
 cmake-build-*
 build
 **path_*.csv

CMakeLists.txt

@@ -5,20 +5,33 @@ project(coverage_mission_planner)
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
-if(MSVC)
-  add_compile_options(/W4 /WX)
-else()
-  add_compile_options(-Wall -Wextra -pedantic -Werror)
-endif()
-
 if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE Release)
 endif()
 
 add_executable(${PROJECT_NAME} src/main.cpp src/MapPolygon.cpp include/MapPolygon.hpp src/utils.cpp src/algorithms.cpp src/EnergyCalculator.cpp src/ShortestPathCalculator.cpp include/ShortestPathCalculator.hpp include/custom_types.hpp include/mstsp_solver/Target.h src/mstsp_solver/TargetSet.cpp include/mstsp_solver/TargetSet.h include/mstsp_solver/SolverConfig.h src/mstsp_solver/MstspSolver.cpp include/mstsp_solver/MstspSolver.h include/mstsp_solver/Insertion.h include/SimpleLogger.h include/LoggerRos.h)
 
-find_package(yaml-cpp)
+if(MSVC)
+  target_compile_options(${PROJECT_NAME} PRIVATE /W4 /WX /D_USE_MATH_DEFINES)
+else()
+  target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra -pedantic -Werror)
+endif()
+
+include(FetchContent)
+
+FetchContent_Declare(
+  yaml-cpp
+  GIT_REPOSITORY https://github.com/jbeder/yaml-cpp.git
+  GIT_TAG 0.8.0
+)
+FetchContent_GetProperties(yaml-cpp)
+
+if(NOT yaml-cpp_POPULATED)
+  message(STATUS "Fetching yaml-cpp...")
+  FetchContent_Populate(yaml-cpp)
+  add_subdirectory(${yaml-cpp_SOURCE_DIR} ${yaml-cpp_BINARY_DIR})
+endif()
 
 
-target_include_directories(${PROJECT_NAME} PUBLIC include ${YAML_CPP_INCLUDE_DIRS})
-target_link_libraries(${PROJECT_NAME} ${YAML_CPP_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} PRIVATE yaml-cpp::yaml-cpp)
+target_include_directories(${PROJECT_NAME} PRIVATE include)

include/mstsp_solver/SolverConfig.h

@@ -13,6 +13,7 @@ namespace mstsp_solver {
         int rotations_per_cell;
         double sweeping_step;
         point_t starting_point;
+        point_t ending_point;
         size_t n_uavs;
         double sweeping_alt;
         double unique_alt_step = 1.0; // 1m difference while not sweeping by default

include/mstsp_solver/TargetSet.h

@@ -9,6 +9,7 @@
 #include <vector>
 #include "Target.h"
 #include <cmath>
+#include <math.h>
 #include "MapPolygon.hpp"
 #include "EnergyCalculator.h"
 

sample_configs/cape/config.yaml

@@ -56,7 +56,8 @@ decomposition_method: 1
 # If a smaller number is generated by the decomposition method, large sub polygons are divided into smaller ones
 min_sub_polygons_per_uav: 4
 
-# Start and end position of each UAV. If points_in_lat_lon is defined, x and y are latitude and longitude
+# Start position of each UAV. If points_in_lat_lon is defined, x and y are latitude and longitude
+# If end_x & end_y are not defined, this will also be the ending position
 start_x: -77.46165699991123
 start_y: 169.1839688376855
 

sample_configs/complex/config.yaml

@@ -57,7 +57,8 @@ decomposition_method: 1
 # If a smaller number is generated by the decomposition method, large sub polygons are divided into smaller ones
 min_sub_polygons_per_uav: 3
 
-# Start and end position of each UAV. If points_in_lat_lon is defined, x and y are latitude and longitude
+# Start position of each UAV. If points_in_lat_lon is defined, x and y are latitude and longitude
+# If end_x & end_y are not defined, this will also be the ending position
 start_x: 49.365642730404964
 start_y: 14.2505300863795
 

sample_configs/island/config.yaml

@@ -57,7 +57,8 @@ decomposition_method: 1
 # If a smaller number is generated by the decomposition method, large sub polygons are divided into smaller ones
 min_sub_polygons_per_uav: 1
 
-# Start and end position of each UAV. If points_in_lat_lon is defined, x and y are latitude and longitude
+# Start position of each UAV. If points_in_lat_lon is defined, x and y are latitude and longitude
+# If end_x & end_y are not defined, this will also be the ending position
 start_x: 50.111826583361925
 start_y: 14.41890374448402
 

sample_configs/rectangle/config.yaml

@@ -56,7 +56,8 @@ decomposition_method: 1
 # If a smaller number is generated by the decomposition method, large sub polygons are divided into smaller ones
 min_sub_polygons_per_uav: 1
 
-# Start and end position of each UAV. If points_in_lat_lon is defined, x and y are latitude and longitude
+# Start position of each UAV. If points_in_lat_lon is defined, x and y are latitude and longitude
+# If end_x & end_y are not defined, this will also be the ending position
 start_x: 49.36314644627111
 start_y: 14.260587804055804
 

sample_configs/simple/config.yaml

@@ -56,10 +56,15 @@ decomposition_method: 1
 # If a smaller number is generated by the decomposition method, large sub polygons are divided into smaller ones
 min_sub_polygons_per_uav: 1
 
-# Start and end position of each UAV. If points_in_lat_lon is defined, x and y are latitude and longitude
+# Start position of each UAV. If points_in_lat_lon is defined, x and y are latitude and longitude.
+# If end_x & end_y are not defined, this will also be the ending position
 start_x: 49.362348212496755
 start_y: 14.261290785325457
 
+# End position of each UAV. If points_in_lat_lon is defined, x and y are latitude and longitude
+end_x: 49.36217269052025
+end_y: 14.258554158283146
+
 # Parameters affecting algorithm performance and completion time
 rotations_per_cell: 5
 no_improvement_cycles_before_stop: 400

scripts/visualize_paths.py

@@ -75,12 +75,19 @@ def main():
     for i in range(len(paths_points)):
         # Plot each path using a distinct color
         plt.plot(paths_points[i][:, 0], paths_points[i][:, 1],
-                 color=hsv_to_rgb([0.2 + 0.8 * (i / len(paths_points)), 0.8, 0.8]))
+                 color=hsv_to_rgb([0.2 + 0.8 * (i / len(paths_points)), 0.8, 0.8]), label = sys.argv[2 + i])
+
+    if len(paths_points) > 0:
+        plt.plot(paths_points[0][0, 0], paths_points[0][0, 1], 'o',
+                 color=[0., 0., 0.], label = 'Start point')
+        plt.plot(paths_points[0][-1, 0], paths_points[0][-1, 1], 'x',
+                 color=[1., 0., 0.], label = 'End point')
 
     plt.xlabel("x")
     plt.ylabel("y")
     plt.title("Visualization of AOI and generated paths")
     plt.axis('equal')
+    plt.legend()
 
     plt.show()
 

src/MapPolygon.cpp

@@ -5,6 +5,7 @@
 #include <stdexcept>
 #include <cmath>
 #include "algorithms.hpp"
+#include <math.h>
 
 // TODO: make most of methods external functions (maybe, working on not MapPolygons but just on vector<pair<double, double>>
 // TODO: make use of some external polygons library like
@@ -242,7 +243,7 @@ std::vector<MapPolygon> MapPolygon::split_into_pieces(double max_piece_area) {
 
     try {
 
-        int res_polygons = std::ceil(area() / max_piece_area);
+        auto res_polygons = static_cast<int>(std::ceil(area() / max_piece_area));
         double split_piece_area = area() / res_polygons;
         auto cur_polygon = *this;
         std::vector<MapPolygon> res;
@@ -269,7 +270,7 @@ std::vector<MapPolygon> MapPolygon::split_into_pieces(double max_piece_area) {
         }
         res.push_back(cur_polygon);
         return res;
-    } catch (std::runtime_error &e) {
+    } catch (std::runtime_error &) {
 //        m_logger->log_err("[PathGenerator]: ERROR while dividing polygon: " + std::string(e.what()));
     }
     return {};
@@ -328,7 +329,7 @@ namespace {
         }
         double l = 0, r = largest_area;
         auto current_num_of_pols = sub_polygons.size();
-        while (r - l > 1 and current_num_of_pols != n) {
+        while (r - l > 1 && current_num_of_pols != n) {
             double m = (r + l) / 2;
             size_t number_of_decomposed = 0;
             for (auto &pol: sub_polygons) {

src/ShortestPathCalculator.cpp

@@ -89,7 +89,7 @@ ShortestPathCalculator::ShortestPathCalculator(const MapPolygon &polygon) {
     // And 2-d matrix for the Floyd-Warshall algorithm
     m_next_vertex_in_path = std::vector<std::vector<size_t>>(m_polygon_points.size());
     std::for_each(m_next_vertex_in_path.begin(), m_next_vertex_in_path.end(),
-                  [&](auto &row) { row = std::vector<size_t>(m_polygon_points.size(), -1); });
+                  [&](auto &row) { row = std::vector<size_t>(m_polygon_points.size(), ~static_cast<size_t>(0)); });
     m_floyd_warshall_d = std::vector<std::vector<double>>(m_polygon_points.size());
     std::for_each(m_floyd_warshall_d.begin(), m_floyd_warshall_d.end(),
                   [&](auto &row) { row = std::vector<double>(m_polygon_points.size(), HUGE_VAL); });

src/algorithms.cpp

@@ -155,7 +155,7 @@ vpdd thin_polygon_coverage(const MapPolygon &polygon, double sweeping_step, doub
 
 vpdd sweeping(const MapPolygon &polygon, double angle, double sweeping_step, double wall_distance, bool start_up) {
     auto thin_sweeping = thin_polygon_coverage(polygon, sweeping_step, wall_distance);
-    if (not thin_sweeping.empty()) {
+    if (!thin_sweeping.empty()) {
         return thin_sweeping;
     }
 

src/main.cpp

@@ -24,6 +24,7 @@ struct algorithm_config_t {
     decomposition_type_t decomposition_type;
     int min_sub_polygons_per_uav;
     std::pair<double, double> start_pos;
+    std::pair<double, double> end_pos;
 
     int rotations_per_cell;
     int no_improvement_cycles_before_stop;
@@ -139,7 +140,7 @@ int main(int argc, char *argv[]) {
         };
         best_solution = generate_with_constraints(algorithm_config.max_single_path_energy * 3600,
                                                   algorithm_config.number_of_drones, f);
-    } catch (const polygon_decomposition_error &e) {
+    } catch (const polygon_decomposition_error &) {
         std::cout << "Error while decomposing the polygon" << std::endl;
         return -1;
     } catch (const std::runtime_error &e) {
@@ -187,7 +188,7 @@ generate_with_constraints(double max_energy_bound, unsigned int n_uavs, F f) {
             return solution;
         }
         // if the energy consumption is divided well, this should be enough
-        unsigned int updated_n_uavs = std::ceil(solution.path_energies_sum / max_energy_bound);
+        auto updated_n_uavs = static_cast<unsigned int>(std::ceil(solution.path_energies_sum / max_energy_bound));
 
         // if the needed number of UAVs is estimated to be smaller than on previous step, make it larger
         if (updated_n_uavs <= current_n_uavs) {
@@ -227,7 +228,7 @@ std::vector<point_t> read_points_from_csv(const std::string &filename) {
 
     is.close();
     // Make sure that the first point is always the same as the last one
-    if (not points.empty() and points[0] != points[points.size() - 1]) {
+    if (!points.empty() && points[0] != points[points.size() - 1]) {
         points.push_back(points[0]);
     }
     return points;
@@ -341,6 +342,13 @@ algorithm_config_t parse_algorithm_config(const YAML::Node &config) {
     algorithm_config.min_sub_polygons_per_uav = config["min_sub_polygons_per_uav"].as<int>();
 
     algorithm_config.start_pos = {config["start_x"].as<double>(), config["start_y"].as<double>()};
+    if (config["end_x"] && config["end_y"]) {
+        algorithm_config.end_pos = {config["end_x"].as<double>(), config["end_y"].as<double>()};
+    } else {
+        // If no end point is given, return to the starting position
+        algorithm_config.end_pos = algorithm_config.start_pos;
+    }
+
     algorithm_config.rotations_per_cell = config["rotations_per_cell"].as<int>();
     algorithm_config.no_improvement_cycles_before_stop = config["no_improvement_cycles_before_stop"].as<int>();
     algorithm_config.max_single_path_energy = config["max_single_path_energy"].as<double>();
@@ -402,8 +410,13 @@ solve_for_uavs(int n_uavs, const algorithm_config_t &algorithm_config,
                                                                                              algorithm_config.lat_lon_origin)
                                                                  :
                               algorithm_config.start_pos;
+        auto ending_point = algorithm_config.points_in_lat_lon ? gps_coordinates_to_meters(algorithm_config.end_pos,
+                                                                                             algorithm_config.lat_lon_origin)
+                                                                 :
+                              algorithm_config.end_pos;
         mstsp_solver::SolverConfig solver_config{algorithm_config.rotations_per_cell, algorithm_config.sweeping_step,
                                                  starting_point,
+                                                 ending_point,
                                                  static_cast<size_t>(n_uavs), 0,
                                                  0,
                                                  algorithm_config.no_improvement_cycles_before_stop};

src/mstsp_solver/MstspSolver.cpp

@@ -75,7 +75,7 @@ namespace mstsp_solver {
         energy += m_energy_calculator.calculate_straight_line_energy(0, a, 0, -a, m_config.starting_point,
                                                                      path[0].starting_point);
         energy += m_energy_calculator.calculate_straight_line_energy(0, a, 0, -a, path[path.size() - 1].end_point,
-                                                                     m_config.starting_point);
+                                                                     m_config.ending_point);
 
         return energy;
     }
@@ -199,13 +199,13 @@ namespace mstsp_solver {
             res.insert(res.end(), path.begin(), path.end());
             last_heading = target.rotation_angle;
         }
-        auto path_to_start = add_path_heading(
+        auto path_to_end = add_path_heading(
                 m_shortest_path_calculator.shortest_path_between_points({res.back().x, res.back().y},
-                                                                        m_config.starting_point), last_heading,
+                                                                        m_config.ending_point), last_heading,
                 unique_alt);
-        path_to_start.erase(path_to_start.begin());
+        path_to_end.erase(path_to_end.begin());
 
-        res.insert(res.end(), path_to_start.begin(), path_to_start.end());
+        res.insert(res.end(), path_to_end.begin(), path_to_end.end());
         return res;
     }