planner_3d: remove duplicated interpolation method

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,8 @@ 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 double interpolation_interval = 1.0);
   void enableHysteresis(const bool enable);
   void createEuclidCostCache();
   float euclidCost(const Vec& v) const;
@@ -125,6 +124,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

@@ -49,6 +49,7 @@ class MotionCache
     friend class MotionCache;
 
     std::vector<CyclicVecInt<3, 2>> motion_;
+    std::vector<CyclicVecFloat<3, 2>> motion_f_;
     float distance_;
 
   public:
@@ -60,6 +61,10 @@ class MotionCache
     {
       return motion_;
     }
+    const std::vector<CyclicVecFloat<3, 2>>& getInterpolatedMotion() const
+    {
+      return motion_f_;
+    }
   };
 
   using Cache =
@@ -76,6 +81,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 +107,10 @@ 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_interval);
+
+  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,8 @@ 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 double interpolation_interval)
   : hysteresis_(false)
   , map_info_(map_info)
   , euclid_cost_coef_(euclid_cost_coef)
@@ -71,6 +72,7 @@ GridAstarModel3D::GridAstarModel3D(
   , cm_rough_(cm_rough)
   , cc_(cc)
   , range_(range)
+
 {
   rot_cache_.reset(map_info_.linear_resolution, map_info_.angular_resolution, range_);
 
@@ -81,12 +83,14 @@ GridAstarModel3D::GridAstarModel3D(
       map_info_linear.linear_resolution,
       map_info_linear.angular_resolution,
       range_,
-      cm_rough_.getAddressor());
+      cm_rough_.getAddressor(),
+      interpolation_interval);
   motion_cache_.reset(
       map_info_.linear_resolution,
       map_info_.angular_resolution,
       range_,
-      cm_.getAddressor());
+      cm_.getAddressor(),
+      interpolation_interval);
 
   // Make boundary check threshold
   min_boundary_ = motion_cache_.getMaxRange();
@@ -112,7 +116,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 +322,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 +366,6 @@ const std::vector<GridAstarModel3D::Vec>& GridAstarModel2D::searchGrids(
 {
   return base_->search_list_rough_;
 }
+
 }  // namespace planner_3d
 }  // namespace planner_cspace

planner_cspace/src/motion_cache.cpp

@@ -32,6 +32,8 @@
 #include <unordered_map>
 #include <vector>
 
+#include <ros/ros.h>
+
 #include <planner_cspace/cyclic_vec.h>
 #include <planner_cspace/planner_3d/motion_cache.h>
 
@@ -43,7 +45,8 @@ void MotionCache::reset(
     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_interval)
 {
   const int angle = std::lround(M_PI * 2 / angular_resolution);
 
@@ -80,17 +83,17 @@ void MotionCache::reset(
           const float cos_v = cosf(motion[2]);
           const float sin_v = sinf(motion[2]);
 
-          const float inter = 1.0 / d.len();
+          const float inter = interpolation_interval / d.len();
 
           if (std::abs(sin_v) < 0.1)
           {
-            for (float i = 0; i < 1.0; i += inter)
+            for (float i = 0; i < 1.0 - inter / 2; i += inter)
             {
               const float x = diff_val[0] * i;
               const float y = diff_val[1] * i;
 
-              CyclicVecInt<3, 2> pos(
-                  x / linear_resolution, y / linear_resolution, yaw / angular_resolution);
+              CyclicVecFloat<3, 2> posf(x / linear_resolution, y / linear_resolution, yaw / angular_resolution);
+              CyclicVecInt<3, 2> pos(posf[0], posf[1], posf[2]);
               pos.cycleUnsigned(angle);
               if (registered.find(pos) == registered.end())
               {
@@ -99,6 +102,7 @@ void MotionCache::reset(
                   max_range[i] = std::max(max_range[i], std::abs(pos[i]));
                 registered[pos] = true;
               }
+              page.motion_f_.push_back(posf);
             }
             page.distance_ = d.len();
             cache_[syaw][d] = page;
@@ -124,7 +128,7 @@ void MotionCache::reset(
 
           CyclicVecFloat<3, 2> posf_prev(0, 0, 0);
 
-          for (float i = 0; i < 1.0; i += inter)
+          for (float i = 0; i < 1.0 - inter / 2; i += inter)
           {
             const float r = r1 * (1.0 - i) + r2 * i;
             const float cx2 = cx_s * (1.0 - i) + cx * i;
@@ -137,14 +141,15 @@ void MotionCache::reset(
                     (cy2 - r * sinf(cyaw + M_PI / 2)) / linear_resolution,
                     cyaw / angular_resolution,
                 };
-            const CyclicVecFloat<3, 2> posf(posf_raw[0], posf_raw[1], posf_raw[2]);
+            CyclicVecFloat<3, 2> posf(posf_raw[0], posf_raw[1], posf_raw[2]);
             CyclicVecInt<3, 2> pos(posf_raw[0], posf_raw[1], posf_raw[2]);
             pos.cycleUnsigned(angle);
             if (registered.find(pos) == registered.end())
             {
               page.motion_.push_back(pos);
               registered[pos] = true;
             }
+            page.motion_f_.push_back(posf);
             distf += (posf - posf_prev).len();
             posf_prev = posf;
           }
@@ -174,5 +179,42 @@ void MotionCache::reset(
   }
   max_range_ = max_range;
 }
+
+std::list<CyclicVecFloat<3, 2>> MotionCache::interpolatePath(const std::list<CyclicVecInt<3, 2>>& grid_path) const
+{
+  std::list<CyclicVecFloat<3, 2>> result;
+  if (grid_path.size() < 2)
+  {
+    return result;
+  }
+  auto it_prev = grid_path.begin();
+  for (auto it = std::next(it_prev); it != grid_path.end(); ++it, ++it_prev)
+  {
+    if (((*it_prev)[0] == (*it)[0]) && ((*it_prev)[1] == (*it)[1]))
+    {
+      result.push_back(CyclicVecFloat<3, 2>((*it_prev)[0], (*it_prev)[1], (*it_prev)[2]));
+      continue;
+    }
+
+    const auto motion_it = find(*it_prev, *it);
+    if (motion_it == end((*it)[2]))
+    {
+      ROS_ERROR("Failed to find motion between [%d %d %d] and [%d %d %d]",
+                (*it_prev)[0], (*it_prev)[1], (*it_prev)[2], (*it)[0], (*it)[1], (*it)[2]);
+      result.push_back(CyclicVecFloat<3, 2>((*it_prev)[0], (*it_prev)[1], (*it_prev)[2]));
+    }
+    else
+    {
+      const auto& motion = motion_it->second.getInterpolatedMotion();
+      for (const auto& pos_diff : motion)
+      {
+        result.push_back(CyclicVecFloat<3, 2>((*it_prev)[0] + pos_diff[0], (*it_prev)[1] + pos_diff[1], pos_diff[2]));
+      }
+    }
+  }
+  result.push_back(CyclicVecFloat<3, 2>(grid_path.back()[0], grid_path.back()[1], grid_path.back()[2]));
+  return result;
+}
+
 }  // namespace planner_3d
 }  // namespace planner_cspace