Add multiple resolution

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,7 +50,7 @@ class MotionCache
     friend class MotionCache;
 
     std::vector<CyclicVecInt<3, 2>> motion_;
-    std::vector<CyclicVecFloat<3, 2>> motion_f_;
+    std::vector<CyclicVecFloat<3, 2>> interpolated_motion_;
     float distance_;
 
   public:
@@ -63,7 +64,7 @@ class MotionCache
     }
     const std::vector<CyclicVecFloat<3, 2>>& getInterpolatedMotion() const
     {
-      return motion_f_;
+      return interpolated_motion_;
     }
   };
 
@@ -108,7 +109,8 @@ class MotionCache
       const float angular_resolution,
       const int range,
       const std::function<void(CyclicVecInt<3, 2>, size_t&, size_t&)> gm_addr,
-      const float interpolation_interval);
+      const float interpolation_resolution,
+      const float grid_enumeration_resolution = 0.1);
 
   std::list<CyclicVecFloat<3, 2>> interpolatePath(const std::list<CyclicVecInt<3, 2>>& path_grid) const;
 

planner_cspace/src/motion_cache.cpp

@@ -29,6 +29,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include <list>
 #include <unordered_map>
 #include <vector>
 
@@ -46,7 +47,8 @@ void MotionCache::reset(
     const float angular_resolution,
     const int range,
     const std::function<void(CyclicVecInt<3, 2>, size_t&, size_t&)> gm_addr,
-    const float interpolation_interval)
+    const float interpolation_resolution,
+    const float grid_enumeration_resolution)
 {
   const int angle = std::lround(M_PI * 2 / angular_resolution);
 
@@ -83,26 +85,32 @@ void MotionCache::reset(
           const float cos_v = cosf(motion[2]);
           const float sin_v = sinf(motion[2]);
 
-          const float inter = interpolation_interval / d.len();
-
+          const int total_step = static_cast<int>(std::round(d.len() / grid_enumeration_resolution));
+          const int interpolation_step =
+              std::max(1, static_cast<int>(std::round(interpolation_resolution / grid_enumeration_resolution)));
           if (std::abs(sin_v) < 0.1)
           {
-            for (float i = 0; i < 1.0 - inter / 2; i += inter)
+            for (int step = 0; step < total_step; ++step)
             {
-              const float x = diff_val[0] * i;
-              const float y = diff_val[1] * i;
+              const float ratio = step / static_cast<float>(total_step);
+              const float x = diff_val[0] * ratio;
+              const float y = diff_val[1] * ratio;
 
               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())
+
+              if ((pos != d) && (registered.find(pos) == registered.end()))
               {
                 page.motion_.push_back(pos);
                 for (int i = 0; i < 3; ++i)
                   max_range[i] = std::max(max_range[i], std::abs(pos[i]));
                 registered[pos] = true;
               }
-              page.motion_f_.push_back(posf);
+              if (step % interpolation_step == 0)
+              {
+                page.interpolated_motion_.push_back(posf);
+              }
             }
             page.distance_ = d.len();
             cache_[syaw][d] = page;
@@ -128,12 +136,13 @@ void MotionCache::reset(
 
           CyclicVecFloat<3, 2> posf_prev(0, 0, 0);
 
-          for (float i = 0; i < 1.0 - inter / 2; i += inter)
+          for (int step = 0; step < total_step; ++step)
           {
-            const float r = r1 * (1.0 - i) + r2 * i;
-            const float cx2 = cx_s * (1.0 - i) + cx * i;
-            const float cy2 = cy_s * (1.0 - i) + cy * i;
-            const float cyaw = yaw + i * dyaw;
+            const float ratio = step / static_cast<float>(total_step);
+            const float r = r1 * (1.0 - ratio) + r2 * ratio;
+            const float cx2 = cx_s * (1.0 - ratio) + cx * ratio;
+            const float cy2 = cy_s * (1.0 - ratio) + cy * ratio;
+            const float cyaw = yaw + ratio * dyaw;
 
             const float posf_raw[3] =
                 {
@@ -144,13 +153,17 @@ void MotionCache::reset(
             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())
+            if ((pos != d) && (registered.find(pos) == registered.end()))
             {
               page.motion_.push_back(pos);
               registered[pos] = true;
             }
-            page.motion_f_.push_back(posf);
-            distf += (posf - posf_prev).len();
+            if (step % interpolation_step == 0)
+            {
+              page.interpolated_motion_.push_back(posf);
+            }
+            if (ratio > 0)
+              distf += (posf - posf_prev).len();
             posf_prev = posf;
           }
           distf += (CyclicVecFloat<3, 2>(d) - posf_prev).len();
@@ -185,6 +198,10 @@ std::list<CyclicVecFloat<3, 2>> MotionCache::interpolatePath(const std::list<Cyc
   std::list<CyclicVecFloat<3, 2>> result;
   if (grid_path.size() < 2)
   {
+    for (const auto& p : grid_path)
+    {
+      result.push_back(CyclicVecFloat<3, 2>(p[0], p[1], p[2]));
+    }
     return result;
   }
   auto it_prev = grid_path.begin();

planner_cspace/test/src/test_start_pose_predictor.cpp

@@ -68,7 +68,7 @@ class StartPosePredictorTester : public ::testing::Test
     map_info_.origin.orientation.w = 1.0;
     map_info_.linear_resolution = 1.0;
     map_info_.angular_resolution = 2 * M_PI / map_info_.angle;
-    motion_cache_.reset(map_info_.linear_resolution, map_info_.angular_resolution, 10, cm_.getAddressor(), 0.1);
+    motion_cache_.reset(map_info_.linear_resolution, map_info_.angular_resolution, 4, cm_.getAddressor(), 0.1);
 
     const GridAstar<3, 2>::Vec size3d(
         static_cast<int>(map_info_.width),