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]>

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();