From d99bdf77e2ddb8cf874c12f67940fa4fa7d3e184 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Tue, 23 Nov 2021 08:25:29 -0500 Subject: [PATCH 1/2] add test to check if the goal is unsafe with theta star planner Signed-off-by: Daisuke Sato --- nav2_theta_star_planner/test/test_theta_star.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 3deb02a4cc4..7776179963f 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -166,6 +166,17 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { nav_msgs::msg::Path path = planner_2d->createPlan(start, goal); EXPECT_GT(static_cast(path.poses.size()), 0); + // test if the goal is unsafe + for (int i = 7; i <= 14; i++) { + for (int j = 7; j <= 14; j++) { + costmap_ros->getCostmap()->setCost(i, j, 254); + } + } + goal.pose.position.x = 1.0; + goal.pose.position.y = 1.0; + path = planner_2d->createPlan(start, goal); + EXPECT_EQ(static_cast(path.poses.size()), 0); + planner_2d->deactivate(); planner_2d->cleanup(); From 7d51e5da761074fb8c4d23a60f5fe2979b2c9136 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Tue, 23 Nov 2021 08:26:38 -0500 Subject: [PATCH 2/2] check if a plan is generated (#2698) Signed-off-by: Daisuke Sato --- nav2_theta_star_planner/src/theta_star_planner.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 6a46908d105..cc98649ccc2 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -118,14 +118,17 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( logger_, "Got the src and dst... (%i, %i) && (%i, %i)", planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y); getPlan(global_path); - global_path.poses.back().pose.orientation = goal.pose.orientation; + // check if a plan is generated + size_t plan_size = global_path.poses.size(); + if (plan_size > 0) { + global_path.poses.back().pose.orientation = goal.pose.orientation; + } // If use_final_approach_orientation=true, interpolate the last pose orientation from the // previous pose to set the orientation to the 'final approach' orientation of the robot so // it does not rotate. // And deal with corner case of plan of length 1 if (use_final_approach_orientation_) { - size_t plan_size = global_path.poses.size(); if (plan_size == 1) { global_path.poses.back().pose.orientation = start.pose.orientation; } else if (plan_size > 1) {