diff --git a/.gitignore b/.gitignore index 95bd13f..a7ced60 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ .idea +.vscode cmake-build-* build **path_*.csv diff --git a/CMakeLists.txt b/CMakeLists.txt index 01b1da6..3d43f19 100644 --- a/CMakeLists.txt +++ b/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) \ No newline at end of file diff --git a/include/mstsp_solver/SolverConfig.h b/include/mstsp_solver/SolverConfig.h index 95f1425..05cf0e2 100644 --- a/include/mstsp_solver/SolverConfig.h +++ b/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 diff --git a/include/mstsp_solver/TargetSet.h b/include/mstsp_solver/TargetSet.h index bc47e33..7fc0109 100644 --- a/include/mstsp_solver/TargetSet.h +++ b/include/mstsp_solver/TargetSet.h @@ -9,6 +9,7 @@ #include #include "Target.h" #include +#include #include "MapPolygon.hpp" #include "EnergyCalculator.h" diff --git a/sample_configs/cape/config.yaml b/sample_configs/cape/config.yaml index e9e2a79..54b4693 100644 --- a/sample_configs/cape/config.yaml +++ b/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 diff --git a/sample_configs/complex/config.yaml b/sample_configs/complex/config.yaml index 5716799..a1d6bce 100644 --- a/sample_configs/complex/config.yaml +++ b/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 diff --git a/sample_configs/island/config.yaml b/sample_configs/island/config.yaml index 9283522..155517e 100644 --- a/sample_configs/island/config.yaml +++ b/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 diff --git a/sample_configs/rectangle/config.yaml b/sample_configs/rectangle/config.yaml index fb0801b..7f435bb 100644 --- a/sample_configs/rectangle/config.yaml +++ b/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 diff --git a/sample_configs/simple/config.yaml b/sample_configs/simple/config.yaml index fb58c0f..eb96a1f 100644 --- a/sample_configs/simple/config.yaml +++ b/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 diff --git a/scripts/visualize_paths.py b/scripts/visualize_paths.py index 35e3da6..a20476d 100644 --- a/scripts/visualize_paths.py +++ b/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() diff --git a/src/MapPolygon.cpp b/src/MapPolygon.cpp index ec18867..a9f5830 100644 --- a/src/MapPolygon.cpp +++ b/src/MapPolygon.cpp @@ -5,6 +5,7 @@ #include #include #include "algorithms.hpp" +#include // TODO: make most of methods external functions (maybe, working on not MapPolygons but just on vector> // TODO: make use of some external polygons library like @@ -242,7 +243,7 @@ std::vector MapPolygon::split_into_pieces(double max_piece_area) { try { - int res_polygons = std::ceil(area() / max_piece_area); + auto res_polygons = static_cast(std::ceil(area() / max_piece_area)); double split_piece_area = area() / res_polygons; auto cur_polygon = *this; std::vector res; @@ -269,7 +270,7 @@ std::vector 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) { diff --git a/src/ShortestPathCalculator.cpp b/src/ShortestPathCalculator.cpp index 199eb54..99f9d8e 100644 --- a/src/ShortestPathCalculator.cpp +++ b/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>(m_polygon_points.size()); std::for_each(m_next_vertex_in_path.begin(), m_next_vertex_in_path.end(), - [&](auto &row) { row = std::vector(m_polygon_points.size(), -1); }); + [&](auto &row) { row = std::vector(m_polygon_points.size(), ~static_cast(0)); }); m_floyd_warshall_d = std::vector>(m_polygon_points.size()); std::for_each(m_floyd_warshall_d.begin(), m_floyd_warshall_d.end(), [&](auto &row) { row = std::vector(m_polygon_points.size(), HUGE_VAL); }); diff --git a/src/algorithms.cpp b/src/algorithms.cpp index 19c7e9a..d5f0d65 100644 --- a/src/algorithms.cpp +++ b/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; } diff --git a/src/main.cpp b/src/main.cpp index 076f070..55041a1 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -24,6 +24,7 @@ struct algorithm_config_t { decomposition_type_t decomposition_type; int min_sub_polygons_per_uav; std::pair start_pos; + std::pair 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(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 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(); algorithm_config.start_pos = {config["start_x"].as(), config["start_y"].as()}; + if (config["end_x"] && config["end_y"]) { + algorithm_config.end_pos = {config["end_x"].as(), config["end_y"].as()}; + } 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(); algorithm_config.no_improvement_cycles_before_stop = config["no_improvement_cycles_before_stop"].as(); algorithm_config.max_single_path_energy = config["max_single_path_energy"].as(); @@ -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(n_uavs), 0, 0, algorithm_config.no_improvement_cycles_before_stop}; diff --git a/src/mstsp_solver/MstspSolver.cpp b/src/mstsp_solver/MstspSolver.cpp index 3b087ec..29f762c 100644 --- a/src/mstsp_solver/MstspSolver.cpp +++ b/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; }