From c14f8901104abc5a87560e39100a31fbb79cf008 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Tue, 27 Dec 2022 18:06:18 +0900 Subject: [PATCH] fix(mission_planner): set goal in shoulder lane (#2548) (#223) * fix(mission_planner): set goal in shoulder lane Signed-off-by: kosuke55 * fix map without shoulder lane Signed-off-by: kosuke55 Signed-off-by: kosuke55 Signed-off-by: kosuke55 Co-authored-by: Kosuke Takeuchi --- .../src/lanelet2_plugins/default_planner.cpp | 38 +++++++++---------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index b741bd4479763..55ed5a4cf57b0 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -272,6 +272,25 @@ bool DefaultPlanner::is_goal_valid( const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets) { const auto logger = node_->get_logger(); + + const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position); + + // check if goal is in shoulder lanelet + lanelet::Lanelet closest_shoulder_lanelet; + if (lanelet::utils::query::getClosestLanelet( + shoulder_lanelets_, goal, &closest_shoulder_lanelet)) { + if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) { + const auto lane_yaw = + lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); + const auto goal_yaw = tf2::getYaw(goal.orientation); + const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); + const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); + if (std::abs(angle_diff) < th_angle) { + return true; + } + } + } + lanelet::Lanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal, &closest_lanelet)) { return false; @@ -298,8 +317,6 @@ bool DefaultPlanner::is_goal_valid( return false; } - const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position); - if (is_in_lane(closest_lanelet, goal_lanelet_pt)) { const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal.position); const auto goal_yaw = tf2::getYaw(goal.orientation); @@ -323,23 +340,6 @@ bool DefaultPlanner::is_goal_valid( return true; } - // check if goal is in shoulder lanelet - lanelet::Lanelet closest_shoulder_lanelet; - if (!lanelet::utils::query::getClosestLanelet( - shoulder_lanelets_, goal, &closest_shoulder_lanelet)) { - return false; - } - // check if goal pose is in shoulder lane - if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) { - const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); - const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - - const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); - if (std::abs(angle_diff) < th_angle) { - return true; - } - } return false; }