planner_3d: remove duplicated interpolation method (#733)

planner_cspace/CMakeLists.txt

@@ -52,7 +52,6 @@ add_executable(planner_3d
   src/grid_astar_model_3dof.cpp
   src/motion_cache.cpp
   src/motion_primitive_builder.cpp
-  src/path_interpolator.cpp
   src/planner_3d.cpp
   src/distance_map.cpp
   src/rotation_cache.cpp

planner_cspace/include/planner_cspace/planner_3d/grid_astar_model.h

@@ -31,6 +31,7 @@
 #define PLANNER_CSPACE_PLANNER_3D_GRID_ASTAR_MODEL_H
 
 #include <array>
+#include <list>
 #include <memory>
 #include <utility>
 #include <vector>
@@ -41,7 +42,6 @@
 #include <planner_cspace/cyclic_vec.h>
 #include <planner_cspace/grid_astar_model.h>
 #include <planner_cspace/planner_3d/motion_cache.h>
-#include <planner_cspace/planner_3d/path_interpolator.h>
 #include <planner_cspace/planner_3d/rotation_cache.h>
 
 namespace planner_cspace
@@ -78,8 +78,6 @@ class GridAstarModel3D : public GridAstarModelBase<3, 2>
   using Vec = CyclicVecInt<3, 2>;
   using Vecf = CyclicVecFloat<3, 2>;
 
-  PathInterpolator path_interpolator_;
-
 protected:
   bool hysteresis_;
   costmap_cspace_msgs::MapMetaData3D map_info_;
@@ -111,7 +109,9 @@ class GridAstarModel3D : public GridAstarModelBase<3, 2>
       const BlockMemGridmapBase<char, 3, 2>& cm_hyst,
       const BlockMemGridmapBase<char, 3, 2>& cm_rough,
       const CostCoeff& cc,
-      const int range);
+      const int range,
+      const float path_interpolation_resolution = 0.5,
+      const float grid_enumeration_resolution = 0.1);
   void enableHysteresis(const bool enable);
   void createEuclidCostCache();
   float euclidCost(const Vec& v) const;
@@ -125,6 +125,8 @@ class GridAstarModel3D : public GridAstarModelBase<3, 2>
       const Vec& p,
       const std::vector<VecWithCost>& ss,
       const Vec& es) const override;
+  std::list<Vecf> interpolatePath(
+      const std::list<Vec>& path) const;
 };
 
 class GridAstarModel2D : public GridAstarModelBase<3, 2>

planner_cspace/include/planner_cspace/planner_3d/motion_cache.h

@@ -31,6 +31,7 @@
 #define PLANNER_CSPACE_PLANNER_3D_MOTION_CACHE_H
 
 #include <memory>
+#include <list>
 #include <unordered_map>
 #include <vector>
 
@@ -49,6 +50,7 @@ class MotionCache
     friend class MotionCache;
 
     std::vector<CyclicVecInt<3, 2>> motion_;
+    std::vector<CyclicVecFloat<3, 2>> interpolated_motion_;
     float distance_;
 
   public:
@@ -60,6 +62,10 @@ class MotionCache
     {
       return motion_;
     }
+    const std::vector<CyclicVecFloat<3, 2>>& getInterpolatedMotion() const
+    {
+      return interpolated_motion_;
+    }
   };
 
   using Cache =
@@ -76,6 +82,14 @@ class MotionCache
       i += page_size_;
     return cache_[i].find(goal);
   }
+  inline const typename Cache::const_iterator find(
+      const CyclicVecInt<3, 2>& from,
+      const CyclicVecInt<3, 2>& to) const
+  {
+    const int start_yaw = from[2];
+    const CyclicVecInt<3, 2> goal(to[0] - from[0], to[1] - from[1], to[2]);
+    return find(start_yaw, goal);
+  }
   inline const typename Cache::const_iterator end(
       const int start_yaw) const
   {
@@ -94,7 +108,11 @@ class MotionCache
       const float linear_resolution,
       const float angular_resolution,
       const int range,
-      const std::function<void(CyclicVecInt<3, 2>, size_t&, size_t&)> gm_addr);
+      const std::function<void(CyclicVecInt<3, 2>, size_t&, size_t&)> gm_addr,
+      const float interpolation_resolution,
+      const float grid_enumeration_resolution);
+
+  std::list<CyclicVecFloat<3, 2>> interpolatePath(const std::list<CyclicVecInt<3, 2>>& path_grid) const;
 
 protected:
   std::vector<Cache> cache_;

planner_cspace/src/grid_astar_model_3dof.cpp

@@ -29,6 +29,7 @@
 
 #include <cmath>
 #include <limits>
+#include <list>
 #include <utility>
 #include <vector>
 
@@ -40,7 +41,6 @@
 #include <planner_cspace/planner_3d/grid_astar_model.h>
 #include <planner_cspace/planner_3d/motion_cache.h>
 #include <planner_cspace/planner_3d/motion_primitive_builder.h>
-#include <planner_cspace/planner_3d/path_interpolator.h>
 #include <planner_cspace/planner_3d/rotation_cache.h>
 
 namespace planner_cspace
@@ -56,7 +56,9 @@ GridAstarModel3D::GridAstarModel3D(
     const BlockMemGridmapBase<char, 3, 2>& cm_hyst,
     const BlockMemGridmapBase<char, 3, 2>& cm_rough,
     const CostCoeff& cc,
-    const int range)
+    const int range,
+    const float path_interpolation_resolution,
+    const float grid_enumeration_resolution)
   : hysteresis_(false)
   , map_info_(map_info)
   , euclid_cost_coef_(euclid_cost_coef)
@@ -81,12 +83,16 @@ GridAstarModel3D::GridAstarModel3D(
       map_info_linear.linear_resolution,
       map_info_linear.angular_resolution,
       range_,
-      cm_rough_.getAddressor());
+      cm_rough_.getAddressor(),
+      path_interpolation_resolution,
+      grid_enumeration_resolution);
   motion_cache_.reset(
       map_info_.linear_resolution,
       map_info_.angular_resolution,
       range_,
-      cm_.getAddressor());
+      cm_.getAddressor(),
+      path_interpolation_resolution,
+      grid_enumeration_resolution);
 
   // Make boundary check threshold
   min_boundary_ = motion_cache_.getMaxRange();
@@ -112,7 +118,6 @@ GridAstarModel3D::GridAstarModel3D(
       search_list_rough_.push_back(d);
     }
   }
-  path_interpolator_.reset(map_info_.angular_resolution, range_);
 }
 
 void GridAstarModel3D::enableHysteresis(const bool enable)
@@ -319,6 +324,11 @@ const std::vector<GridAstarModel3D::Vec>& GridAstarModel3D::searchGrids(
   return search_list_rough_;
 }
 
+std::list<GridAstarModel3D::Vecf> GridAstarModel3D::interpolatePath(const std::list<Vec>& grid_path) const
+{
+  return motion_cache_.interpolatePath(grid_path);
+}
+
 float GridAstarModel2D::cost(
     const Vec& cur, const Vec& next, const std::vector<VecWithCost>& start, const Vec& goal) const
 {
@@ -358,5 +368,6 @@ const std::vector<GridAstarModel3D::Vec>& GridAstarModel2D::searchGrids(
 {
   return base_->search_list_rough_;
 }
+
 }  // namespace planner_3d
 }  // namespace planner_cspace