diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index e2dff5a56f4..89ad666b789 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/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(); diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index 9952912596b..a70ae86fc7d 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/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(); diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index dcc925e9e72..c187abd8591 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/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();