4320: Changed precision of calculations of the HybridNode MotionTable::getClosestAngularBin. (#4324)

Signed-off-by: Krzysztof Pawełczyk <[email protected]>
Co-authored-by: Krzysztof Pawełczyk <[email protected]>

Author: AzaelCicero

nav2_smac_planner/src/node_hybrid.cpp

@@ -330,7 +330,8 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node)
 
 unsigned int HybridMotionTable::getClosestAngularBin(const double & theta)
 {
-  return static_cast<unsigned int>(floor(static_cast<float>(theta) / bin_size));
+  return static_cast<unsigned int>(floor(theta / static_cast<double>(bin_size))) %
+         num_angle_quantization;
 }
 
 float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx)

nav2_smac_planner/test/test_nodehybrid.cpp

@@ -13,6 +13,7 @@
 // limitations under the License. Reserved.
 
 #include <math.h>
+#include <cmath>
 #include <memory>
 #include <string>
 #include <vector>
@@ -384,6 +385,7 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test)
 
   {
     motion_table.bin_size = 3.1415926;
+    motion_table.num_angle_quantization = 2;
     double test_theta = 3.1415926;
     unsigned int expected_angular_bin = 1;
     unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
@@ -392,17 +394,28 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test)
 
   {
     motion_table.bin_size = M_PI;
+    motion_table.num_angle_quantization = 2;
     double test_theta = M_PI;
-    unsigned int expected_angular_bin = 1;
+    unsigned int expected_angular_bin = 0;
     unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
     EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
   }
 
   {
     motion_table.bin_size = M_PI;
+    motion_table.num_angle_quantization = 2;
     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);
   }
+
+  {
+    motion_table.bin_size = 0.0872664675;
+    motion_table.num_angle_quantization = 72;
+    double test_theta = 6.28318526567925;
+    unsigned int expected_angular_bin = 71;
+    unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
+    EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
+  }
 }