Merge pull request #129 from ar0ser0/flag-to-redirect-swaths-in-genRoute

add redirect flag

include/fields2cover/route_planning/route_planner_base.h

@@ -33,7 +33,7 @@ class RoutePlannerBase {
   /// @return Route that covers all the swaths
   virtual F2CRoute genRoute(
       const F2CCells& cells, const F2CSwathsByCells& swaths_by_cells,
-      bool show_log = false, double d_tol = 1e-4);
+      bool show_log = false, double d_tol = 1e-4, bool redirect_swaths = true);
 
   /// Set the start and the end of the route.
   void setStartAndEndPoint(const F2CPoint& p);
@@ -58,7 +58,7 @@ class RoutePlannerBase {
   virtual F2CGraph2D createCoverageGraph(
       const F2CCells& cells, const F2CSwathsByCells& swaths_by_cells,
       F2CGraph2D& shortest_graph,
-      double d_tol) const;
+      double d_tol, bool redirect_swaths = true) const;
 
 
  protected:

src/fields2cover/path_planning/path_planning.cpp

@@ -111,7 +111,7 @@ std::vector<std::pair<F2CPoint, double>> PathPlanning::simplifyConnection(
   std::vector<std::pair<F2CPoint, double>> path;
   path.emplace_back(p1, ang1);
 
-  if (p1.distance(p2) < 6.0 * safe_dist || mp.size() <2) {
+  if (p1.distance(p2) < 6.0 * safe_dist || mp.size() < 2) {
     path.emplace_back(p2, ang2);
     return path;
   }
@@ -125,9 +125,17 @@ std::vector<std::pair<F2CPoint, double>> PathPlanning::simplifyConnection(
     }
   }
 
+  if (vp.size() < 2) {
+    path.emplace_back(p2, ang2);
+    return path;
+  }
+
   for (int i = 1; i < vp.size() - 1; ++i) {
     double dist_in  = vp[i].distance(vp[i-1]);
     double dist_out  = vp[i].distance(vp[i+1]);
+    if (dist_in == 0.0 || dist_out == 0.0){
+      continue;
+    }
     double d_in  = min(0.5 * dist_in,  safe_dist);
     double d_out  = min(0.5 * dist_out,  safe_dist);
     F2CPoint p_in =  (vp[i-1] - vp[i]) * (d_in  / dist_in)  + vp[i];

src/fields2cover/route_planning/route_planner_base.cpp

@@ -20,11 +20,11 @@ namespace ortools = operations_research;
 
 F2CRoute RoutePlannerBase::genRoute(
     const F2CCells& cells, const F2CSwathsByCells& swaths,
-    bool show_log, double d_tol) {
+    bool show_log, double d_tol, bool redirect_swaths) {
   F2CGraph2D shortest_graph = createShortestGraph(cells, swaths, d_tol);
 
   F2CGraph2D cov_graph = createCoverageGraph(
-      cells, swaths, shortest_graph, d_tol);
+      cells, swaths, shortest_graph, d_tol, redirect_swaths);
 
   std::vector<int64_t> v_route = computeBestRoute(cov_graph, show_log);
   return transformSolutionToRoute(
@@ -89,13 +89,18 @@ F2CGraph2D RoutePlannerBase::createShortestGraph(
 F2CGraph2D RoutePlannerBase::createCoverageGraph(
     const F2CCells& cells, const F2CSwathsByCells& swaths_by_cells,
     F2CGraph2D& shortest_graph,
-    double d_tol) const {
+    double d_tol, bool redirect_swaths) const {
   F2CGraph2D g;
   for (auto&& swaths : swaths_by_cells) {
     for (auto&& s : swaths) {
       F2CPoint mid_p {(s.startPoint() + s.endPoint()) * 0.5};
-      g.addEdge(s.startPoint(), mid_p, 0);
-      g.addEdge(s.endPoint(), mid_p, 0);
+      if (redirect_swaths) {
+        g.addEdge(s.startPoint(), mid_p, 0);
+        g.addEdge(s.endPoint(), mid_p, 0);
+      } else {
+        g.addDirectedEdge(s.startPoint(), mid_p, 0);
+        g.addDirectedEdge(mid_p, s.endPoint(), 0);
+      }
     }
   }
 
@@ -107,10 +112,14 @@ F2CGraph2D RoutePlannerBase::createCoverageGraph(
         for (const auto& s2 : swaths2) {
           auto s2_s = s2.startPoint();
           auto s2_e = s2.endPoint();
-          g.addEdge(s1_s, s2_s, shortest_graph);
-          g.addEdge(s1_s, s2_e, shortest_graph);
-          g.addEdge(s1_e, s2_s, shortest_graph);
-          g.addEdge(s1_e, s2_e, shortest_graph);
+          if (redirect_swaths) {
+            g.addEdge(s1_s, s2_s, shortest_graph);
+            g.addEdge(s1_e, s2_e, shortest_graph);
+            g.addEdge(s1_s, s2_e, shortest_graph);
+            g.addEdge(s1_e, s2_s, shortest_graph);
+          } else {
+            g.addDirectedEdge(s1_e, s2_s, shortest_graph);
+          }
         }
       }
     }

tests/cpp/route_planning/route_planner_base_test.cpp

@@ -57,6 +57,66 @@ TEST(fields2cover_rp_route_plan_base, simple_example) {
   */
 }
 
+TEST(fields2cover_rp_route_plan_base, redirect_flag) {
+  f2c::Random rand(4);
+  F2CCells cells {
+    F2CCell(F2CLinearRing({
+          F2CPoint(0,0), F2CPoint(2,0),F2CPoint(2,2),F2CPoint(0,2), F2CPoint(0,0)
+    }))
+  };
+  cells.addRing(0, F2CLinearRing({
+          F2CPoint(.4,.4), F2CPoint(.4,.6),F2CPoint(.6,.6),F2CPoint(.6,.4), F2CPoint(.4,.4)
+  }));
+  cells.addRing(0, F2CLinearRing({
+          F2CPoint(1.2,1.2), F2CPoint(1.2,1.6),F2CPoint(1.6,1.6),F2CPoint(1.6,1.2), F2CPoint(1.2,1.2)
+  }));
+
+  cells *= 3e1;
+
+  f2c::hg::ConstHL const_hl;
+  F2CCells no_hl = const_hl.generateHeadlandArea(cells, 1, 3);
+  auto hl_swaths = const_hl.generateHeadlandSwaths(cells, 1, 3, false);
+
+  f2c::decomp::BoustrophedonDecomp decomp;
+  decomp.setSplitAngle(M_PI/2.0);
+  auto decomp_cells = decomp.decompose(no_hl);
+
+  f2c::sg::BruteForce bf;
+  F2CSwathsByCells swaths = bf.generateSwaths(M_PI/2.0, 5, decomp_cells);
+
+  f2c::rp::RoutePlannerBase route_planner;
+  F2CRoute route = route_planner.genRoute(hl_swaths[1], swaths, false, 1e-4, false);
+
+  F2CSwaths old_swaths = swaths.flatten();
+  F2CSwaths new_swaths;
+  for (size_t sbc = 0; sbc < route.sizeVectorSwaths(); ++sbc) {
+    new_swaths.append(route.getSwaths(sbc));
+  }
+
+  EXPECT_EQ(new_swaths.size(), old_swaths.size());
+
+  for (size_t s = 0; s < new_swaths.size(); ++s) {
+    F2CSwath old_swath = old_swaths.at(s);
+    F2CSwath new_swath = new_swaths.at(s);
+    EXPECT_TRUE(new_swath.hasSameDir(old_swath));
+  }
+
+  EXPECT_FALSE(route.isEmpty());
+  EXPECT_GT(route.sizeVectorSwaths(), 1);
+  EXPECT_EQ(route.sizeVectorSwaths(), route.sizeConnections());
+
+  /*
+  f2c::Visualizer::figure();
+  f2c::Visualizer::plot(cells);
+  //f2c::Visualizer::plot(no_hl);
+  //f2c::Visualizer::plot(hl_swaths[1]);
+  //f2c::Visualizer::plot(decomp_cells);
+  //for (auto&& c : route.getConnections())
+  f2c::Visualizer::plot(route);
+  f2c::Visualizer::show();
+  */
+}
+