Fix/benchmark

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()
+catkin_python_setup()

polygon_coverage_benchmark/package.xml

@@ -11,4 +11,9 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <depend>rospy</depend>
+  <depend>roslib</depend>
+  <depend>polygon_coverage_msgs</depend>
+  <depend>polygon_coverage_geometry</depend>
+  <depend>polygon_coverage_planners</depend>
+  <depend>polygon_coverage_ros</depend>
 </package>

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]")

polygon_coverage_benchmark/test/benchmark-test.cpp

@@ -21,8 +21,7 @@
 #include <sstream>
 
 #include <CGAL/Boolean_set_operations_2.h>
-#include <glog/logging.h>
-#include <gtest/gtest.h>
+#include <ros/assert.h>
 #include <ros/package.h>
 #include <ros/ros.h>
 #include <yaml-cpp/yaml.h>
@@ -31,12 +30,13 @@
 
 #include <polygon_coverage_geometry/cgal_definitions.h>
 #include <polygon_coverage_planners/cost_functions/path_cost_functions.h>
+#include <polygon_coverage_planners/graphs/sweep_plan_graph.h>
 #include <polygon_coverage_planners/planners/polygon_stripmap_planner.h>
 #include <polygon_coverage_planners/planners/polygon_stripmap_planner_exact.h>
 #include <polygon_coverage_planners/sensor_models/line.h>
 #include <polygon_coverage_planners/timing.h>
 
-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<Polygon>* polys,
+bool loadAllInstances(std::vector<PolygonWithHoles>* polys,
                       std::vector<std::string>* 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<Polygon>* 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<Polygon>* polys,
   return true;
 }
 
-template <class StripmapPlanner>
-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<Line>(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 <class StripmapPlanner>
 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,24 +255,29 @@ 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);
   ROS_INFO_STREAM("Path cost: " << result->cost);
   return true;
 }
 
-TEST(BenchmarkTest, Benchmark) {
-  std::vector<Polygon> polys;
+void runBenchmark() {
+  std::vector<PolygonWithHoles> polys;
   std::vector<std::string> 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<PolygonStripmapPlanner>(polys[i],
+    sweep_plan_graph::SweepPlanGraph::Settings our_bcd_settings =
+        createSettings(polys[i],
                                                DecompositionType::kBCD, false);
-    PolygonStripmapPlanner::Settings our_tcd_settings =
-        createSettings<PolygonStripmapPlanner>(
-            polys[i], DecompositionType::kTrapezoidal, false);
-    PolygonStripmapPlanner::Settings one_dir_gkma_settings =
-        createSettings<PolygonStripmapPlanner>(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<PolygonStripmapPlanner>(polys[i],
+    sweep_plan_graph::SweepPlanGraph::Settings gtsp_exact_settings =
+        createSettings(polys[i],
                                                DecompositionType::kBCD, false);
-    PolygonStripmapPlanner::Settings one_dir_exact_settings =
-        createSettings<PolygonStripmapPlanner>(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<PolygonStripmapPlanner>(&our_bcd, &our_bcd_result));
-    EXPECT_TRUE(runPlanner<PolygonStripmapPlanner>(&our_tcd, &our_tcd_result));
-    EXPECT_TRUE(runPlanner<PolygonStripmapPlanner>(&one_dir_gkma,
+    ROS_ASSERT(runPlanner<PolygonStripmapPlanner>(&our_bcd, &our_bcd_result));
+    ROS_ASSERT(runPlanner<PolygonStripmapPlanner>(&our_tcd, &our_tcd_result));
+    ROS_ASSERT(runPlanner<PolygonStripmapPlanner>(&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<PolygonStripmapPlannerExact>(
-          &gtsp_exact, &gtsp_exact_result);
+         &gtsp_exact, &gtsp_exact_result);
       success_one_dir_exact = runPlanner<PolygonStripmapPlannerExact>(
-          &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;
 }

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<double> 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<double> 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<double> 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.