Skip to content

Commit 47ded06

Browse files
mergify[bot]hsong-MLE
authored andcommitted
Added cast to float to getClosestAngularBin return for behavior consistency (ros-navigation#4123) (ros-navigation#4125)
* Added cast to float to getClosestAngularBin return for behavior consistency. Signed-off-by: Hunter Song <[email protected]> * Revised test name Signed-off-by: Hunter Song <[email protected]> --------- Signed-off-by: Hunter Song <[email protected]> (cherry picked from commit c59e0f3) Co-authored-by: hsong-MLE <[email protected]>
1 parent 2cb1a0b commit 47ded06

File tree

2 files changed

+31
-1
lines changed

2 files changed

+31
-1
lines changed

nav2_smac_planner/src/node_hybrid.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -250,7 +250,7 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node)
250250

251251
unsigned int HybridMotionTable::getClosestAngularBin(const double & theta)
252252
{
253-
return static_cast<unsigned int>(floor(theta / bin_size));
253+
return static_cast<unsigned int>(floor(static_cast<float>(theta) / bin_size));
254254
}
255255

256256
float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx)

nav2_smac_planner/test/test_nodehybrid.cpp

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -304,3 +304,33 @@ TEST(NodeHybridTest, test_node_reeds_neighbors)
304304
// should be empty since totally invalid
305305
EXPECT_EQ(neighbors.size(), 0u);
306306
}
307+
308+
TEST(NodeHybridTest, basic_get_closest_angular_bin_test)
309+
{
310+
// Tests to check getClosestAngularBin behavior for different input types
311+
nav2_smac_planner::HybridMotionTable motion_table;
312+
313+
{
314+
motion_table.bin_size = 3.1415926;
315+
double test_theta = 3.1415926;
316+
unsigned int expected_angular_bin = 1;
317+
unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
318+
EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
319+
}
320+
321+
{
322+
motion_table.bin_size = M_PI;
323+
double test_theta = M_PI;
324+
unsigned int expected_angular_bin = 1;
325+
unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
326+
EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
327+
}
328+
329+
{
330+
motion_table.bin_size = M_PI;
331+
float test_theta = M_PI;
332+
unsigned int expected_angular_bin = 1;
333+
unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
334+
EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
335+
}
336+
}

0 commit comments

Comments
 (0)