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,15 @@ 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;
+
+  try {
+    planner_2d->createPlan(start, goal, dummy_cancel_checker);
+  } catch (...) {
+  }
+
   planner_2d->deactivate();
   planner_2d->cleanup();
 

nav2_smac_planner/test/test_smac_hybrid.cpp

@@ -76,6 +76,15 @@ 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;
+
+  try {
+    planner->createPlan(start, goal, dummy_cancel_checker);
+  } catch (...) {
+  }
+
   planner->deactivate();
   planner->cleanup();
 

nav2_smac_planner/test/test_smac_lattice.cpp

@@ -84,6 +84,15 @@ 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;
+
+  try {
+    planner->createPlan(start, goal, dummy_cancel_checker);
+  } catch (...) {
+  }
+
   planner->deactivate();
   planner->cleanup();