4320: Changed precision of calculations of the HybridNode MotionTable…

…::getClosestAngularBin. (#4324) Signed-off-by: Krzysztof Pawełczyk <k.t.pawelczyk@gmail.com> Co-authored-by: Krzysztof Pawełczyk <kpawelczyk@autonomous-systems.pl>
ros-navigation · May 10, 2024 · 30d3aff · 30d3aff
1 parent 7ef4473
commit 30d3aff
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 2 deletions.
diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/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)

diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/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);
+  }
 }