Backporting duplicate pose handling in Humble for Smac Planners (#5713)

* Backporting duplicate pose handling in Humble for Smac Planners

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

* linting

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

---------

Signed-off-by: SteveMacenski <stevenmacenski@gmail.com>

Author: SteveMacenski

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -290,34 +290,38 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   _a_star->setCollisionChecker(&_collision_checker);
 
   // Set starting point, in A* bin search coordinates
-  unsigned int mx, my;
-  if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) {
+  unsigned int mx_start, my_start, mx_goal, my_goal;
+  if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start)) {
     throw std::runtime_error("Start pose is out of costmap!");
   }
 
-  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);
+  double orientation_bin_start = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size);
+  while (orientation_bin_start < 0.0) {
+    orientation_bin_start += 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);
+  if (orientation_bin_start >= static_cast<float>(_angle_quantizations)) {
+    orientation_bin_start -= static_cast<float>(_angle_quantizations);
   }
-  _a_star->setStart(mx, my, static_cast<unsigned int>(orientation_bin));
+  unsigned int start_orientation_bin_int =
+    static_cast<unsigned int>(orientation_bin_start);
+  _a_star->setStart(mx_start, my_start, start_orientation_bin_int);
 
   // Set goal point, in A* bin search coordinates
-  if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) {
+  if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) {
     throw std::runtime_error("Goal pose is out of costmap!");
   }
-  orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size);
-  while (orientation_bin < 0.0) {
-    orientation_bin += static_cast<float>(_angle_quantizations);
+  double orientation_bin_goal = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size);
+  while (orientation_bin_goal < 0.0) {
+    orientation_bin_goal += 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);
+  if (orientation_bin_goal >= static_cast<float>(_angle_quantizations)) {
+    orientation_bin_goal -= static_cast<float>(_angle_quantizations);
   }
-  _a_star->setGoal(mx, my, static_cast<unsigned int>(orientation_bin));
+  unsigned int goal_orientation_bin_int =
+    static_cast<unsigned int>(orientation_bin_goal);
+  _a_star->setGoal(mx_goal, my_goal, goal_orientation_bin_int);
 
   // Setup message
   nav_msgs::msg::Path plan;
@@ -331,6 +335,23 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   pose.pose.orientation.z = 0.0;
   pose.pose.orientation.w = 1.0;
 
+  // Corner case of start and goal being on the same cell
+  if (std::floor(mx_start) == std::floor(mx_goal) &&
+    std::floor(my_start) == std::floor(my_goal) &&
+    start_orientation_bin_int == goal_orientation_bin_int)
+  {
+    pose.pose = start.pose;
+    pose.pose.orientation = goal.pose.orientation;
+    plan.poses.push_back(pose);
+
+    // Publish raw path for debug
+    if (_raw_plan_publisher->get_subscription_count() > 0) {
+      _raw_plan_publisher->publish(plan);
+    }
+
+    return plan;
+  }
+
   // Compute plan
   NodeHybrid::CoordinateVector path;
   int num_iterations = 0;

nav2_smac_planner/src/smac_planner_lattice.cpp

@@ -250,17 +250,19 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
   _a_star->setCollisionChecker(&_collision_checker);
 
   // Set starting point, in A* bin search coordinates
-  unsigned int mx, my;
-  _costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my);
+  unsigned int mx_start, my_start, mx_goal, my_goal;
+  _costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start);
+  unsigned int start_bin =
+    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation));
   _a_star->setStart(
-    mx, my,
-    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation)));
+    mx_start, my_start, start_bin);
 
   // Set goal point, in A* bin search coordinates
-  _costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my);
+  _costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal);
+  unsigned int goal_bin =
+    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation));
   _a_star->setGoal(
-    mx, my,
-    NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)));
+    mx_goal, my_goal, goal_bin);
 
   // Setup message
   nav_msgs::msg::Path plan;
@@ -274,6 +276,23 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
   pose.pose.orientation.z = 0.0;
   pose.pose.orientation.w = 1.0;
 
+  // Corner case of start and goal being on the same cell
+  if (std::floor(mx_start) == std::floor(mx_goal) &&
+    std::floor(my_start) == std::floor(my_goal) &&
+    start_bin == goal_bin)
+  {
+    pose.pose = start.pose;
+    pose.pose.orientation = goal.pose.orientation;
+    plan.poses.push_back(pose);
+
+    // Publish raw path for debug
+    if (_raw_plan_publisher->get_subscription_count() > 0) {
+      _raw_plan_publisher->publish(plan);
+    }
+
+    return plan;
+  }
+
   // Compute plan
   NodeLattice::CoordinateVector path;
   int num_iterations = 0;