nav2_smac_planner: handle corner case where start and goal are on the…

… same cell (#4793)

* nav2_smac_planner: handle corner case where start and goal are on the same cell

This case was already properly handled in the smac_planner_2d, but it
was still leading to an A* backtrace failure in the smac_planner_hybrid
and smac_planner_lattice. Let's harmonize the handling of this case.

This commit fixes issue #4792.

Signed-off-by: Dylan De Coeyer <[email protected]>

* nav2_smac_planner: use goal orientation when path is made of one point

Signed-off-by: Dylan De Coeyer <[email protected]>

* nav2_smac_planner: publish raw path also when start and goal are on the same cell

Signed-off-by: Dylan De Coeyer <[email protected]>

* nav2_smac_planner: add corner case to unit tests

Add a plan where the start and goal are placed on the same cell.

Signed-off-by: Dylan De Coeyer <[email protected]>

---------

Signed-off-by: Dylan De Coeyer <[email protected]>

nav2_smac_planner/src/smac_planner_2d.cpp

@@ -258,7 +258,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(
   pose.pose.orientation.z = 0.0;
   pose.pose.orientation.w = 1.0;
 
-  // Corner case of start and goal beeing on the same cell
+  // 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)) {
     pose.pose = start.pose;
     // if we have a different start and goal orientation, set the unique path pose to the goal
@@ -268,6 +268,12 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(
       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;
   }
 

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -359,8 +359,13 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   _a_star->setCollisionChecker(&_collision_checker);
 
   // Set starting point, in A* bin search coordinates
-  float mx, my;
-  if (!costmap->worldToMapContinuous(start.pose.position.x, start.pose.position.y, mx, my)) {
+  float mx_start, my_start, mx_goal, my_goal;
+  if (!costmap->worldToMapContinuous(
+    start.pose.position.x,
+    start.pose.position.y,
+    mx_start,
+    my_start))
+  {
     throw nav2_core::StartOutsideMapBounds(
             "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " +
             std::to_string(start.pose.position.y) + ") was outside bounds");
@@ -374,10 +379,15 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
     orientation_bin -= static_cast<float>(_angle_quantizations);
   }
-  _a_star->setStart(mx, my, static_cast<unsigned int>(orientation_bin));
+  _a_star->setStart(mx_start, my_start, static_cast<unsigned int>(orientation_bin));
 
   // Set goal point, in A* bin search coordinates
-  if (!costmap->worldToMapContinuous(goal.pose.position.x, goal.pose.position.y, mx, my)) {
+  if (!costmap->worldToMapContinuous(
+    goal.pose.position.x,
+    goal.pose.position.y,
+    mx_goal,
+    my_goal))
+  {
     throw nav2_core::GoalOutsideMapBounds(
             "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
             std::to_string(goal.pose.position.y) + ") was outside bounds");
@@ -390,7 +400,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
     orientation_bin -= static_cast<float>(_angle_quantizations);
   }
-  _a_star->setGoal(mx, my, static_cast<unsigned int>(orientation_bin));
+  _a_star->setGoal(mx_goal, my_goal, static_cast<unsigned int>(orientation_bin));
 
   // Setup message
   nav_msgs::msg::Path plan;
@@ -404,6 +414,22 @@ 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))
+  {
+    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

@@ -286,24 +286,34 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
   _a_star->setCollisionChecker(&_collision_checker);
 
   // Set starting point, in A* bin search coordinates
-  float mx, my;
-  if (!_costmap->worldToMapContinuous(start.pose.position.x, start.pose.position.y, mx, my)) {
+  float mx_start, my_start, mx_goal, my_goal;
+  if (!_costmap->worldToMapContinuous(
+    start.pose.position.x,
+    start.pose.position.y,
+    mx_start,
+    my_start))
+  {
     throw nav2_core::StartOutsideMapBounds(
             "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " +
             std::to_string(start.pose.position.y) + ") was outside bounds");
   }
   _a_star->setStart(
-    mx, my,
+    mx_start, my_start,
     NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation)));
 
   // Set goal point, in A* bin search coordinates
-  if (!_costmap->worldToMapContinuous(goal.pose.position.x, goal.pose.position.y, mx, my)) {
+  if (!_costmap->worldToMapContinuous(
+    goal.pose.position.x,
+    goal.pose.position.y,
+    mx_goal,
+    my_goal))
+  {
     throw nav2_core::GoalOutsideMapBounds(
             "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
             std::to_string(goal.pose.position.y) + ") was outside bounds");
   }
   _a_star->setGoal(
-    mx, my,
+    mx_goal, my_goal,
     NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)));
 
   // Setup message
@@ -318,6 +328,22 @@ 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))
+  {
+    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;

nav2_smac_planner/test/test_smac_2d.cpp

@@ -77,6 +77,13 @@ TEST(SmacTest, test_smac_2d) {
   } catch (...) {
   }
 
+  // corner case where the start and goal are on the same cell
+  goal.pose.position.x = 0.01;
+  goal.pose.position.y = 0.01;
+
+  nav_msgs::msg::Path plan = planner_2d->createPlan(start, goal, dummy_cancel_checker);
+  EXPECT_EQ(plan.poses.size(), 1);  // single point path
+
   planner_2d->deactivate();
   planner_2d->cleanup();
 

nav2_smac_planner/test/test_smac_hybrid.cpp

@@ -76,6 +76,13 @@ TEST(SmacTest, test_smac_se2)
   } catch (...) {
   }
 
+  // corner case where the start and goal are on the same cell
+  goal.pose.position.x = 0.01;
+  goal.pose.position.y = 0.01;
+
+  nav_msgs::msg::Path plan = planner->createPlan(start, goal, dummy_cancel_checker);
+  EXPECT_EQ(plan.poses.size(), 1);  // single point path
+
   planner->deactivate();
   planner->cleanup();
 

nav2_smac_planner/test/test_smac_lattice.cpp

@@ -84,6 +84,13 @@ TEST(SmacTest, test_smac_lattice)
   } catch (...) {
   }
 
+  // corner case where the start and goal are on the same cell
+  goal.pose.position.x = 0.01;
+  goal.pose.position.y = 0.01;
+
+  nav_msgs::msg::Path plan = planner->createPlan(start, goal, dummy_cancel_checker);
+  EXPECT_EQ(plan.poses.size(), 1);  // single point path
+
   planner->deactivate();
   planner->cleanup();