diff --git a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml index 9518185d30d63..3e8907cdccdf6 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml +++ b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml @@ -85,3 +85,11 @@ bicycle: false # [-] motorcycle: false # [-] pedestrian: false # [-] + + constraints: + # lateral constraints + lateral: + velocity: [1.0, 1.38, 11.1] # [m/s] + max_accel_values: [0.5, 0.5, 0.5] # [m/ss] + min_jerk_values: [0.2, 0.2, 0.2] # [m/sss] + max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp index e177244930da6..8e7d1f67d3157 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp @@ -16,8 +16,6 @@ #include "behavior_path_avoidance_module/data_structs.hpp" -#include - namespace behavior_path_planner { struct AvoidanceByLCParameters : public AvoidanceParameters @@ -27,6 +25,10 @@ struct AvoidanceByLCParameters : public AvoidanceParameters // execute only when lane change end point is before the object. bool execute_only_when_lane_change_finish_before_object{false}; + + explicit AvoidanceByLCParameters(const AvoidanceParameters & param) : AvoidanceParameters(param) + { + } }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp index 016a6b2977701..897956a392008 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp @@ -40,6 +40,8 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface bool isExecutionRequested() const override; + void processOnEntry() override; + protected: void updateRTCStatus(const double start_distance, const double finish_distance) override; }; diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp index f22291c99b8e7..7a82ba60a1eaa 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ #include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" +#include "behavior_path_avoidance_module/helper.hpp" #include "behavior_path_lane_change_module/scene.hpp" #include @@ -23,6 +24,7 @@ namespace behavior_path_planner { using AvoidanceDebugData = DebugData; +using helper::avoidance::AvoidanceHelper; class AvoidanceByLaneChange : public NormalLaneChange { @@ -46,6 +48,7 @@ class AvoidanceByLaneChange : public NormalLaneChange ObjectDataArray registered_objects_; mutable ObjectDataArray stopped_objects_; + std::shared_ptr avoidance_helper_; ObjectData createObjectData( const AvoidancePlanningData & data, const PredictedObject & object) const; diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp index 2e451461abab7..efdb302a58ac2 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -45,7 +45,11 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( bool AvoidanceByLaneChangeInterface::isExecutionRequested() const { - return module_type_->specialRequiredCheck() && module_type_->isLaneChangeRequired(); + return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck(); +} +void AvoidanceByLaneChangeInterface::processOnEntry() +{ + waitApproval(); } void AvoidanceByLaneChangeInterface::updateRTCStatus( diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 61b0a36ae0fb0..3f810710ef37b 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -14,6 +14,8 @@ #include "behavior_path_avoidance_by_lane_change_module/manager.hpp" +#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" +#include "behavior_path_avoidance_module/parameter_helper.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -37,7 +39,9 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) // init lane change manager LaneChangeModuleManager::init(node); - AvoidanceByLCParameters p{}; + const auto avoidance_params = getParameter(node); + AvoidanceByLCParameters p(avoidance_params); + // unique parameters { const std::string ns = "avoidance_by_lane_change."; @@ -139,6 +143,18 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); } + // avoidance maneuver (longitudinal) + { + const std::string ns = "avoidance.avoidance.longitudinal."; + p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); + p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); + p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); + p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); + p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); + p.nominal_avoidance_speed = + getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); + } + { const std::string ns = "avoidance.target_filtering.detection_area."; p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 0299e839996b0..619cbc515f816 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -20,6 +20,8 @@ #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include + #include #include @@ -41,7 +43,8 @@ AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, std::shared_ptr avoidance_parameters) : NormalLaneChange(parameters, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE, Direction::NONE), - avoidance_parameters_(std::move(avoidance_parameters)) + avoidance_parameters_(std::move(avoidance_parameters)), + avoidance_helper_{std::make_shared(avoidance_parameters_)} { } @@ -49,48 +52,66 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const { const auto & data = avoidance_data_; - if (!status_.is_safe) { + if (data.target_objects.empty()) { return false; } const auto & object_parameters = avoidance_parameters_->object_parameters; - const auto is_more_than_threshold = - std::any_of(object_parameters.begin(), object_parameters.end(), [&](const auto & p) { - const auto & objects = avoidance_data_.target_objects; - - const size_t num = std::count_if(objects.begin(), objects.end(), [&p](const auto & object) { - const auto target_class = - utils::getHighestProbLabel(object.object.classification) == p.first; - return target_class && object.avoid_required; - }); - return num >= p.second.execute_num; - }); + const auto count_target_object = [&](const auto sum, const auto & p) { + const auto & objects = avoidance_data_.target_objects; - if (!is_more_than_threshold) { - return false; - } + const auto is_avoidance_target = [&p](const auto & object) { + const auto target_class = utils::getHighestProbLabel(object.object.classification) == p.first; + return target_class && object.avoid_required; + }; - const auto & front_object = data.target_objects.front(); + return sum + std::count_if(objects.begin(), objects.end(), is_avoidance_target); + }; + const auto num_of_avoidance_targets = + std::accumulate(object_parameters.begin(), object_parameters.end(), 0UL, count_target_object); - const auto THRESHOLD = avoidance_parameters_->execute_object_longitudinal_margin; - if (front_object.longitudinal < THRESHOLD) { + if (num_of_avoidance_targets < 1) { return false; } - const auto path = status_.lane_change_path.path; - const auto to_lc_end = motion_utils::calcSignedArcLength( - status_.lane_change_path.path.points, getEgoPose().position, - status_.lane_change_path.info.shift_line.end.position); - const auto execute_only_when_lane_change_finish_before_object = - avoidance_parameters_->execute_only_when_lane_change_finish_before_object; - const auto not_enough_distance = front_object.longitudinal < to_lc_end; - - if (execute_only_when_lane_change_finish_before_object && not_enough_distance) { + const auto current_lanes = getCurrentLanes(); + if (current_lanes.empty()) { return false; } - return true; + const auto & nearest_object = data.target_objects.front(); + + // get minimum lane change distance + const auto shift_intervals = + getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); + const double minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, shift_intervals, + lane_change_parameters_->backward_length_buffer_for_end_of_lane); + + // get minimum avoid distance + + const auto ego_width = getCommonParam().vehicle_width; + const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification); + const auto nearest_object_parameter = + avoidance_parameters_->object_parameters.at(nearest_object_type); + const auto avoid_margin = + nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor + + nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width; + + avoidance_helper_->setData(planner_data_); + const auto shift_length = avoidance_helper_->getShiftLength( + nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin); + + const auto maximum_avoid_distance = avoidance_helper_->getMaxAvoidanceDistance(shift_length); + + RCLCPP_DEBUG( + logger_, + "nearest_object.longitudinal %.3f, minimum_lane_change_length %.3f, maximum_avoid_distance " + "%.3f", + nearest_object.longitudinal, minimum_lane_change_length, maximum_avoid_distance); + + return nearest_object.longitudinal > std::max(minimum_lane_change_length, maximum_avoid_distance); } bool AvoidanceByLaneChange::specialExpiredCheck() const diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp new file mode 100644 index 0000000000000..212ed04ade6c6 --- /dev/null +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -0,0 +1,364 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ +#define BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ + +#include "tier4_autoware_utils/ros/parameter.hpp" + +#include +#include + +#include + +#include +#include + +namespace behavior_path_planner +{ +using autoware_auto_perception_msgs::msg::ObjectClassification; +using tier4_autoware_utils::getOrDeclareParameter; + +AvoidanceParameters getParameter(rclcpp::Node * node) +{ + AvoidanceParameters p{}; + // general params + { + const std::string ns = "avoidance."; + p.resample_interval_for_planning = + getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); + p.resample_interval_for_output = + getOrDeclareParameter(*node, ns + "resample_interval_for_output"); + p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); + p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); + p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable_yield_maneuver"); + p.enable_yield_maneuver_during_shifting = + getOrDeclareParameter(*node, ns + "enable_yield_maneuver_during_shifting"); + p.disable_path_update = getOrDeclareParameter(*node, ns + "disable_path_update"); + p.publish_debug_marker = getOrDeclareParameter(*node, ns + "publish_debug_marker"); + p.print_debug_info = getOrDeclareParameter(*node, ns + "print_debug_info"); + } + + // drivable area + { + const std::string ns = "avoidance."; + p.use_adjacent_lane = getOrDeclareParameter(*node, ns + "use_adjacent_lane"); + p.use_opposite_lane = getOrDeclareParameter(*node, ns + "use_opposite_lane"); + p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); + p.use_hatched_road_markings = + getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); + } + + // target object + { + const auto get_object_param = [&](std::string && ns) { + ObjectParameter param{}; + param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); + param.moving_speed_threshold = + getOrDeclareParameter(*node, ns + "moving_speed_threshold"); + param.moving_time_threshold = + getOrDeclareParameter(*node, ns + "moving_time_threshold"); + param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); + param.envelope_buffer_margin = + getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); + param.avoid_margin_lateral = + getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); + param.safety_buffer_lateral = + getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); + param.safety_buffer_longitudinal = + getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); + param.use_conservative_buffer_longitudinal = + getOrDeclareParameter(*node, ns + "use_conservative_buffer_longitudinal"); + return param; + }; + + const std::string ns = "avoidance.target_object."; + p.object_parameters.emplace( + ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); + p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); + p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); + p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); + p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); + p.object_parameters.emplace( + ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); + p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); + p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); + + p.lower_distance_for_polygon_expansion = + getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); + p.upper_distance_for_polygon_expansion = + getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); + } + + // target filtering + { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_avoidance_target = + getOrDeclareParameter(*node, ns); + }; + + const std::string ns = "avoidance.target_filtering."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + + p.object_check_goal_distance = + getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + p.threshold_distance_object_is_on_center = + getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); + p.object_check_shiftable_ratio = + getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); + p.object_check_min_road_shoulder_width = + getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); + p.object_check_yaw_deviation = + getOrDeclareParameter(*node, ns + "intersection.yaw_deviation"); + p.object_last_seen_threshold = + getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); + } + + { + const std::string ns = "avoidance.target_filtering.force_avoidance."; + p.enable_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "enable"); + p.threshold_time_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "time_threshold"); + p.object_ignore_section_traffic_light_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + } + + { + const std::string ns = "avoidance.target_filtering.detection_area."; + p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.object_check_min_forward_distance = + getOrDeclareParameter(*node, ns + "min_forward_distance"); + p.object_check_max_forward_distance = + getOrDeclareParameter(*node, ns + "max_forward_distance"); + p.object_check_backward_distance = + getOrDeclareParameter(*node, ns + "backward_distance"); + } + + // safety check general params + { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_safety_check_target = + getOrDeclareParameter(*node, ns); + }; + + const std::string ns = "avoidance.safety_check."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + + p.enable_safety_check = getOrDeclareParameter(*node, ns + "enable"); + p.check_current_lane = getOrDeclareParameter(*node, ns + "check_current_lane"); + p.check_shift_side_lane = getOrDeclareParameter(*node, ns + "check_shift_side_lane"); + p.check_other_side_lane = getOrDeclareParameter(*node, ns + "check_other_side_lane"); + p.check_unavoidable_object = + getOrDeclareParameter(*node, ns + "check_unavoidable_object"); + p.check_other_object = getOrDeclareParameter(*node, ns + "check_other_object"); + p.check_all_predicted_path = + getOrDeclareParameter(*node, ns + "check_all_predicted_path"); + p.safety_check_backward_distance = + getOrDeclareParameter(*node, ns + "safety_check_backward_distance"); + p.hysteresis_factor_expand_rate = + getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); + p.hysteresis_factor_safe_count = + getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); + } + + // safety check predicted path params + { + const std::string ns = "avoidance.safety_check."; + p.ego_predicted_path_params.min_velocity = + getOrDeclareParameter(*node, ns + "min_velocity"); + p.ego_predicted_path_params.max_velocity = + getOrDeclareParameter(*node, ns + "max_velocity"); + p.ego_predicted_path_params.acceleration = + getOrDeclareParameter(*node, "avoidance.constraints.longitudinal.max_acceleration"); + p.ego_predicted_path_params.time_horizon_for_rear_object = + getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); + p.ego_predicted_path_params.time_resolution = + getOrDeclareParameter(*node, ns + "time_resolution"); + p.ego_predicted_path_params.delay_until_departure = + getOrDeclareParameter(*node, ns + "delay_until_departure"); + } + + // safety check rss params + { + const std::string ns = "avoidance.safety_check."; + p.rss_params.longitudinal_distance_min_threshold = + getOrDeclareParameter(*node, ns + "longitudinal_distance_min_threshold"); + p.rss_params.longitudinal_velocity_delta_time = + getOrDeclareParameter(*node, ns + "longitudinal_velocity_delta_time"); + p.rss_params.front_vehicle_deceleration = + getOrDeclareParameter(*node, ns + "expected_front_deceleration"); + p.rss_params.rear_vehicle_deceleration = + getOrDeclareParameter(*node, ns + "expected_rear_deceleration"); + p.rss_params.rear_vehicle_reaction_time = + getOrDeclareParameter(*node, ns + "rear_vehicle_reaction_time"); + p.rss_params.rear_vehicle_safety_time_margin = + getOrDeclareParameter(*node, ns + "rear_vehicle_safety_time_margin"); + p.rss_params.lateral_distance_max_threshold = + getOrDeclareParameter(*node, ns + "lateral_distance_max_threshold"); + } + + // avoidance maneuver (lateral) + { + const std::string ns = "avoidance.avoidance.lateral."; + p.soft_road_shoulder_margin = + getOrDeclareParameter(*node, ns + "soft_road_shoulder_margin"); + p.hard_road_shoulder_margin = + getOrDeclareParameter(*node, ns + "hard_road_shoulder_margin"); + p.lateral_execution_threshold = + getOrDeclareParameter(*node, ns + "lateral_execution_threshold"); + p.lateral_small_shift_threshold = + getOrDeclareParameter(*node, ns + "lateral_small_shift_threshold"); + p.lateral_avoid_check_threshold = + getOrDeclareParameter(*node, ns + "lateral_avoid_check_threshold"); + p.max_right_shift_length = getOrDeclareParameter(*node, ns + "max_right_shift_length"); + p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); + p.max_deviation_from_lane = + getOrDeclareParameter(*node, ns + "max_deviation_from_lane"); + } + + // avoidance maneuver (longitudinal) + { + const std::string ns = "avoidance.avoidance.longitudinal."; + p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); + p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); + p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); + p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); + p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); + p.nominal_avoidance_speed = + getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); + } + + // avoidance maneuver (return shift dead line) + { + const std::string ns = "avoidance.avoidance.return_dead_line."; + p.enable_dead_line_for_goal = getOrDeclareParameter(*node, ns + "goal.enable"); + p.enable_dead_line_for_traffic_light = + getOrDeclareParameter(*node, ns + "traffic_light.enable"); + p.dead_line_buffer_for_goal = getOrDeclareParameter(*node, ns + "goal.buffer"); + p.dead_line_buffer_for_traffic_light = + getOrDeclareParameter(*node, ns + "traffic_light.buffer"); + } + + // yield + { + const std::string ns = "avoidance.yield."; + p.yield_velocity = getOrDeclareParameter(*node, ns + "yield_velocity"); + } + + // stop + { + const std::string ns = "avoidance.stop."; + p.stop_max_distance = getOrDeclareParameter(*node, ns + "max_distance"); + p.stop_buffer = getOrDeclareParameter(*node, ns + "stop_buffer"); + } + + // policy + { + const std::string ns = "avoidance.policy."; + p.policy_approval = getOrDeclareParameter(*node, ns + "make_approval_request"); + p.policy_deceleration = getOrDeclareParameter(*node, ns + "deceleration"); + p.policy_lateral_margin = getOrDeclareParameter(*node, ns + "lateral_margin"); + p.use_shorten_margin_immediately = + getOrDeclareParameter(*node, ns + "use_shorten_margin_immediately"); + + if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") { + throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); + } + + if (p.policy_lateral_margin != "best_effort" && p.policy_lateral_margin != "reliable") { + throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); + } + } + + // constraints (longitudinal) + { + const std::string ns = "avoidance.constraints.longitudinal."; + p.nominal_deceleration = getOrDeclareParameter(*node, ns + "nominal_deceleration"); + p.nominal_jerk = getOrDeclareParameter(*node, ns + "nominal_jerk"); + p.max_deceleration = getOrDeclareParameter(*node, ns + "max_deceleration"); + p.max_jerk = getOrDeclareParameter(*node, ns + "max_jerk"); + p.max_acceleration = getOrDeclareParameter(*node, ns + "max_acceleration"); + } + + // constraints (lateral) + { + const std::string ns = "avoidance.constraints.lateral."; + p.velocity_map = getOrDeclareParameter>(*node, ns + "velocity"); + p.lateral_max_accel_map = + getOrDeclareParameter>(*node, ns + "max_accel_values"); + p.lateral_min_jerk_map = + getOrDeclareParameter>(*node, ns + "min_jerk_values"); + p.lateral_max_jerk_map = + getOrDeclareParameter>(*node, ns + "max_jerk_values"); + + if (p.velocity_map.empty()) { + throw std::domain_error("invalid velocity map."); + } + + if (p.velocity_map.size() != p.lateral_max_accel_map.size()) { + throw std::domain_error("inconsistency among the constraints map."); + } + + if (p.velocity_map.size() != p.lateral_min_jerk_map.size()) { + throw std::domain_error("inconsistency among the constraints map."); + } + + if (p.velocity_map.size() != p.lateral_max_jerk_map.size()) { + throw std::domain_error("inconsistency among the constraints map."); + } + } + + // shift line pipeline + { + const std::string ns = "avoidance.shift_line_pipeline."; + p.quantize_filter_threshold = + getOrDeclareParameter(*node, ns + "trim.quantize_filter_threshold"); + p.same_grad_filter_1_threshold = + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_1_threshold"); + p.same_grad_filter_2_threshold = + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_2_threshold"); + p.same_grad_filter_3_threshold = + getOrDeclareParameter(*node, ns + "trim.same_grad_filter_3_threshold"); + p.sharp_shift_filter_threshold = + getOrDeclareParameter(*node, ns + "trim.sharp_shift_filter_threshold"); + } + return p; +} +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index f4997d2af12f1..68cee1aa2b523 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -14,6 +14,7 @@ #include "behavior_path_avoidance_module/manager.hpp" +#include "behavior_path_avoidance_module/parameter_helper.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -33,332 +34,7 @@ void AvoidanceModuleManager::init(rclcpp::Node * node) // init manager interface initInterface(node, {"left", "right"}); - AvoidanceParameters p{}; - // general params - { - const std::string ns = "avoidance."; - p.resample_interval_for_planning = - getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); - p.resample_interval_for_output = - getOrDeclareParameter(*node, ns + "resample_interval_for_output"); - p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); - p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); - p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable_yield_maneuver"); - p.enable_yield_maneuver_during_shifting = - getOrDeclareParameter(*node, ns + "enable_yield_maneuver_during_shifting"); - p.disable_path_update = getOrDeclareParameter(*node, ns + "disable_path_update"); - p.publish_debug_marker = getOrDeclareParameter(*node, ns + "publish_debug_marker"); - p.print_debug_info = getOrDeclareParameter(*node, ns + "print_debug_info"); - } - - // drivable area - { - const std::string ns = "avoidance."; - p.use_adjacent_lane = getOrDeclareParameter(*node, ns + "use_adjacent_lane"); - p.use_opposite_lane = getOrDeclareParameter(*node, ns + "use_opposite_lane"); - p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); - p.use_hatched_road_markings = - getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); - } - - // target object - { - const auto get_object_param = [&](std::string && ns) { - ObjectParameter param{}; - param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); - param.moving_speed_threshold = - getOrDeclareParameter(*node, ns + "moving_speed_threshold"); - param.moving_time_threshold = - getOrDeclareParameter(*node, ns + "moving_time_threshold"); - param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); - param.envelope_buffer_margin = - getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = - getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = - getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); - param.safety_buffer_longitudinal = - getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); - param.use_conservative_buffer_longitudinal = - getOrDeclareParameter(*node, ns + "use_conservative_buffer_longitudinal"); - return param; - }; - - const std::string ns = "avoidance.target_object."; - p.object_parameters.emplace( - ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); - p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); - p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); - p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); - p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); - p.object_parameters.emplace( - ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); - p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); - p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); - - p.lower_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); - p.upper_distance_for_polygon_expansion = - getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); - } - - // target filtering - { - const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { - if (p.object_parameters.count(object_type) == 0) { - return; - } - p.object_parameters.at(object_type).is_avoidance_target = - getOrDeclareParameter(*node, ns); - }; - - const std::string ns = "avoidance.target_filtering."; - set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); - set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); - set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); - set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); - set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); - set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); - set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); - set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); - - p.object_check_goal_distance = - getOrDeclareParameter(*node, ns + "object_check_goal_distance"); - p.threshold_distance_object_is_on_center = - getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); - p.object_check_shiftable_ratio = - getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); - p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); - p.object_check_yaw_deviation = - getOrDeclareParameter(*node, ns + "intersection.yaw_deviation"); - p.object_last_seen_threshold = - getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); - } - - { - const std::string ns = "avoidance.target_filtering.force_avoidance."; - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable"); - p.threshold_time_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "time_threshold"); - p.object_ignore_section_traffic_light_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); - p.object_ignore_section_crosswalk_behind_distance = - getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); - } - - { - const std::string ns = "avoidance.target_filtering.detection_area."; - p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); - p.object_check_min_forward_distance = - getOrDeclareParameter(*node, ns + "min_forward_distance"); - p.object_check_max_forward_distance = - getOrDeclareParameter(*node, ns + "max_forward_distance"); - p.object_check_backward_distance = - getOrDeclareParameter(*node, ns + "backward_distance"); - } - - // safety check general params - { - const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { - if (p.object_parameters.count(object_type) == 0) { - return; - } - p.object_parameters.at(object_type).is_safety_check_target = - getOrDeclareParameter(*node, ns); - }; - - const std::string ns = "avoidance.safety_check."; - set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); - set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); - set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); - set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); - set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); - set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); - set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); - set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); - - p.enable_safety_check = getOrDeclareParameter(*node, ns + "enable"); - p.check_current_lane = getOrDeclareParameter(*node, ns + "check_current_lane"); - p.check_shift_side_lane = getOrDeclareParameter(*node, ns + "check_shift_side_lane"); - p.check_other_side_lane = getOrDeclareParameter(*node, ns + "check_other_side_lane"); - p.check_unavoidable_object = - getOrDeclareParameter(*node, ns + "check_unavoidable_object"); - p.check_other_object = getOrDeclareParameter(*node, ns + "check_other_object"); - p.check_all_predicted_path = - getOrDeclareParameter(*node, ns + "check_all_predicted_path"); - p.safety_check_backward_distance = - getOrDeclareParameter(*node, ns + "safety_check_backward_distance"); - p.hysteresis_factor_expand_rate = - getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); - p.hysteresis_factor_safe_count = - getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); - } - - // safety check predicted path params - { - const std::string ns = "avoidance.safety_check."; - p.ego_predicted_path_params.min_velocity = - getOrDeclareParameter(*node, ns + "min_velocity"); - p.ego_predicted_path_params.max_velocity = - getOrDeclareParameter(*node, ns + "max_velocity"); - p.ego_predicted_path_params.acceleration = - getOrDeclareParameter(*node, "avoidance.constraints.longitudinal.max_acceleration"); - p.ego_predicted_path_params.time_horizon_for_rear_object = - getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); - p.ego_predicted_path_params.time_resolution = - getOrDeclareParameter(*node, ns + "time_resolution"); - p.ego_predicted_path_params.delay_until_departure = - getOrDeclareParameter(*node, ns + "delay_until_departure"); - } - - // safety check rss params - { - const std::string ns = "avoidance.safety_check."; - p.rss_params.longitudinal_distance_min_threshold = - getOrDeclareParameter(*node, ns + "longitudinal_distance_min_threshold"); - p.rss_params.longitudinal_velocity_delta_time = - getOrDeclareParameter(*node, ns + "longitudinal_velocity_delta_time"); - p.rss_params.front_vehicle_deceleration = - getOrDeclareParameter(*node, ns + "expected_front_deceleration"); - p.rss_params.rear_vehicle_deceleration = - getOrDeclareParameter(*node, ns + "expected_rear_deceleration"); - p.rss_params.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, ns + "rear_vehicle_reaction_time"); - p.rss_params.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, ns + "rear_vehicle_safety_time_margin"); - p.rss_params.lateral_distance_max_threshold = - getOrDeclareParameter(*node, ns + "lateral_distance_max_threshold"); - } - - // avoidance maneuver (lateral) - { - const std::string ns = "avoidance.avoidance.lateral."; - p.soft_road_shoulder_margin = - getOrDeclareParameter(*node, ns + "soft_road_shoulder_margin"); - p.hard_road_shoulder_margin = - getOrDeclareParameter(*node, ns + "hard_road_shoulder_margin"); - p.lateral_execution_threshold = - getOrDeclareParameter(*node, ns + "lateral_execution_threshold"); - p.lateral_small_shift_threshold = - getOrDeclareParameter(*node, ns + "lateral_small_shift_threshold"); - p.lateral_avoid_check_threshold = - getOrDeclareParameter(*node, ns + "lateral_avoid_check_threshold"); - p.max_right_shift_length = getOrDeclareParameter(*node, ns + "max_right_shift_length"); - p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); - p.max_deviation_from_lane = - getOrDeclareParameter(*node, ns + "max_deviation_from_lane"); - } - - // avoidance maneuver (longitudinal) - { - const std::string ns = "avoidance.avoidance.longitudinal."; - p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); - p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); - p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); - p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); - p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); - p.nominal_avoidance_speed = - getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); - } - - // avoidance maneuver (return shift dead line) - { - const std::string ns = "avoidance.avoidance.return_dead_line."; - p.enable_dead_line_for_goal = getOrDeclareParameter(*node, ns + "goal.enable"); - p.enable_dead_line_for_traffic_light = - getOrDeclareParameter(*node, ns + "traffic_light.enable"); - p.dead_line_buffer_for_goal = getOrDeclareParameter(*node, ns + "goal.buffer"); - p.dead_line_buffer_for_traffic_light = - getOrDeclareParameter(*node, ns + "traffic_light.buffer"); - } - - // yield - { - const std::string ns = "avoidance.yield."; - p.yield_velocity = getOrDeclareParameter(*node, ns + "yield_velocity"); - } - - // stop - { - const std::string ns = "avoidance.stop."; - p.stop_max_distance = getOrDeclareParameter(*node, ns + "max_distance"); - p.stop_buffer = getOrDeclareParameter(*node, ns + "stop_buffer"); - } - - // policy - { - const std::string ns = "avoidance.policy."; - p.policy_approval = getOrDeclareParameter(*node, ns + "make_approval_request"); - p.policy_deceleration = getOrDeclareParameter(*node, ns + "deceleration"); - p.policy_lateral_margin = getOrDeclareParameter(*node, ns + "lateral_margin"); - p.use_shorten_margin_immediately = - getOrDeclareParameter(*node, ns + "use_shorten_margin_immediately"); - - if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") { - throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); - } - - if (p.policy_lateral_margin != "best_effort" && p.policy_lateral_margin != "reliable") { - throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); - } - } - - // constraints (longitudinal) - { - const std::string ns = "avoidance.constraints.longitudinal."; - p.nominal_deceleration = getOrDeclareParameter(*node, ns + "nominal_deceleration"); - p.nominal_jerk = getOrDeclareParameter(*node, ns + "nominal_jerk"); - p.max_deceleration = getOrDeclareParameter(*node, ns + "max_deceleration"); - p.max_jerk = getOrDeclareParameter(*node, ns + "max_jerk"); - p.max_acceleration = getOrDeclareParameter(*node, ns + "max_acceleration"); - } - - // constraints (lateral) - { - const std::string ns = "avoidance.constraints.lateral."; - p.velocity_map = getOrDeclareParameter>(*node, ns + "velocity"); - p.lateral_max_accel_map = - getOrDeclareParameter>(*node, ns + "max_accel_values"); - p.lateral_min_jerk_map = - getOrDeclareParameter>(*node, ns + "min_jerk_values"); - p.lateral_max_jerk_map = - getOrDeclareParameter>(*node, ns + "max_jerk_values"); - - if (p.velocity_map.empty()) { - throw std::domain_error("invalid velocity map."); - } - - if (p.velocity_map.size() != p.lateral_max_accel_map.size()) { - throw std::domain_error("inconsistency among the constraints map."); - } - - if (p.velocity_map.size() != p.lateral_min_jerk_map.size()) { - throw std::domain_error("inconsistency among the constraints map."); - } - - if (p.velocity_map.size() != p.lateral_max_jerk_map.size()) { - throw std::domain_error("inconsistency among the constraints map."); - } - } - - // shift line pipeline - { - const std::string ns = "avoidance.shift_line_pipeline."; - p.quantize_filter_threshold = - getOrDeclareParameter(*node, ns + "trim.quantize_filter_threshold"); - p.same_grad_filter_1_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_1_threshold"); - p.same_grad_filter_2_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_2_threshold"); - p.same_grad_filter_3_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_3_threshold"); - p.sharp_shift_filter_threshold = - getOrDeclareParameter(*node, ns + "trim.sharp_shift_filter_threshold"); - } + auto p = getParameter(node); parameters_ = std::make_shared(p); } diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 9ff4ed95e09dd..df73990b770a3 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -244,7 +244,9 @@ lanelet::ConstLanelets getTargetNeighborLanes( for (const auto & current_lane : current_lanes) { if (route_handler.getNumLaneToPreferredLane(current_lane) != 0) { - if (type == LaneChangeModuleType::NORMAL) { + if ( + type == LaneChangeModuleType::NORMAL || + type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { neighbor_lanes.push_back(current_lane); } } else { @@ -782,7 +784,9 @@ std::optional getLaneChangeTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType type, const Direction & direction) { - if (type == LaneChangeModuleType::NORMAL) { + if ( + type == LaneChangeModuleType::NORMAL || + type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { return route_handler.getLaneChangeTarget(current_lanes, direction); } diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 36d5c7773fd24..e4bfff8fc63b8 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -751,9 +751,11 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrname().find("lane_change") != std::string::npos; }); + const auto success_lane_change = + std::any_of(itr, approved_module_ptrs_.end(), [](const auto & m) { + return m->name().find("lane_change") != std::string::npos || + m->name().find("avoidance_by_lc") != std::string::npos; + }); if (success_lane_change) { root_lanelet_ = updateRootLanelet(data); @@ -837,9 +839,11 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr & data) // when lane change module is running, don't update root lanelet. const bool is_lane_change_running = std::invoke([&]() { - const auto lane_change_itr = std::find_if( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), - [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); + const auto lane_change_itr = + std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [](const auto & m) { + return m->name().find("lane_change") != std::string::npos || + m->name().find("avoidance_by_lc") != std::string::npos; + }); return lane_change_itr != approved_module_ptrs_.end(); }); if (is_lane_change_running) {