Skip to content

Commit

Permalink
Added cast to float to getClosestAngularBin return for behavior consi…
Browse files Browse the repository at this point in the history
…stency (ros-navigation#4123)

* Added cast to float to getClosestAngularBin return for behavior consistency.

Signed-off-by: Hunter Song <hsong@macleanengineering.com>

* Revised test name

Signed-off-by: Hunter Song <hsong@macleanengineering.com>

---------

Signed-off-by: Hunter Song <hsong@macleanengineering.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
  • Loading branch information
hsong-MLE authored and stevedanomodolor committed Mar 25, 2024
1 parent 105ab50 commit d663738
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 1 deletion.
2 changes: 1 addition & 1 deletion nav2_smac_planner/src/node_hybrid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -332,7 +332,7 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node)

unsigned int HybridMotionTable::getClosestAngularBin(const double & theta)
{
return static_cast<unsigned int>(floor(theta / bin_size));
return static_cast<unsigned int>(floor(static_cast<float>(theta) / bin_size));
}

float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx)
Expand Down
30 changes: 30 additions & 0 deletions nav2_smac_planner/test/test_nodehybrid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}

0 comments on commit d663738

Please sign in to comment.