From a11ecf779c8a708c34e89c5163fbd9ce2f2fbf7b Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 6 Jan 2023 18:25:59 +0900 Subject: [PATCH] feat(behavior_path_planner): apply abort lane change Signed-off-by: Fumiya Watanabe --- .../external_request_lane_change_module.hpp | 19 +- .../scene_module/lane_change/util.hpp | 3 +- .../external_request_lane_change_module.cpp | 248 +++++++++++------- .../src/scene_module/lane_change/util.cpp | 23 +- .../pull_over/pull_over_module.cpp | 8 +- .../src/rtc_auto_mode_manager_interface.cpp | 4 + 6 files changed, 192 insertions(+), 113 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp index 72b78a574857c..71408c6c7620e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/external_request_lane_change_module.hpp @@ -59,8 +59,6 @@ class ExternalRequestLaneChangeModule : public SceneModuleInterface const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const Direction & direction); - BehaviorModuleOutput run() override; - bool isExecutionRequested() const override; bool isExecutionReady() const override; BT::NodeStatus updateState() override; @@ -79,20 +77,29 @@ class ExternalRequestLaneChangeModule : public SceneModuleInterface LaneChangeStatus status_; PathShifter path_shifter_; mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; + LaneChangeStates current_lane_change_state_; + std::shared_ptr abort_path_; + PathWithLaneId prev_approved_path_; Direction direction_; double lane_change_lane_length_{200.0}; double check_distance_{100.0}; - + bool is_abort_path_approved_{false}; + bool is_abort_approval_requested_{false}; + bool is_abort_condition_satisfied_{false}; bool is_activated_ = false; + void resetParameters(); + void resetPathIfAbort(); + void waitApproval(const double start_distance, const double finish_distance) { updateRTCStatus(start_distance, finish_distance); is_waiting_approval_ = true; } + lanelet::ConstLanelets get_original_lanes() const; PathWithLaneId getReferencePath() const; lanelet::ConstLanelets getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const; @@ -106,13 +113,15 @@ class ExternalRequestLaneChangeModule : public SceneModuleInterface void updateSteeringFactorPtr(const BehaviorModuleOutput & output); void updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const; + bool isSafe() const; bool isValidPath(const PathWithLaneId & path) const; + bool isApprovedPathSafe(Pose & ego_pose_before_collision) const; bool isNearEndOfLane() const; bool isCurrentSpeedLow() const; - bool isAbortConditionSatisfied() const; + bool isAbortConditionSatisfied(); bool hasFinishedLaneChange() const; - void resetParameters(); + bool isAbortState() const; // getter Pose getEgoPose() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index 70817cfe5e040..0c25567c48671 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -113,8 +113,7 @@ PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double prepare_distance, const double lane_changing_distance, const double forward_path_length, - const double lane_changing_speed, const int num_to_preferred_lane, - const double minimum_lane_change_length); + const double lane_changing_speed, const double lane_change_buffer); PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp index 80cefb0a0220b..8e4e0021cb751 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request_lane_change_module.cpp @@ -54,26 +54,11 @@ ExternalRequestLaneChangeModule::ExternalRequestLaneChangeModule( std::make_unique(&node, getTopicName(direction)); } -BehaviorModuleOutput ExternalRequestLaneChangeModule::run() -{ - RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); - current_state_ = BT::NodeStatus::RUNNING; - auto output = plan(); - - if (!isSafe()) { - current_state_ = BT::NodeStatus::SUCCESS; // for breaking loop - return output; - } - - updateSteeringFactorPtr(output); - - return output; -} - void ExternalRequestLaneChangeModule::onEntry() { RCLCPP_DEBUG(getLogger(), "LANE_CHANGE onEntry"); current_state_ = BT::NodeStatus::SUCCESS; + current_lane_change_state_ = LaneChangeStates::Normal; updateLaneChangeStatus(); } @@ -91,8 +76,8 @@ bool ExternalRequestLaneChangeModule::isExecutionRequested() const } const auto current_lanes = util::getCurrentLanes(planner_data_); - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); + LaneChangePath selected_path; const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); @@ -107,8 +92,8 @@ bool ExternalRequestLaneChangeModule::isExecutionReady() const } const auto current_lanes = util::getCurrentLanes(planner_data_); - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); + LaneChangePath selected_path; const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); @@ -124,12 +109,19 @@ BT::NodeStatus ExternalRequestLaneChangeModule::updateState() return current_state_; } + const auto is_within_current_lane = lane_change_utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters); + if (isAbortState() && !is_within_current_lane) { + current_state_ = BT::NodeStatus::RUNNING; + return current_state_; + } + if (isAbortConditionSatisfied()) { if (isNearEndOfLane() && isCurrentSpeedLow()) { current_state_ = BT::NodeStatus::RUNNING; return current_state_; } - // cancel lane change path + current_state_ = BT::NodeStatus::FAILURE; return current_state_; } @@ -145,6 +137,7 @@ BT::NodeStatus ExternalRequestLaneChangeModule::updateState() BehaviorModuleOutput ExternalRequestLaneChangeModule::plan() { resetPathCandidate(); + is_activated_ = isActivated(); auto path = status_.lane_change_path.path; @@ -152,21 +145,60 @@ BehaviorModuleOutput ExternalRequestLaneChangeModule::plan() status_.is_safe = false; return BehaviorModuleOutput{}; } - generateExtendedDrivableArea(path); - if (isAbortConditionSatisfied()) { - if (isNearEndOfLane() && isCurrentSpeedLow()) { - const auto stop_point = util::insertStopPoint(0.1, &path); + if ((is_abort_condition_satisfied_ && isNearEndOfLane() && isCurrentSpeedLow())) { + const auto stop_point = util::insertStopPoint(0.1, &path); + } + + if (isAbortState()) { + resetPathIfAbort(); + if (is_activated_) { + path = abort_path_->path; } } + generateExtendedDrivableArea(path); + + prev_approved_path_ = path; + BehaviorModuleOutput output; output.path = std::make_shared(path); updateOutputTurnSignal(output); + updateSteeringFactorPtr(output); + return output; } +void ExternalRequestLaneChangeModule::resetPathIfAbort() +{ + if (!abort_path_) { + RCLCPP_WARN(getLogger(), "[abort] No abort path found!"); + } + + if (!is_abort_approval_requested_) { + RCLCPP_DEBUG(getLogger(), "[abort] uuid is reset to request abort approval."); + uuid_ = generateUUID(); + is_abort_approval_requested_ = true; + is_abort_path_approved_ = false; + return; + } + + if (isActivated()) { + RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is true. set is_abort_path_approved to true."); + is_abort_path_approved_ = true; + clearWaitingApproval(); + } else { + RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is False."); + is_abort_path_approved_ = false; + const double start_distance = motion_utils::calcSignedArcLength( + abort_path_->path.points, getEgoPose().position, abort_path_->shift_line.start.position); + const double finish_distance = motion_utils::calcSignedArcLength( + abort_path_->path.points, getEgoPose().position, abort_path_->shift_line.end.position); + waitApproval(start_distance, finish_distance); + } +} + CandidateOutput ExternalRequestLaneChangeModule::planCandidate() const { CandidateOutput output; @@ -180,16 +212,16 @@ CandidateOutput ExternalRequestLaneChangeModule::planCandidate() const getSafePath(lane_change_lanes, check_distance_, selected_path); selected_path.path.header = planner_data_->route_handler->getRouteHeader(); + if (isAbortState()) { + selected_path = *abort_path_; + } + if (selected_path.path.points.empty()) { return output; } - const auto & start_idx = selected_path.shift_line.start_idx; - const auto & end_idx = selected_path.shift_line.end_idx; - output.path_candidate = selected_path.path; - output.lateral_shift = selected_path.shifted_path.shift_length.at(end_idx) - - selected_path.shifted_path.shift_length.at(start_idx); + output.lateral_shift = lane_change_utils::getLateralShift(selected_path); output.start_distance_to_path_change = motion_utils::calcSignedArcLength( selected_path.path.points, getEgoPose().position, selected_path.shift_line.start.position); output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( @@ -201,11 +233,20 @@ CandidateOutput ExternalRequestLaneChangeModule::planCandidate() const BehaviorModuleOutput ExternalRequestLaneChangeModule::planWaitingApproval() { + const auto is_within_current_lane = lane_change_utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters); + + if (is_within_current_lane) { + prev_approved_path_ = getReferencePath(); + } + BehaviorModuleOutput out; out.path = std::make_shared(getReferencePath()); + const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); waitApproval(candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + is_abort_path_approved_ = false; return out; } @@ -265,20 +306,19 @@ PathWithLaneId ExternalRequestLaneChangeModule::getReferencePath() const *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, common_parameters.forward_path_length, common_parameters, optional_lengths); } - const double & buffer = - common_parameters.backward_length_buffer_for_end_of_lane; // buffer for min_lane_change_length + const double lane_change_buffer = - num_lane_change * (common_parameters.minimum_lane_change_length + buffer) + optional_lengths; + util::calcLaneChangeBuffer(common_parameters, num_lane_change, optional_lengths); reference_path = util::setDecelerationVelocity( *route_handler, reference_path, current_lanes, parameters_->lane_change_prepare_duration, lane_change_buffer); const auto drivable_lanes = util::generateDrivableLanes(current_lanes); + const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); const auto expanded_lanes = util::expandLanelets( drivable_lanes, parameters_->drivable_area_left_bound_offset, parameters_->drivable_area_right_bound_offset); - util::generateDrivableArea( reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); @@ -440,18 +480,12 @@ bool ExternalRequestLaneChangeModule::isCurrentSpeedLow() const return util::l2Norm(getEgoTwist().linear) < threshold_ms; } -bool ExternalRequestLaneChangeModule::isAbortConditionSatisfied() const +bool ExternalRequestLaneChangeModule::isAbortConditionSatisfied() { - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - const auto & dynamic_objects = planner_data_->dynamic_object; - const auto & common_parameters = planner_data_->parameters; - - const auto & current_lanes = status_.current_lanes; + is_abort_condition_satisfied_ = false; + current_lane_change_state_ = LaneChangeStates::Normal; - // check abort enable flag - if (!parameters_->enable_abort_lane_change) { + if (!parameters_->enable_cancel_lane_change) { return false; } @@ -459,70 +493,70 @@ bool ExternalRequestLaneChangeModule::isAbortConditionSatisfied() const return false; } - // find closest lanelet in original lane - lanelet::ConstLanelet closest_lanelet{}; - auto clock{rclcpp::Clock{RCL_ROS_TIME}}; - if (!lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &closest_lanelet)) { - RCLCPP_ERROR_THROTTLE( - getLogger(), clock, 1000, - "Failed to find closest lane! Lane change aborting function is not working!"); - return false; - } - - // check if lane change path is still safe - const bool is_path_safe = std::invoke([this, &route_handler, &dynamic_objects, ¤t_lanes, - ¤t_pose, ¤t_twist, &common_parameters]() { - constexpr double check_distance = 100.0; - // get lanes used for detection - const auto & path = status_.lane_change_path; - const double check_distance_with_path = - check_distance + path.preparation_length + path.lane_change_length; - const auto check_lanes = route_handler->getCheckTargetLanesFromPath( - path.path, status_.lane_change_lanes, check_distance_with_path); - - std::unordered_map debug_data; - - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, - common_parameters.ego_nearest_yaw_threshold); - return lane_change_utils::isLaneChangePathSafe( - path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, - current_twist, common_parameters, *parameters_, - common_parameters.expected_front_deceleration_for_abort, - common_parameters.expected_rear_deceleration_for_abort, debug_data, false, - status_.lane_change_path.acceleration); - }); + Pose ego_pose_before_collision; + const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); - // abort only if velocity is low or vehicle pose is close enough if (!is_path_safe) { - // check vehicle velocity thresh - const bool is_velocity_low = - util::l2Norm(current_twist.linear) < parameters_->abort_lane_change_velocity_thresh; - const bool is_within_original_lane = - lane_change_utils::isEgoWithinOriginalLane(current_lanes, current_pose, common_parameters); - if (is_velocity_low && is_within_original_lane) { + const auto & common_parameters = planner_data_->parameters; + const bool is_within_original_lane = lane_change_utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), common_parameters); + + if (is_within_original_lane) { + current_lane_change_state_ = LaneChangeStates::Cancel; return true; } - const bool is_distance_small = - lane_change_utils::isEgoDistanceNearToCenterline(closest_lanelet, current_pose, *parameters_); + // check abort enable flag + RCLCPP_WARN_STREAM_THROTTLE( + getLogger(), *clock_, 1000, + "DANGER!!! Path is not safe anymore, but it is too late to CANCEL! Please be cautious"); + + if (!parameters_->enable_abort_lane_change) { + current_lane_change_state_ = LaneChangeStates::Stop; + return false; + } - // check angle thresh from original lane - const bool is_angle_diff_small = lane_change_utils::isEgoHeadingAngleLessThanThreshold( - closest_lanelet, current_pose, *parameters_); + const auto found_abort_path = lane_change_utils::getAbortPaths( + planner_data_, status_.lane_change_path, ego_pose_before_collision, common_parameters, + *parameters_); - if (is_distance_small && is_angle_diff_small) { + if (!found_abort_path && !is_abort_path_approved_) { + current_lane_change_state_ = LaneChangeStates::Stop; return true; } - auto clock{rclcpp::Clock{RCL_ROS_TIME}}; - RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), clock, 1000, - "DANGER!!! Path is not safe anymore, but it is too late to abort! Please be cautious"); + + current_lane_change_state_ = LaneChangeStates::Abort; + + if (!is_abort_path_approved_) { + abort_path_ = std::make_shared(*found_abort_path); + } + + return true; } return false; } +bool ExternalRequestLaneChangeModule::isAbortState() const +{ + if (!parameters_->enable_abort_lane_change) { + return false; + } + + if (current_lane_change_state_ != LaneChangeStates::Abort) { + return false; + } + + if (!abort_path_) { + return false; + } + + RCLCPP_WARN_STREAM_THROTTLE( + getLogger(), *clock_, 1000, + "DANGER!!! Lane change transition to ABORT state, return path will be computed!"); + return true; +} + bool ExternalRequestLaneChangeModule::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); @@ -638,6 +672,36 @@ void ExternalRequestLaneChangeModule::generateExtendedDrivableArea(PathWithLaneI util::generateDrivableArea(path, expanded_lanes, common_parameters.vehicle_length, planner_data_); } +bool ExternalRequestLaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) const +{ + const auto current_pose = getEgoPose(); + const auto current_twist = getEgoTwist(); + const auto & dynamic_objects = planner_data_->dynamic_object; + const auto & current_lanes = status_.current_lanes; + const auto & common_parameters = planner_data_->parameters; + const auto & route_handler = planner_data_->route_handler; + const auto path = status_.lane_change_path; + + constexpr double check_distance = 100.0; + // get lanes used for detection + const double check_distance_with_path = + check_distance + path.preparation_length + path.lane_change_length; + const auto check_lanes = route_handler->getCheckTargetLanesFromPath( + path.path, status_.lane_change_lanes, check_distance_with_path); + + std::unordered_map debug_data; + + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.path.points, current_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); + return lane_change_utils::isLaneChangePathSafe( + path.path, current_lanes, check_lanes, dynamic_objects, current_pose, current_seg_idx, + current_twist, common_parameters, *parameters_, + common_parameters.expected_front_deceleration_for_abort, + common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, + false, status_.lane_change_path.acceleration); +} + void ExternalRequestLaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output) { const auto turn_signal_info = util::getPathTurnSignal( diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index e10106b074094..7ddeba036f63c 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -201,7 +201,6 @@ LaneChangePaths getLaneChangePaths( const auto & minimum_lane_change_velocity = parameter.minimum_lane_change_velocity; const auto & maximum_deceleration = parameter.maximum_deceleration; const auto & lane_change_sampling_num = parameter.lane_change_sampling_num; - const auto & backward_length_buffer = common_parameter.backward_length_buffer_for_end_of_lane; const int num_to_preferred_lane = std::abs(route_handler.getNumLaneToPreferredLane(target_lanelets.back())); @@ -271,10 +270,15 @@ LaneChangePaths getLaneChangePaths( const Pose & lane_changing_start_pose = prepare_segment_reference.points.back().point.pose; + double optional_lengths{0.0}; + util::checkLaneIsInIntersection( + route_handler, prepare_segment_reference, original_lanelets, common_parameter, + num_to_preferred_lane, optional_lengths); + const PathWithLaneId target_lane_reference_path = getReferencePathFromTargetLane( route_handler, target_lanelets, lane_changing_start_pose, prepare_distance, - lane_changing_distance, forward_path_length, lane_changing_speed, num_to_preferred_lane, - minimum_lane_change_length + backward_length_buffer); + lane_changing_distance, forward_path_length, lane_changing_speed, + util::calcLaneChangeBuffer(common_parameter, num_to_preferred_lane, optional_lengths)); const ShiftLine shift_line = getLaneChangeShiftLine( prepare_segment_reference, lane_changing_segment_reference, target_lanelets, @@ -541,28 +545,25 @@ PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double prepare_distance, const double lane_changing_distance, const double forward_path_length, - const double lane_changing_speed, const int num_to_preferred_lane, - const double minimum_lane_change_length) + const double lane_changing_speed, const double lane_change_buffer) { const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); - const double total_minimum_lane_change_length = - static_cast(num_to_preferred_lane) * minimum_lane_change_length; const double & s_start = lane_change_start_arc_position.length; double s_end = s_start; - if (num_to_preferred_lane == 0) { + if (std::fabs(lane_change_buffer) < 1.0E-03) { s_end += prepare_distance + lane_changing_distance + forward_path_length; } else { - s_end += util::getDistanceToEndOfLane(lane_changing_start_pose, target_lanes) - - total_minimum_lane_change_length; + s_end += + util::getDistanceToEndOfLane(lane_changing_start_pose, target_lanes) - lane_change_buffer; } if (route_handler.isInGoalRouteSection(target_lanes.back())) { const auto goal_arc_coordinates = lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); - s_end = std::min(s_end, goal_arc_coordinates.length - total_minimum_lane_change_length); + s_end = std::min(s_end, goal_arc_coordinates.length - lane_change_buffer); } const auto lane_changing_reference_path = route_handler.getCenterLinePath(target_lanes, s_start, s_end); diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index daf51f8a114d6..a25a023419590 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -499,7 +499,7 @@ BehaviorModuleOutput PullOverModule::plan() // safe: use pull over path status_.stop_pose.reset(); output.path = std::make_shared(getCurrentPath()); - path_candidate_ = std::make_shared(getFullPath()); + path_candidate_ = std::make_shared(status_.pull_over_path.getFullPath()); } else { // not safe: use stop_path if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { @@ -573,8 +573,10 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() updateOccupancyGrid(); BehaviorModuleOutput out; plan(); // update status_ - out.path = std::make_shared(getReferencePath()); - path_candidate_ = status_.is_safe ? std::make_shared(getFullPath()) : out.path; + out.path = std::make_shared(generateStopPath()); + path_candidate_ = status_.is_safe + ? std::make_shared(status_.pull_over_path.getFullPath()) + : out.path; const auto distance_to_path_change = calcDistanceToPathChange(); updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); diff --git a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp index 9ac306534e62e..a63b74e91aae3 100644 --- a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp +++ b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp @@ -35,6 +35,10 @@ Module getModuleType(const std::string & module_name) module.type = Module::TRAFFIC_LIGHT; } else if (module_name == "virtual_traffic_light") { module.type = Module::TRAFFIC_LIGHT; + } else if (module_name == "ext_request_lane_change_left") { + module.type = Module::EXT_REQUEST_LANE_CHANGE_LEFT; + } else if (module_name == "ext_request_lane_change_right") { + module.type = Module::EXT_REQUEST_LANE_CHANGE_RIGHT; } else if (module_name == "lane_change_left") { module.type = Module::LANE_CHANGE_LEFT; } else if (module_name == "lane_change_right") {