From 1593c745726382311cdff2113b4543a4651057fc 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 (cherry picked from commit c59e0f362a867244d589562ecff56a9a5f23df59) --- 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 24b32f527b..73f774bf73 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -250,7 +250,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 16d6278bad..5580e64d04 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -304,3 +304,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); + } +}