getClosestAngularBin() returns unexpected bin number

[hsong-MLE]

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)

[SteveMacenski]

Am I correct in saying that simply casting the double back down to a float resolve the issue?

return static_cast<unsigned int>(floor(static_cast<float>(theta) / bin_size));

If so, can you submit a PR, that should be easy enough to merge. I'll also take a look when you submit that PR at other implicit double -> float conversions. I did a quick glance and I see a small number of other places that I should be more explicit about that conversion, but this is the only critical one.

From the 3 places that its used, it looks like double really is the valid input type, so we can squash it in the function itself. Two could be made into floats, but one can't/shouldn't, so its easier to just change it in the method instead.

[hsong-MLE]

Am I correct in saying that simply casting the double back down to a float resolve the issue?

return static_cast<unsigned int>(floor(static_cast<float>(theta) / bin_size));

If so, can you submit a PR, that should be easy enough to merge. I'll also take a look when you submit that PR at other implicit double -> float conversions. I did a quick glance and I see a small number of other places that I should be more explicit about that conversion, but this is the only critical one.

From the 3 places that its used, it looks like double really is the valid input type, so we can squash it in the function itself. Two could be made into floats, but one can't/shouldn't, so its easier to just change it in the method instead.

Correct, casting the double to a float resolves the issue.

I can submit a PR but I wonder which branch should it go towards?

[SteveMacenski]

Put it towards main, but I'll immediately backport to Humble/Iron.

Thanks for the report and soon-to-come PR! 😄

[hsong-MLE]

My pleasure!

I had trouble building main on my machine but humble works flawlessly. Would it be ok if I PR into humble instead?

[SteveMacenski]

All PRs need to be targeted to main and I'll backport to Humble using our tools. If the static cast works in Humble, there's no reason it shouldn't in rolling as well. I looked into this as well and I don't see an issue with that. If your tests pass in Rolling as well (add those unit tests) then we're all good :-)

[SteveMacenski]

Any update? Should be a small change 😄

[hsong-MLE]

Sorry work got a bit busy last week. I should be able to get it done this week. Sorry about the delay!

[SteveMacenski]

Merging imminent, waiting on CI

Thanks @hsong-MLE