Theta star planner fix

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) {

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<int>(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<int>(path.poses.size()), 0);
+
   planner_2d->deactivate();
   planner_2d->cleanup();