From c59e0f362a867244d589562ecff56a9a5f23df59 Mon Sep 17 00:00:00 2001 From: hsong-MLE <158601314+hsong-MLE@users.noreply.github.com> Date: Mon, 19 Feb 2024 19:47:10 -0500 Subject: [PATCH] Added cast to float to getClosestAngularBin return for behavior consistency (#4123) * Added cast to float to getClosestAngularBin return for behavior consistency. Signed-off-by: Hunter Song * Revised test name Signed-off-by: Hunter Song --------- 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..1c127c4799 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_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); + } +}