From 5ed817637ead935f07e7d9729929df9f1e80b566 Mon Sep 17 00:00:00 2001 From: Hunter Song Date: Thu, 15 Feb 2024 18:57:29 -0500 Subject: [PATCH 1/2] Added cast to float to getClosestAngularBin return for behavior consistency. Signed-off-by: Hunter Song --- nav2_smac_planner/src/node_hybrid.cpp | 2 +- nav2_smac_planner/test/test_nodehybrid.cpp | 30 ++++++++++++++++++++++ 2 files changed, 31 insertions(+), 1 deletion(-) diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 71158f32ff..2e81a5f47c 100644 --- 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(floor(theta / bin_size)); + return static_cast(floor(static_cast(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 index 7ed61c4ec4..ce08bb37b5 100644 --- 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_noPi_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); + } +} From f21b70fde9164b2eaa50d0a573c17425d818dbc5 Mon Sep 17 00:00:00 2001 From: Hunter Song Date: Mon, 19 Feb 2024 19:04:52 -0500 Subject: [PATCH 2/2] Revised test name Signed-off-by: Hunter Song --- nav2_smac_planner/test/test_nodehybrid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index ce08bb37b5..1c127c4799 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -375,7 +375,7 @@ TEST(NodeHybridTest, test_node_reeds_neighbors) EXPECT_EQ(neighbors.size(), 0u); } -TEST(NodeHybridTest, basic_get_closest_angular_bin_noPi_test) +TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { // Tests to check getClosestAngularBin behavior for different input types nav2_smac_planner::HybridMotionTable motion_table;