Update Smac Planner for rounding to closest bin rather than flooring (ros-navigation#4636)

(cherry picked from commit a63090f)

Author: SteveMacenski

nav2_smac_planner/src/node_hybrid.cpp

@@ -251,8 +251,8 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node)
 
 unsigned int HybridMotionTable::getClosestAngularBin(const double & theta)
 {
-  return static_cast<unsigned int>(floor(theta / static_cast<double>(bin_size))) %
-         num_angle_quantization;
+  auto bin = static_cast<unsigned int>(round(static_cast<float>(theta) / bin_size));
+  return bin < num_angle_quantization ? bin : 0u;
 }
 
 float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx)

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -337,33 +337,31 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
             std::to_string(start.pose.position.y) + ") was outside bounds");
   }
 
-  double orientation_bin = tf2::getYaw(start.pose.orientation) / _angle_bin_size;
+  double orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size);
   while (orientation_bin < 0.0) {
     orientation_bin += static_cast<float>(_angle_quantizations);
   }
   // This is needed to handle precision issues
   if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
     orientation_bin -= static_cast<float>(_angle_quantizations);
   }
-  unsigned int orientation_bin_id = static_cast<unsigned int>(floor(orientation_bin));
-  _a_star->setStart(mx, my, orientation_bin_id);
+  _a_star->setStart(mx, my, static_cast<unsigned int>(orientation_bin));
 
   // Set goal point, in A* bin search coordinates
   if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) {
     throw nav2_core::GoalOutsideMapBounds(
             "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
             std::to_string(goal.pose.position.y) + ") was outside bounds");
   }
-  orientation_bin = tf2::getYaw(goal.pose.orientation) / _angle_bin_size;
+  orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size);
   while (orientation_bin < 0.0) {
     orientation_bin += static_cast<float>(_angle_quantizations);
   }
   // This is needed to handle precision issues
   if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
     orientation_bin -= static_cast<float>(_angle_quantizations);
   }
-  orientation_bin_id = static_cast<unsigned int>(floor(orientation_bin));
-  _a_star->setGoal(mx, my, orientation_bin_id);
+  _a_star->setGoal(mx, my, static_cast<unsigned int>(orientation_bin));
 
   // Setup message
   nav_msgs::msg::Path plan;

nav2_smac_planner/test/test_nodehybrid.cpp

@@ -344,7 +344,7 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test)
   {
     motion_table.bin_size = M_PI;
     motion_table.num_angle_quantization = 2;
-    double test_theta = M_PI;
+    double test_theta = M_PI / 2.0 - 0.000001;
     unsigned int expected_angular_bin = 0;
     unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
     EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
@@ -362,8 +362,8 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test)
   {
     motion_table.bin_size = 0.0872664675;
     motion_table.num_angle_quantization = 72;
-    double test_theta = 6.28318526567925;
-    unsigned int expected_angular_bin = 71;
+    double test_theta = 6.28317530718;  // 0.0001 less than 2 pi
+    unsigned int expected_angular_bin = 0;  // should be closer to wrap around
     unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta);
     EXPECT_EQ(expected_angular_bin, calculated_angular_bin);
   }