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>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

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)

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);
+  }
+}