From 036b8f48d0b865e7a1f8cb5f24a9c8c8d35a1ba0 Mon Sep 17 00:00:00 2001 From: Takumi Ito Date: Wed, 9 Aug 2023 12:13:25 +0900 Subject: [PATCH 1/2] Append detection wethear the goal position is out of range during A-star planning. Signed-off-by: Takumi Ito --- .../src/astar_search.cpp | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index 1ab5e9d5df487..9ce1cf57bd2f4 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/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) { From 73c12f707cfcedc53df581ef5b6aba85c2b59374 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 9 Aug 2023 06:59:39 +0000 Subject: [PATCH 2/2] style(pre-commit): autofix --- .../freespace_planning_algorithms/src/astar_search.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index 9ce1cf57bd2f4..53085fd0ef825 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/freespace_planning_algorithms/src/astar_search.cpp @@ -260,10 +260,13 @@ bool AstarSearch::search() } 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.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 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;