Append detection wethear the goal position is out of range during A-star planning.

Signed-off-by: Takumi Ito <takumi.ito@tier4.jp>

Author: Takumi Ito

planning/freespace_planning_algorithms/src/astar_search.cpp

@@ -259,6 +259,27 @@ bool AstarSearch::search()
         continue;
       }
 
+      geometry_msgs::msg::Pose relative_goal_pose;
+      relative_goal_pose.position.x = goal_pose_.position.x - (next_pose.position.x - start_pose_.position.x);
+      relative_goal_pose.position.y = goal_pose_.position.y - (next_pose.position.y - start_pose_.position.y);
+      relative_goal_pose.orientation = goal_pose_.orientation;
+      const auto relative_goal_index = pose2index(costmap_, relative_goal_pose, planner_common_param_.theta_size);
+
+      const auto & vertex_indexes_2d = vertex_indexes_table_[relative_goal_index.theta];
+      bool relative_goal_is_out_of_range = false;
+      for (const auto & vertex_index_2d : vertex_indexes_2d) {
+        IndexXYT vertex_index{vertex_index_2d.x, vertex_index_2d.y, 0};
+        // must slide to current base position
+        vertex_index.x += relative_goal_index.x;
+        vertex_index.y += relative_goal_index.y;
+        if (isOutOfRange(vertex_index)) {
+          relative_goal_is_out_of_range = true;
+        }
+      }
+      if (relative_goal_is_out_of_range) {
+        continue;
+      }
+
       // Compare cost
       AstarNode * next_node = getNodeRef(next_index);
       if (next_node->status == NodeStatus::None) {