getClosestAngularBin() returns unexpected bin number #4094
Description
Bug report
Background information
When using getClosestAngularBin() in a custom planner implementation, I noticed the method sometimes returns unexpected bin numbers which could be a result of the division theta / bin_size where theta is of type double while bin_size is of type float. In diagnosing the issue, I noticed there was no tests, so I wrote tests for the getClosestAngularBin() method to verify its behavior. An example of these tests is given below.
Required Info:
Operating System:
Ubuntu 22.04
ROS2 Version:
humble
Version or commit hash:
ros-humble-navigation2 1.1.12-1jammy.20231005.224358
DDS implementation:
default
Steps to reproduce issue
To reproduce within the nav2_smac_planner package:
- Add the following tests to
test_nodehybrid.cpp:
TEST(NodeHybridTest, basic_get_closest_angular_bin_noPi_test)
{
nav2_smac_planner::HybridMotionTable motion_table;
motion_table.bin_size = 3.1415926;
// This test passes
{
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);
}
}
TEST(NodeHybridTest, get_closest_angular_bin_input_double_test)
{
nav2_smac_planner::HybridMotionTable motion_table;
motion_table.bin_size = M_PI;
// This test fails
{
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);
}
}
TEST(NodeHybridTest, get_closest_angular_bin_input_float_test)
{
nav2_smac_planner::HybridMotionTable motion_table;
motion_table.bin_size = M_PI;
// This test passes
{
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);
}
}- Run
colcon build --packages-up-to nav2_smac_planner
colcon test --packages-select nav2_smac_planner
To reproduce in a standalone implementation:
- Save the following as
my_test_file.cpp
#include <iostream>
#include <math.h>
using namespace std;
unsigned int getBinNoPi(double angle)
{
cout << "getBinNoPi ";
unsigned int num_quantization = 72;
float bin_size = 2.0f * static_cast<float>(3.1416) / static_cast<float>(num_quantization);
return static_cast<unsigned int>(floor(angle / bin_size));
}
unsigned int getBinMixedPi(double angle)
{
cout << "getBinMixedDoublePi ";
unsigned int num_quantization = 72;
float bin_size = 2.0f * static_cast<float>(M_PI) / static_cast<float>(num_quantization);
return static_cast<unsigned int>(floor(angle / bin_size));
}
unsigned int getBinMixedPi(float angle)
{
cout << "getBinMixedFloatPi ";
unsigned int num_quantization = 72;
float bin_size = 2.0f * static_cast<float>(M_PI) / static_cast<float>(num_quantization);
return static_cast<unsigned int>(floor(angle / bin_size));
}
int main()
{
double theta_d = M_PI;
float theta_f = M_PI;
// This test passes
{
auto bin_index = getBinNoPi(3.1416);
cout << "int getBinNoPi(3.1416) " << bin_index << " " << (bin_index == 36) << endl;
}
// This test fails
{
auto bin_index = getBinMixedPi(theta_d);
cout << "int getBinMixedPi(theta_d) " << bin_index << " " << (bin_index == 36) << endl;
}
// This test passes
{
auto bin_index = getBinMixedPi(theta_f);
cout << "int getBinMixedPi(theta_f) " << bin_index << " " << (bin_index == 36) << endl;
}
return 0;
}- Compile with
g++ my_file.cpp -o my_file - Run with
./my_file - You should see this:
Expected behavior
- Test passes and see the following in test results
[ RUN ] NodeHybridTest.basic_get_closest_angular_bin_noPi_test
[ OK ] NodeHybridTest.basic_get_closest_angular_bin_noPi_test (0 ms)
[ RUN ] NodeHybridTest.basic_get_closest_angular_bin_input_double_test
[ OK ] NodeHybridTest.basic_get_closest_angular_bin_input_double_test (0 ms)
[ RUN ] NodeHybridTest.basic_get_closest_angular_bin_input_float_test
[ OK ] NodeHybridTest.basic_get_closest_angular_bin_input_float_test (0 ms)
Actual behavior
- Test fails and see the following in test results
[ RUN ] NodeHybridTest.basic_get_closest_angular_bin_noPi_test
[ OK ] NodeHybridTest.basic_get_closest_angular_bin_noPi_test (0 ms)
[ RUN ] NodeHybridTest.basic_get_closest_angular_bin_input_double_test
/var/lib/build/auto_simulation/auto_navigation2/nav2_smac_planner/test/test_nodehybrid.cpp:1154: Failure
Expected equality of these values:
expected_angular_bin
Which is: 1
calculated_angular_bin
Which is: 0
[ FAILED ] NodeHybridTest.basic_get_closest_angular_bin_input_double_test (0 ms)
[ RUN ] NodeHybridTest.basic_get_closest_angular_bin_input_float_test
[ OK ] NodeHybridTest.basic_get_closest_angular_bin_input_float_test (0 ms)