From cbd5f46e422af75d5d3b5f83e696da037283fe57 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Krzysztof=20Pawe=C5=82czyk?= Date: Fri, 10 May 2024 09:24:20 +0200 Subject: [PATCH] 4320: Changed precision of calculations of the HybridNode MotionTable::getClosestAngularBin. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Krzysztof Pawełczyk --- nav2_smac_planner/src/node_hybrid.cpp | 3 ++- nav2_smac_planner/test/test_nodehybrid.cpp | 15 ++++++++++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index b18e8e559e..f5eec4ec5d 100644 --- 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(floor(static_cast(theta) / bin_size)); + return static_cast(floor(theta / static_cast(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 index f499ec8fb4..856f37c0a1 100644 --- 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 +#include #include #include #include @@ -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); + } }