From 930f699d3825f71f211f579bdabd7807638afe68 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 10 Jul 2023 17:00:22 +0900 Subject: [PATCH 1/2] fix(start_planner): publish stop path when goal is behind from ego Signed-off-by: Takamasa Horibe --- .../start_planner/start_planner_module.hpp | 6 +++ .../start_planner/start_planner_module.cpp | 52 +++++++++++++++++++ 2 files changed, 58 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 313162d6473cc..205b3c6d4db46 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -149,6 +149,12 @@ class StartPlannerModule : public SceneModuleInterface bool isStopped(); bool hasFinishedCurrentPath(); + // check if the goal is located behind the ego in the same route segment. + bool IsGoalBehindOfEgoInSameRouteSegment() const; + + // generate BehaviorPathOutput with stopping path. + BehaviorModuleOutput generateStopOutput() const; + void setDebugData() const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 2441f62ef6cf8..648c85e3c80ff 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -153,6 +153,12 @@ ModuleStatus StartPlannerModule::updateState() BehaviorModuleOutput StartPlannerModule::plan() { + if (IsGoalBehindOfEgoInSameRouteSegment()) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); + return generateStopOutput(); + } + if (isWaitingApproval()) { clearWaitingApproval(); resetPathCandidate(); @@ -294,6 +300,12 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() updatePullOutStatus(); waitApproval(); + if (IsGoalBehindOfEgoInSameRouteSegment()) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); + return generateStopOutput(); + } + BehaviorModuleOutput output; if (!status_.is_safe) { RCLCPP_WARN_THROTTLE( @@ -832,6 +844,46 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const return turn_signal; } +bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const +{ + const auto & rh = planner_data_->route_handler; + + // Check if the goal and ego are in the same route segment. If not, this is out of scope of this + // function. Return false. + lanelet::ConstLanelet ego_lanelet; + rh->getClosestLaneletWithinRoute(getEgoPose(), &ego_lanelet); + const auto is_ego_in_goal_route_section = rh->isInGoalRouteSection(ego_lanelet); + + if (!is_ego_in_goal_route_section) { + return false; + } + + // If the goal and ego are in the same route segment, check the goal and ego pose relation. + // Return true when the goal is located behind of ego. + const auto ego_lane_path = rh->getCenterLinePath( + lanelet::ConstLanelets{ego_lanelet}, 0.0, std::numeric_limits::max()); + const auto dist_ego_to_goal = motion_utils::calcSignedArcLength( + ego_lane_path.points, getEgoPosition(), rh->getGoalPose().position); + + const bool is_goal_behind_of_ego = (dist_ego_to_goal < 0.0); + return is_goal_behind_of_ego; +} + +// NOTE: this must be called after updatePullOutStatus(). This must be fixed. +BehaviorModuleOutput StartPlannerModule::generateStopOutput() const +{ + BehaviorModuleOutput output; + output.path = std::make_shared(generateStopPath()); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = status_.lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + + output.reference_path = getPreviousModuleOutput().reference_path; + return output; +} + void StartPlannerModule::setDebugData() const { using marker_utils::createPathMarkerArray; From a35f4e26695218d5460bf3f9eaa32265d8da27a3 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 10 Jul 2023 17:00:57 +0900 Subject: [PATCH 2/2] pre-commit Signed-off-by: Takamasa Horibe --- .../src/scene_module/start_planner/start_planner_module.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 648c85e3c80ff..dba81909d21fa 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -156,7 +156,7 @@ BehaviorModuleOutput StartPlannerModule::plan() if (IsGoalBehindOfEgoInSameRouteSegment()) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); - return generateStopOutput(); + return generateStopOutput(); } if (isWaitingApproval()) { @@ -303,7 +303,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() if (IsGoalBehindOfEgoInSameRouteSegment()) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); - return generateStopOutput(); + return generateStopOutput(); } BehaviorModuleOutput output;