Float calculations rounding errors causes wrong angular bin

[AzaelCicero]

Bug report

Required Info:

• Operating System:
  • Ubuntu 22.04
• ROS2 Version:
  • ROS2 Humble
• Version or commit hash:
  • 1.1.14 - code in main is the same
• DDS implementation:
  • CycloneDDS

Steps to reproduce issue

TEST(NodeHybridTest, basic_get_closest_angular_bin_test)
{
(...)
  {
    motion_table.bin_size = 0.0872664675;  // 72 bins 
    double test_theta = 6.28318526567925;
    unsigned int expected_angular_bin = 71;
    unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
    EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
  }
}

Expected behavior

With such data getClosestAngularBin returns 71 as the closets bin.

Actual behavior

getClosestAngularBin returns 72 due to float rounding error during division. This issue causes exception in https://github.com/open-navigation/navigation2/blob/36b7728b602197e5466e3fee49726700e7560736/nav2_smac_planner/src/analytic_expansion.cpp#L182, as this calculated value is taken as the index for std::vector (https://github.com/open-navigation/navigation2/blob/f41920ee0912bb34caf746f0c8f4773e66bd0f14/nav2_smac_planner/src/collision_checker.cpp#L141) leading to crash.

Additional information

Performing calculations with double precision alleviates the issue, take the look at variables window on the left:

I would also add modulus operation on the result with num_angle_quantization just to be extra safe.

[SteveMacenski]

Please open a PR against main and I can backport immediately to humble/iron! Thanks!