Added cast to float to getClosestAngularBin return for behavior consi…

…stency (ros-navigation#4123) * Added cast to float to getClosestAngularBin return for behavior consistency. Signed-off-by: Hunter Song <hsong@macleanengineering.com> * Revised test name Signed-off-by: Hunter Song <hsong@macleanengineering.com> --------- Signed-off-by: Hunter Song <hsong@macleanengineering.com>
EnjoyRobotics · Feb 20, 2024 · c59e0f3 · c59e0f3
1 parent bc242b6
commit c59e0f3
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 1 deletion.
diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp
@@ -332,7 +332,7 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node)
 
 unsigned int HybridMotionTable::getClosestAngularBin(const double & theta)
 {
-  return static_cast<unsigned int>(floor(theta / bin_size));
+  return static_cast<unsigned int>(floor(static_cast<float>(theta) / bin_size));
 }
 
 float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx)

diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp
@@ -374,3 +374,33 @@ TEST(NodeHybridTest, test_node_reeds_neighbors)
   // should be empty since totally invalid
   EXPECT_EQ(neighbors.size(), 0u);
 }
+
+TEST(NodeHybridTest, basic_get_closest_angular_bin_test)
+{
+  // Tests to check getClosestAngularBin behavior for different input types
+  nav2_smac_planner::HybridMotionTable motion_table;
+
+  {
+    motion_table.bin_size = 3.1415926;
+    double test_theta = 3.1415926;
+    unsigned int expected_angular_bin = 1;
+    unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
+    EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
+  }
+
+  {
+    motion_table.bin_size = M_PI;
+    double test_theta = M_PI;
+    unsigned int expected_angular_bin = 1;
+    unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
+    EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
+  }
+
+  {
+    motion_table.bin_size = M_PI;
+    float test_theta = M_PI;
+    unsigned int expected_angular_bin = 1;
+    unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
+    EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
+  }
+}