Merge branch 'master' into planner_cspace/improve-multi-threadding-pe…

…rformance

planner_cspace/include/planner_cspace/blockmem_gridmap.h

@@ -30,14 +30,25 @@
 #ifndef PLANNER_CSPACE_BLOCKMEM_GRIDMAP_H
 #define PLANNER_CSPACE_BLOCKMEM_GRIDMAP_H
 
+#include <bitset>
 #include <limits>
 
 #include <planner_cspace/cyclic_vec.h>
 
-template <class T, int DIM, int NONCYCLIC, int BLOCK_WIDTH = 0x20>
+template <class T, int DIM, int NONCYCLIC, int BLOCK_WIDTH = 0x20, bool ENABLE_VALIDATION = false>
 class BlockMemGridmap
 {
+private:
+  static constexpr bool isPowOf2(const int v)
+  {
+    return v && ((v & (v - 1)) == 0);
+  }
+  static_assert(isPowOf2(BLOCK_WIDTH), "BLOCK_WIDTH must be power of 2");
+
 protected:
+  constexpr static size_t block_bit_ = std::lround(std::log2(BLOCK_WIDTH));
+  constexpr static size_t block_bit_mask_ = (1 << block_bit_) - 1;
+
   std::unique_ptr<T[]> c_;
   CyclicVecInt<DIM, NONCYCLIC> size_;
   CyclicVecInt<DIM, NONCYCLIC> block_size_;
@@ -46,17 +57,16 @@ class BlockMemGridmap
   size_t block_num_;
   T dummy_;
 
-  void block_addr(
+  inline void block_addr(
       const CyclicVecInt<DIM, NONCYCLIC>& pos, size_t& baddr, size_t& addr) const
   {
     addr = 0;
     baddr = 0;
     for (int i = 0; i < NONCYCLIC; i++)
     {
-      addr *= BLOCK_WIDTH;
-      addr += pos[i] % BLOCK_WIDTH;
+      addr = (addr << block_bit_) + (pos[i] & block_bit_mask_);
       baddr *= block_size_[i];
-      baddr += pos[i] / BLOCK_WIDTH;
+      baddr += pos[i] >> block_bit_;
     }
     for (int i = NONCYCLIC; i < DIM; i++)
     {
@@ -69,7 +79,7 @@ class BlockMemGridmap
   std::function<void(CyclicVecInt<3, 2>, size_t&, size_t&)> getAddressor() const
   {
     return std::bind(
-        &BlockMemGridmap<T, DIM, NONCYCLIC, BLOCK_WIDTH>::block_addr,
+        &BlockMemGridmap<T, DIM, NONCYCLIC, BLOCK_WIDTH, ENABLE_VALIDATION>::block_addr,
         this,
         std::placeholders::_1, std::placeholders::_2, std::placeholders::_3);
   }
@@ -98,7 +108,7 @@ class BlockMemGridmap
   }
   void reset(const CyclicVecInt<DIM, NONCYCLIC>& size)
   {
-    auto size_tmp = size;
+    CyclicVecInt<DIM, NONCYCLIC> size_tmp = size;
 
     for (int i = 0; i < NONCYCLIC; i++)
     {
@@ -131,30 +141,37 @@ class BlockMemGridmap
     size_ = size;
   }
   explicit BlockMemGridmap(const CyclicVecInt<DIM, NONCYCLIC>& size_)
+    : BlockMemGridmap()
   {
     reset(size_);
   }
   BlockMemGridmap()
+    : dummy_(std::numeric_limits<T>::max())
   {
   }
   T& operator[](const CyclicVecInt<DIM, NONCYCLIC>& pos)
   {
     size_t baddr, addr;
     block_addr(pos, baddr, addr);
-    if (addr >= block_ser_size_ || baddr >= block_num_)
+    const size_t a = baddr * block_ser_size_ + addr;
+    if (ENABLE_VALIDATION)
     {
-      dummy_ = std::numeric_limits<T>::max();
-      return dummy_;
+      if (a >= ser_size_)
+        return dummy_;
     }
-    return c_[baddr * block_ser_size_ + addr];
+    return c_[a];
   }
   const T operator[](const CyclicVecInt<DIM, NONCYCLIC>& pos) const
   {
     size_t baddr, addr;
     block_addr(pos, baddr, addr);
-    if (addr >= block_ser_size_ || baddr >= block_num_)
-      return std::numeric_limits<T>::max();
-    return c_[baddr * block_ser_size_ + addr];
+    const size_t a = baddr * block_ser_size_ + addr;
+    if (ENABLE_VALIDATION)
+    {
+      if (a >= ser_size_)
+        return std::numeric_limits<T>::max();
+    }
+    return c_[a];
   }
   bool validate(const CyclicVecInt<DIM, NONCYCLIC>& pos, const int tolerance = 0) const
   {
@@ -170,8 +187,8 @@ class BlockMemGridmap
     }
     return true;
   }
-  const BlockMemGridmap<T, DIM, NONCYCLIC, BLOCK_WIDTH>& operator=(
-      const BlockMemGridmap<T, DIM, NONCYCLIC, BLOCK_WIDTH>& gm)
+  const BlockMemGridmap<T, DIM, NONCYCLIC, BLOCK_WIDTH, ENABLE_VALIDATION>& operator=(
+      const BlockMemGridmap<T, DIM, NONCYCLIC, BLOCK_WIDTH, ENABLE_VALIDATION>& gm)
   {
     reset(gm.size_);
     memcpy(c_.get(), gm.c_.get(), ser_size_);

planner_cspace/include/planner_cspace/planner_3d/motion_cache.h

@@ -80,6 +80,11 @@ class MotionCache
     return cache_[i].cend();
   }
 
+  inline const CyclicVecInt<3, 2>& getMaxRange() const
+  {
+    return max_range_;
+  }
+
   void reset(
       const float linear_resolution,
       const float angular_resolution,
@@ -89,6 +94,7 @@ class MotionCache
 protected:
   std::vector<Cache> cache_;
   int page_size_;
+  CyclicVecInt<3, 2> max_range_;
 };
 
 #endif  // PLANNER_CSPACE_PLANNER_3D_MOTION_CACHE_H

planner_cspace/src/motion_cache.cpp

@@ -42,6 +42,7 @@ void MotionCache::reset(
 {
   const int angle = std::lround(M_PI * 2 / angular_resolution);
 
+  CyclicVecInt<3, 2> max_range(0, 0, 0);
   page_size_ = angle;
   cache_.resize(angle);
   for (int syaw = 0; syaw < angle; syaw++)
@@ -89,6 +90,8 @@ void MotionCache::reset(
               if (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;
               }
             }
@@ -164,4 +167,5 @@ void MotionCache::reset(
       std::sort(cache.second.motion_.begin(), cache.second.motion_.end(), comp);
     }
   }
+  max_range_ = max_range;
 }

planner_cspace/src/planner_3d.cpp

@@ -232,6 +232,8 @@ class Planner3dNode
 
   MotionCache motion_cache_;
   MotionCache motion_cache_linear_;
+  Astar::Vec min_boundary_;
+  Astar::Vec max_boundary_;
 
   diagnostic_updater::Updater diag_updater_;
   ros::Duration costmap_watchdog_;
@@ -802,7 +804,7 @@ class Planner3dNode
         sensor_msgs::PointCloud pc;
         pc.header = map_header_;
         pc.header.stamp = now;
-        Astar::Vec p;
+        Astar::Vec p(0, 0, 0);
         for (p[1] = 0; p[1] < cm_hist_bbf_.size()[1]; p[1]++)
         {
           for (p[0] = 0; p[0] < cm_hist_bbf_.size()[0]; p[0]++)
@@ -1159,6 +1161,11 @@ class Planner3dNode
     cm_hist_bbf_.clear(bbf::BinaryBayesFilter(bbf::MIN_ODDS));
     cm_hist_.clear(0);
 
+    // Make boundary check threshold
+    min_boundary_ = motion_cache_.getMaxRange();
+    max_boundary_ = Astar::Vec(size[0], size[1], size[2]) - min_boundary_;
+    ROS_INFO("x:%d, y:%d grids around the boundary is ignored on path search", min_boundary_[0], min_boundary_[1]);
+
     updateGoal();
   }
   void cbAction()
@@ -1882,6 +1889,11 @@ class Planner3dNode
       if (std::abs(curv_radius) < min_curve_raduis_)
         return -1;
 
+      // Ignore boundary
+      if (s[0] < min_boundary_[0] || s[1] < min_boundary_[1] ||
+          s[0] >= max_boundary_[0] || s[1] >= max_boundary_[1])
+        return -1;
+
       if (fabs(max_vel_ / r1) > max_ang_vel_)
       {
         const float vel = fabs(curv_radius) * max_ang_vel_;

planner_cspace/test/CMakeLists.txt

@@ -16,19 +16,11 @@ catkin_add_gtest(test_motion_cache
 )
 target_link_libraries(test_motion_cache ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
-if(PLANNER_CSPACE_PERFORMANCE_TESTS)
-
-  catkin_add_gtest(test_blockmem_gridmap_performance
-    src/test_blockmem_gridmap_performance.cpp
-  )
-  target_link_libraries(test_blockmem_gridmap_performance ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenMP_CXX_FLAGS})
-  set_target_properties(test_blockmem_gridmap_performance PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_RELEASE}") # Force release build for performance test.
-
-else()
-
-  message("To perform performance tests for planner_cspace, use -DPLANNER_CSPACE_PERFORMANCE_TESTS:=ON")
-
-endif()
+catkin_add_gtest(test_blockmem_gridmap_performance
+  src/test_blockmem_gridmap_performance.cpp
+)
+target_link_libraries(test_blockmem_gridmap_performance ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenMP_CXX_FLAGS})
+set_target_properties(test_blockmem_gridmap_performance PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS_RELEASE}")  # Force release build for performance test.
 
 add_rostest_gtest(test_navigate
   test/navigation_rostest.test

planner_cspace/test/src/test_blockmem_gridmap.cpp

@@ -101,9 +101,9 @@ TEST(BlockmemGridmap, WriteRead)
   }
 }
 
-TEST(BlockmemGridmap, OuterBoundery)
+TEST(BlockmemGridmap, OuterBoundary)
 {
-  BlockMemGridmap<float, 3, 2, 0x20> gm;
+  BlockMemGridmap<float, 3, 2, 0x20, true> gm;
 
   const int s = 0x30;
   gm.reset(CyclicVecInt<3, 2>(s, s, s));