planner_cspace: fix debug build compatibility (#368)

at-wat · Aug 15, 2019 · 48634ac · 48634ac
1 parent a26750a
commit 48634ac
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 1 deletion.
diff --git a/planner_cspace/include/planner_cspace/blockmem_gridmap.h b/planner_cspace/include/planner_cspace/blockmem_gridmap.h
@@ -44,9 +44,15 @@ class BlockMemGridmap
     return v && ((v & (v - 1)) == 0);
   }
   static_assert(isPowOf2(BLOCK_WIDTH), "BLOCK_WIDTH must be power of 2");
+  static_assert(BLOCK_WIDTH > 0, "BLOCK_WIDTH must be >0");
+
+  static constexpr size_t log2Recursive(const size_t v, const size_t depth = 0)
+  {
+    return v == 1 ? depth : log2Recursive(v >> 1, depth + 1);
+  }
 
 protected:
-  constexpr static size_t block_bit_ = std::lround(std::log2(BLOCK_WIDTH));
+  constexpr static size_t block_bit_ = log2Recursive(BLOCK_WIDTH);
   constexpr static size_t block_bit_mask_ = (1 << block_bit_) - 1;
 
   std::unique_ptr<T[]> c_;

diff --git a/planner_cspace/test/src/test_blockmem_gridmap.cpp b/planner_cspace/test/src/test_blockmem_gridmap.cpp
@@ -34,6 +34,23 @@
 
 #include <planner_cspace/blockmem_gridmap.h>
 
+template <int BLOCK_WIDTH>
+class BlockMemGridmapHelper : public BlockMemGridmap<int, 1, 1, BLOCK_WIDTH, false>
+{
+public:
+  size_t getBlockBit() const
+  {
+    return this->block_bit_;
+  }
+};
+TEST(BlockmemGridmap, BlockWidth)
+{
+  ASSERT_EQ(4u, BlockMemGridmapHelper<0x10>().getBlockBit());
+  ASSERT_EQ(8u, BlockMemGridmapHelper<0x100>().getBlockBit());
+  ASSERT_EQ(12u, BlockMemGridmapHelper<0x1000>().getBlockBit());
+  ASSERT_EQ(16u, BlockMemGridmapHelper<0x10000>().getBlockBit());
+}
+
 TEST(BlockmemGridmap, ResetClear)
 {
   BlockMemGridmap<float, 3, 3, 0x20> gm;