From f706bc07236bf8457c732e59c654bc27fbb62071 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 19 Feb 2024 16:49:06 -0800 Subject: [PATCH] Added cast to float to getClosestAngularBin return for behavior consistency (#4123) (#4124) * 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) Co-authored-by: hsong-MLE <158601314+hsong-MLE@users.noreply.github.com> --- 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 24b32f527ba..73f774bf735 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 16d6278bad3..5580e64d04e 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); + } +}