diff --git a/polygon_coverage_benchmark/CMakeLists.txt b/polygon_coverage_benchmark/CMakeLists.txt index f7c940c..9992505 100644 --- a/polygon_coverage_benchmark/CMakeLists.txt +++ b/polygon_coverage_benchmark/CMakeLists.txt @@ -1,7 +1,31 @@ cmake_minimum_required(VERSION 3.16.3) project(polygon_coverage_benchmark) -find_package(catkin REQUIRED) +find_package(catkin REQUIRED COMPONENTS + rosconsole + roslib + polygon_coverage_msgs + polygon_coverage_geometry + polygon_coverage_planners + polygon_coverage_solvers + ) + + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") +set(CMAKE_BUILD_TYPE Release) + +find_package(CGAL QUIET COMPONENTS Core) +include(${CGAL_USE_FILE}) + +find_package(yaml-cpp REQUIRED) + catkin_package() +include_directories(include ${CGAL_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${YAML_INCLUDE_DIRS}) + +add_executable(test_benchmark test/benchmark-test.cpp) +target_link_libraries(test_benchmark ${CGAL_LIBRARIES} ${YAML_CPP_LIBRARIES} ${catkin_LIBRARIES}) -catkin_python_setup() \ No newline at end of file +catkin_python_setup() diff --git a/polygon_coverage_benchmark/package.xml b/polygon_coverage_benchmark/package.xml index 68c7ece..befc367 100644 --- a/polygon_coverage_benchmark/package.xml +++ b/polygon_coverage_benchmark/package.xml @@ -11,4 +11,9 @@ catkin rospy + roslib + polygon_coverage_msgs + polygon_coverage_geometry + polygon_coverage_planners + polygon_coverage_ros diff --git a/polygon_coverage_benchmark/python/polygon_coverage_benchmark/plot_results.py b/polygon_coverage_benchmark/python/polygon_coverage_benchmark/plot_results.py index 2cee057..67884be 100644 --- a/polygon_coverage_benchmark/python/polygon_coverage_benchmark/plot_results.py +++ b/polygon_coverage_benchmark/python/polygon_coverage_benchmark/plot_results.py @@ -32,6 +32,7 @@ mmToInch = 0.0393701 width = 117 * mmToInch aspect = 3 +create_fits = False def linFit(df, planner, uniform_weight=False): is_planner = df['planner'] == planner @@ -135,14 +136,17 @@ def plotTimes(df): # Scatter plot g = sns.lmplot('num_hole_vertices', 't_total', data=df_t_total, hue='planner', fit_reg=False, legend=False, legend_out=False, size=width, aspect=aspect) - # Exponential fit. - df_fit = createFits(df_t_total) min = df_t_total['t_total'].min() max = df_t_total['t_total'].max() - is_smaller = df_fit['y'] < max - df_fit = df_fit[is_smaller] - sns.lineplot('x', 'y', data=df_fit, hue='planner', ax=g.axes[0, 0], legend=False) + # Exponential fit. + if(create_fits): + df_fit = createFits(df_t_total) + is_smaller = df_fit['y'] < max + df_fit = df_fit[is_smaller] + + sns.lineplot('x', 'y', data=df_fit, hue='planner', ax=g.axes[0, 0], legend=False) + g.set(xlim=(df_t_total['num_hole_vertices'].min() - 10, df_t_total['num_hole_vertices'].max() + 10)) g.set(ylim=(df_t_total['t_total'].min() - 10, df_t_total['t_total'].max() + 10)) g.set(xlabel="Number of Hole Vertices [1]") @@ -170,14 +174,18 @@ def plotCosts(df): g.ax.legend(loc="upper left") g.savefig("./data/cost.pdf", bbox_inches='tight') - # Linear fit. - df_fit = createCostFits(df) min = df['cost'].min() max = df['cost'].max() - is_smaller = df_fit['y'] < max - df_fit = df_fit[is_smaller] - sns.lineplot('x', 'y', data=df_fit, hue='planner', ax=g.axes[0, 0], legend=False) + # Linear fit. + print(df.shape) + if(create_fits): + df_fit = createCostFits(df) + is_smaller = df_fit['y'] < max + df_fit = df_fit[is_smaller] + + sns.lineplot('x', 'y', data=df_fit, hue='planner', ax=g.axes[0, 0], legend=False) + g.set(xlim=(df['num_hole_vertices'].min() - 10, df['num_hole_vertices'].max() + 10)) g.set(ylim=(df['cost'].min() - 10, df['cost'].max() + 10)) g.set(xlabel="Number of Hole Vertices [1]") diff --git a/polygon_coverage_benchmark/test/benchmark-test.cpp b/polygon_coverage_benchmark/test/benchmark-test.cpp index f52f1b1..526676a 100644 --- a/polygon_coverage_benchmark/test/benchmark-test.cpp +++ b/polygon_coverage_benchmark/test/benchmark-test.cpp @@ -21,8 +21,7 @@ #include #include -#include -#include +#include #include #include #include @@ -31,12 +30,13 @@ #include #include +#include #include #include #include #include -const std::string kPackageName = "mav_coverage_planning_ros"; +const std::string kPackageName = "polygon_coverage_ros"; const std::string kResultsFile = "/tmp/coverage_results.txt"; const size_t kMaxNoObstacles = 15; const size_t kNthObstacle = 1; @@ -50,10 +50,10 @@ const Point_2 kStart(0.0, 0.0); const Point_2 kGoal = kStart; const double kMapScale = 1.0; -using namespace mav_coverage_planning; +using namespace polygon_coverage_planning; bool loadPolygonFromNode(const YAML::Node& node, Polygon_2* poly) { - CHECK_NOTNULL(poly); + ROS_ASSERT(poly); if (!node) return false; YAML::Node points = node["points"]; if (!points) return false; @@ -72,8 +72,8 @@ bool loadPolygonFromNode(const YAML::Node& node, Polygon_2* poly) { return true; } -bool loadPWHFromFile(const std::string& file, Polygon* polygon) { - CHECK_NOTNULL(polygon); +bool loadPWHFromFile(const std::string& file, PolygonWithHoles* polygon) { + ROS_ASSERT(polygon); YAML::Node node = YAML::LoadFile(file); @@ -88,26 +88,26 @@ bool loadPWHFromFile(const std::string& file, Polygon* polygon) { pwh = *diff.begin(); } - CHECK_NOTNULL(polygon); - *polygon = Polygon(pwh); + ROS_ASSERT(polygon); + *polygon = PolygonWithHoles(pwh); return true; } -size_t computeNoHoleVertices(const Polygon& poly) { +size_t computeNoHoleVertices(const PolygonWithHoles& poly) { size_t no_hole_vertices = 0; for (PolygonWithHoles::Hole_const_iterator hit = - poly.getPolygon().holes_begin(); - hit != poly.getPolygon().holes_end(); ++hit) { + poly.holes_begin(); + hit != poly.holes_end(); ++hit) { no_hole_vertices += hit->size(); } return no_hole_vertices; } -bool loadAllInstances(std::vector* polys, +bool loadAllInstances(std::vector* polys, std::vector* names) { - CHECK_NOTNULL(polys); - CHECK_NOTNULL(names); + ROS_ASSERT(polys); + ROS_ASSERT(names); polys->reserve(kObstacleBins * kNoInstances); names->reserve(kObstacleBins * kNoInstances); @@ -122,7 +122,7 @@ bool loadAllInstances(std::vector* polys, for (size_t j = 0; j < kNoInstances; ++j) { std::stringstream ss; ss << std::setw(4) << std::setfill('0') << j; - polys->push_back(Polygon()); + polys->push_back(PolygonWithHoles()); if (!loadPWHFromFile(subfolder + ss.str() + ".yaml", &polys->back())) return false; names->push_back(std::to_string(i * kNthObstacle) + "/" + ss.str()); @@ -132,15 +132,13 @@ bool loadAllInstances(std::vector* polys, return true; } -template -typename StripmapPlanner::Settings createSettings( - Polygon poly, const DecompositionType& decom, bool sweep_single_direction) { - typename StripmapPlanner::Settings settings; +sweep_plan_graph::SweepPlanGraph::Settings createSettings( + PolygonWithHoles poly, const DecompositionType& decom, bool sweep_single_direction) { + sweep_plan_graph::SweepPlanGraph::Settings settings; settings.polygon = poly; - settings.path_cost_function = std::bind(&computeVelocityRampPathCost, + settings.cost_function = std::bind(&computeVelocityRampPathCost, std::placeholders::_1, kVMax, kAMax); settings.sensor_model = std::make_shared(kSweepDistance, kOverlap); - settings.sweep_around_obstacles = false; settings.offset_polygons = true; settings.decomposition_type = decom; settings.sweep_single_direction = sweep_single_direction; @@ -239,8 +237,8 @@ void saveTimes(Result* result) { template bool runPlanner(StripmapPlanner* planner, Result* result) { - CHECK_NOTNULL(planner); - CHECK_NOTNULL(result); + ROS_ASSERT(planner); + ROS_ASSERT(result); ROS_INFO_STREAM("Planning with: " << result->planner); // Setup. @@ -257,9 +255,10 @@ bool runPlanner(StripmapPlanner* planner, Result* result) { // Save results. result->cost = computeVelocityRampPathCost(solution, kVMax, kAMax); saveTimes(result); - result->num_cells = planner->getDecompositionSize(); - result->num_nodes = planner->getNumberOfNodes(); - result->num_edges = planner->getNumberOfEdges(); + //TODO(stlucas): may change protection of these stats + //result->num_cells = planner->settings_.getDecompositionSize(); + //result->num_nodes = planner->sweep_plan_graph_.size(); + //result->num_edges = planner->sweep_plan_graph_.getNumberOfEdges(); // Get times. timing::Timing::Print(std::cout); @@ -267,14 +266,18 @@ bool runPlanner(StripmapPlanner* planner, Result* result) { return true; } -TEST(BenchmarkTest, Benchmark) { - std::vector polys; +void runBenchmark() { + std::vector polys; std::vector names; // Load polygons. ROS_INFO_STREAM("Loading " << kObstacleBins * kNoInstances << " test instances."); - EXPECT_TRUE(loadAllInstances(&polys, &names)); + if(!loadAllInstances(&polys, &names)) + { + ROS_ERROR("Failed to load all instances"); + return; + } // Run planners. for (size_t i = 0; i < polys.size(); ++i) { @@ -283,7 +286,7 @@ TEST(BenchmarkTest, Benchmark) { // Number of hole vertices. size_t num_hole_vertices = computeNoHoleVertices(polys[i]); - size_t num_holes = polys[i].getPolygon().number_of_holes(); + size_t num_holes = polys[i].number_of_holes(); ROS_INFO_STREAM("Number of holes: " << num_holes); // Create results. @@ -306,20 +309,20 @@ TEST(BenchmarkTest, Benchmark) { one_dir_exact_result.planner = "one_dir_exact"; // Create settings. - PolygonStripmapPlanner::Settings our_bcd_settings = - createSettings(polys[i], + sweep_plan_graph::SweepPlanGraph::Settings our_bcd_settings = + createSettings(polys[i], DecompositionType::kBCD, false); - PolygonStripmapPlanner::Settings our_tcd_settings = - createSettings( - polys[i], DecompositionType::kTrapezoidal, false); - PolygonStripmapPlanner::Settings one_dir_gkma_settings = - createSettings(polys[i], + sweep_plan_graph::SweepPlanGraph::Settings our_tcd_settings = + createSettings( + polys[i], DecompositionType::kTCD, false); + sweep_plan_graph::SweepPlanGraph::Settings one_dir_gkma_settings = + createSettings(polys[i], DecompositionType::kBCD, true); - PolygonStripmapPlanner::Settings gtsp_exact_settings = - createSettings(polys[i], + sweep_plan_graph::SweepPlanGraph::Settings gtsp_exact_settings = + createSettings(polys[i], DecompositionType::kBCD, false); - PolygonStripmapPlanner::Settings one_dir_exact_settings = - createSettings(polys[i], + sweep_plan_graph::SweepPlanGraph::Settings one_dir_exact_settings = + createSettings(polys[i], DecompositionType::kBCD, true); // Create planners. @@ -328,37 +331,39 @@ TEST(BenchmarkTest, Benchmark) { PolygonStripmapPlanner one_dir_gkma(one_dir_gkma_settings); PolygonStripmapPlannerExact gtsp_exact(gtsp_exact_settings); PolygonStripmapPlannerExact one_dir_exact(one_dir_exact_settings); + // Run planners. - EXPECT_TRUE(runPlanner(&our_bcd, &our_bcd_result)); - EXPECT_TRUE(runPlanner(&our_tcd, &our_tcd_result)); - EXPECT_TRUE(runPlanner(&one_dir_gkma, + ROS_ASSERT(runPlanner(&our_bcd, &our_bcd_result)); + ROS_ASSERT(runPlanner(&our_tcd, &our_tcd_result)); + ROS_ASSERT(runPlanner(&one_dir_gkma, &one_dir_gkma_result)); bool success_gtsp_exact = false; bool success_one_dir_exact = false; + if (num_holes < 3) { success_gtsp_exact = runPlanner( - >sp_exact, >sp_exact_result); + >sp_exact, >sp_exact_result); success_one_dir_exact = runPlanner( - &one_dir_exact, &one_dir_exact_result); + &one_dir_exact, &one_dir_exact_result); } // Save results. - if (i == 0) EXPECT_TRUE(initCsv(kResultsFile, our_bcd_result)); + if (i == 0) ROS_ASSERT(initCsv(kResultsFile, our_bcd_result)); - EXPECT_TRUE(resultToCsv(kResultsFile, our_bcd_result)); - EXPECT_TRUE(resultToCsv(kResultsFile, our_tcd_result)); - EXPECT_TRUE(resultToCsv(kResultsFile, one_dir_gkma_result)); + ROS_ASSERT(resultToCsv(kResultsFile, our_bcd_result)); + ROS_ASSERT(resultToCsv(kResultsFile, our_tcd_result)); + ROS_ASSERT(resultToCsv(kResultsFile, one_dir_gkma_result)); if (success_gtsp_exact) - EXPECT_TRUE(resultToCsv(kResultsFile, gtsp_exact_result)); + ROS_ASSERT(resultToCsv(kResultsFile, gtsp_exact_result)); if (success_one_dir_exact) - EXPECT_TRUE(resultToCsv(kResultsFile, one_dir_exact_result)); + ROS_ASSERT(resultToCsv(kResultsFile, one_dir_exact_result)); } + } int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - google::InitGoogleLogging(argv[0]); - return RUN_ALL_TESTS(); + runBenchmark(); + return 0; } diff --git a/polygon_coverage_planners/src/graphs/gtspp_product_graph.cc b/polygon_coverage_planners/src/graphs/gtspp_product_graph.cc index dcdaf41..e56f463 100644 --- a/polygon_coverage_planners/src/graphs/gtspp_product_graph.cc +++ b/polygon_coverage_planners/src/graphs/gtspp_product_graph.cc @@ -94,7 +94,7 @@ bool GtsppProductGraph::addStartNode(const NodeProperty& node_property) { auto current_time = std::chrono::high_resolution_clock::now(); std::chrono::duration elapsed = current_time - start_time; if (elapsed.count() > kTimeOut) { - ROS_ERROR("Timout addStartNode."); + ROS_ERROR("Timeout addStartNode."); return false; } if (lattice_id == boolean_lattice_->getStartIdx()) { @@ -119,7 +119,7 @@ bool GtsppProductGraph::addGoalNode(const NodeProperty& node_property) { auto current_time = std::chrono::high_resolution_clock::now(); std::chrono::duration elapsed = current_time - start_time; if (elapsed.count() > kTimeOut) { - ROS_ERROR("Timout addGoalNode."); + ROS_ERROR("Timeout addGoalNode."); return false; } if (lattice_id == boolean_lattice_->getGoalIdx()) { @@ -459,7 +459,7 @@ bool GtsppProductGraph::createDijkstra(Solution* solution) { auto current_time = std::chrono::high_resolution_clock::now(); std::chrono::duration elapsed = current_time - start_time; if (elapsed.count() > kTimeOut) { - ROS_ERROR("Timout createDijkstra."); + ROS_ERROR("Timeout createDijkstra."); return false; } // Pop vertex with lowest score from open set.