From 132d7d8bcd62eedff8a418d6a9c85cfad5e020ba Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Wed, 31 Jan 2024 20:11:06 +0900 Subject: [PATCH] feat(lane_change): wait approval with abort state (#6234) Signed-off-by: Fumiya Watanabe --- .../src/interface.cpp | 13 +++- .../src/scene.cpp | 59 +++++++++++-------- 2 files changed, 44 insertions(+), 28 deletions(-) diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 9c04ff4c67c58..c8d53d8756cf3 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -70,7 +70,7 @@ bool LaneChangeInterface::isExecutionRequested() const bool LaneChangeInterface::isExecutionReady() const { - return module_type_->isSafe(); + return module_type_->isSafe() && !module_type_->isAbortState(); } void LaneChangeInterface::updateData() @@ -116,7 +116,16 @@ BehaviorModuleOutput LaneChangeInterface::plan() } updateSteeringFactorPtr(output); - clearWaitingApproval(); + if (module_type_->isAbortState()) { + waitApproval(); + removeRTCStatus(); + const auto candidate = planCandidate(); + path_candidate_ = std::make_shared(candidate.path_candidate); + updateRTCStatus( + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + } else { + clearWaitingApproval(); + } return output; } diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index c5a0f21d0f909..a5c49c32e0eb6 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -144,6 +144,19 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const { BehaviorModuleOutput output; + if (isAbortState() && abort_path_) { + output.path = abort_path_->path; + output.reference_path = prev_module_reference_path_; + output.turn_signal_info = prev_turn_signal_info_; + extendOutputDrivableArea(output); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); + output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + return output; + } + const auto current_lanes = getCurrentLanes(); if (current_lanes.empty()) { RCLCPP_WARN(logger_, "Current lanes not found!!! Something wrong."); @@ -157,7 +170,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const const auto terminal_path = calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); if (!terminal_path) { - RCLCPP_WARN(logger_, "Terminal path not found!!!"); + RCLCPP_DEBUG(logger_, "Terminal path not found!!!"); output.path = prev_module_path_; output.reference_path = prev_module_reference_path_; output.drivable_area_info = prev_drivable_area_info_; @@ -1525,7 +1538,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); if (!lane_changing_start_pose) { - RCLCPP_WARN(logger_, "Reject: lane changing start pose not found!!!"); + RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); return {}; } @@ -1534,7 +1547,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( // Check if the lane changing start point is not on the lanes next to target lanes, if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_WARN(logger_, "lane change start getEgoPose() is behind target lanelet!"); + RCLCPP_DEBUG(logger_, "lane change start getEgoPose() is behind target lanelet!"); return {}; } @@ -1552,7 +1565,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( minimum_lane_changing_velocity, next_lane_change_buffer); if (target_segment.points.empty()) { - RCLCPP_WARN(logger_, "Reject: target segment is empty!! something wrong..."); + RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); return {}; } @@ -1581,7 +1594,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; if (!is_valid_start_point) { - RCLCPP_WARN( + RCLCPP_DEBUG( logger_, "Reject: lane changing points are not inside of the target preferred lanes or its " "neighbors"); @@ -1596,7 +1609,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( next_lane_change_buffer); if (target_lane_reference_path.points.empty()) { - RCLCPP_WARN(logger_, "Reject: target_lane_reference_path is empty!!"); + RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); return {}; } @@ -1855,31 +1868,25 @@ bool NormalLaneChange::calcAbortPath() ShiftedPath shifted_path; // offset front side - // bool offset_back = false; if (!path_shifter.generate(&shifted_path)) { RCLCPP_ERROR(logger_, "failed to generate abort shifted path."); } - const auto arc_position = lanelet::utils::getArcCoordinates( - reference_lanelets, shifted_path.path.points.at(abort_return_idx).point.pose); - const PathWithLaneId reference_lane_segment = std::invoke([&]() { - const double s_start = arc_position.length; - double s_end = std::max(lanelet::utils::getLaneletLength2d(reference_lanelets), s_start); - - if ( - !reference_lanelets.empty() && - route_handler->isInGoalRouteSection(reference_lanelets.back())) { - const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(reference_lanelets, route_handler->getGoalPose()); - const double forward_length = std::max(goal_arc_coordinates.length, s_start); - s_end = std::min(s_end, forward_length); + PathWithLaneId reference_lane_segment = prev_module_path_; + { + const auto terminal_path = + calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes); + if (terminal_path) { + reference_lane_segment = terminal_path->path; } - PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end, true); - ref.points.back().point.longitudinal_velocity_mps = std::min( - ref.points.back().point.longitudinal_velocity_mps, - static_cast(lane_change_parameters_->minimum_lane_changing_velocity)); - return ref; - }); + const auto return_pose = shifted_path.path.points.at(abort_return_idx).point.pose; + const auto seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + reference_lane_segment.points, return_pose, common_param.ego_nearest_dist_threshold, + common_param.ego_nearest_yaw_threshold); + reference_lane_segment.points = motion_utils::cropPoints( + reference_lane_segment.points, return_pose.position, seg_idx, + common_param.forward_path_length, 0.0); + } auto abort_path = selected_path; abort_path.shifted_path = shifted_path;