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

Author: SteveMacenski

nav2_smac_planner/src/node_hybrid.cpp

@@ -250,8 +250,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

@@ -294,31 +294,30 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) {
     throw std::runtime_error("Start pose is out of costmap!");
   }
-  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 std::runtime_error("Goal pose is out of costmap!");
   }
-  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

@@ -323,7 +323,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);
@@ -341,8 +341,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);
   }