From a4ef3a0bf5d7be71dd0d35ed497a571f9cfe804f Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Wed, 20 Dec 2023 00:26:33 +0900 Subject: [PATCH 1/8] chore(crosswalk, obstacle_cruise): add maintainer (#5898) * add maintainer Signed-off-by: Yuki Takagi --- planning/behavior_velocity_crosswalk_module/package.xml | 2 ++ planning/obstacle_cruise_planner/package.xml | 1 + 2 files changed, 3 insertions(+) diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 3830aa9edddff..9eef11e72239e 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -10,6 +10,8 @@ Shumpei Wakabayashi Takayuki Murooka Kyoichi Sugahara + Mamoru Sobue + Yuki Takagi Apache License 2.0 diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml index e724ddb3e6bd7..f6263521bd337 100644 --- a/planning/obstacle_cruise_planner/package.xml +++ b/planning/obstacle_cruise_planner/package.xml @@ -7,6 +7,7 @@ Takayuki Murooka Kosuke Takeuchi Satoshi Ota + Yuki Takagi Apache License 2.0 From c9b9fca4567d7fc64590153ca02d8d7eb17ab824 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 20 Dec 2023 08:11:11 +0900 Subject: [PATCH 2/8] refactor(avoidance_by_lane_change): update execution condition (#5869) * refactor(avoidance_by_lane_change): update execution condition Signed-off-by: Muhammad Zulfaqar Azmi * fix lc parameter Signed-off-by: Muhammad Zulfaqar Azmi * fix both lane change and avoidance by lane change both are running Signed-off-by: Zulfaqar Azmi * trying to set maximum_avoid_distance Signed-off-by: Muhammad Zulfaqar Azmi * fix avoidance param not properly assigned Signed-off-by: Zulfaqar Azmi * fixed avoidance not running Signed-off-by: Zulfaqar Azmi * fix root lanelet Signed-off-by: Zulfaqar Azmi * removed gdb Signed-off-by: Zulfaqar Azmi * add debug Signed-off-by: Zulfaqar Azmi * fix unnecessary changes Signed-off-by: Zulfaqar Azmi * Update planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Zulfaqar Azmi Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../config/avoidance_by_lc.param.yaml | 8 + .../data_structs.hpp | 6 +- .../interface.hpp | 2 + .../scene.hpp | 3 + .../src/interface.cpp | 6 +- .../src/manager.cpp | 18 +- .../src/scene.cpp | 79 ++-- .../parameter_helper.hpp | 364 ++++++++++++++++++ .../src/manager.cpp | 328 +--------------- .../src/utils/utils.cpp | 8 +- .../src/planner_manager.cpp | 16 +- 11 files changed, 471 insertions(+), 367 deletions(-) create mode 100644 planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp 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) { From 4ca5500868b10d283f88c21ba92b8eb4ca9d173e Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 20 Dec 2023 11:25:56 +0900 Subject: [PATCH 3/8] chore(motion_utils): Enrich error message (#5665) * chore(motion_utils): colorize error message Signed-off-by: Takamasa Horibe * enrich error log Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../motion_utils/resample/resample_utils.hpp | 29 +++-- .../motion_utils/trajectory/trajectory.hpp | 114 ++++++++++++------ .../test/src/trajectory/test_trajectory.cpp | 14 +-- 3 files changed, 102 insertions(+), 55 deletions(-) diff --git a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp index 87a0d54af6619..8bb5f13e3fb78 100644 --- a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp @@ -15,6 +15,8 @@ #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ #define MOTION_UTILS__RESAMPLE__RESAMPLE_UTILS_HPP_ +#include "tier4_autoware_utils/system/backtrace.hpp" + #include #include #include @@ -25,6 +27,8 @@ namespace resample_utils { constexpr double close_s_threshold = 1e-6; +#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; + template bool validate_size(const T & points) { @@ -58,23 +62,28 @@ bool validate_arguments(const T & input_points, const std::vector & resa { // Check size of the arguments if (!validate_size(input_points)) { - std::cerr << "The number of input points is less than 2" << std::endl; + log_error("[resample_utils] invalid argument: The number of input points is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; } if (!validate_size(resampling_intervals)) { - std::cerr << "The number of resampling intervals is less than 2" << std::endl; + log_error( + "[resample_utils] invalid argument: The number of resampling intervals is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; } // Check resampling range if (!validate_resampling_range(input_points, resampling_intervals)) { - std::cerr << "resampling interval is longer than input points" << std::endl; + log_error("[resample_utils] invalid argument: resampling interval is longer than input points"); + tier4_autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { - std::cerr << "input points has some duplicated points" << std::endl; + log_error("[resample_utils] invalid argument: input points has some duplicated points"); + tier4_autoware_utils::print_backtrace(); return false; } @@ -86,20 +95,24 @@ bool validate_arguments(const T & input_points, const double resampling_interval { // Check size of the arguments if (!validate_size(input_points)) { - std::cerr << "The number of input points is less than 2" << std::endl; + log_error("[resample_utils] invalid argument: The number of input points is less than 2"); + tier4_autoware_utils::print_backtrace(); return false; } // check resampling interval if (resampling_interval < motion_utils::overlap_threshold) { - std::cerr << "resampling interval is less than " << motion_utils::overlap_threshold - << std::endl; + log_error( + "[resample_utils] invalid argument: resampling interval is less than " + + std::to_string(motion_utils::overlap_threshold)); + tier4_autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { - std::cerr << "input points has some duplicated points" << std::endl; + log_error("[resample_utils] invalid argument: input points has some duplicated points"); + tier4_autoware_utils::print_backtrace(); return false; } diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index f2e0c1c0c184c..d81f75815ecc6 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -30,10 +30,12 @@ #include #include #include +#include #include #include namespace motion_utils { +#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; /** * @brief validate if points container is empty or not @@ -44,7 +46,7 @@ void validateNonEmpty(const T & points) { if (points.empty()) { tier4_autoware_utils::print_backtrace(); - throw std::invalid_argument("Points is empty."); + throw std::invalid_argument("[motion_utils] validateNonEmpty(): Points is empty."); } } @@ -82,7 +84,7 @@ void validateNonSharpAngle( constexpr double epsilon = 1e-3; if (std::cos(angle_threshold) < product / dist_1to2 / dist_3to2 + epsilon) { tier4_autoware_utils::print_backtrace(); - throw std::invalid_argument("Sharp angle."); + throw std::invalid_argument("[motion_utils] validateNonSharpAngle(): Too sharp angle."); } } @@ -214,7 +216,7 @@ std::optional searchZeroVelocityIndex( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -246,7 +248,7 @@ std::optional searchZeroVelocityIndex(const T & points_with_twist, const try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -336,7 +338,7 @@ std::optional findNearestIndex( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -401,12 +403,17 @@ double calcLongitudinalOffsetToSegment( const bool throw_exception = false) { if (seg_idx >= points.size() - 1) { - const auto error_message{"Segment index is invalid."}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Failed to calculate longitudinal offset because the given segment index is out of the " + "points size."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -418,18 +425,22 @@ double calcLongitudinalOffsetToSegment( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return std::nan(""); } } if (seg_idx >= overlap_removed_points.size() - 1) { - const auto error_message{"Same points are given."}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Longitudinal offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -575,18 +586,24 @@ double calcLateralOffset( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error( + std::string(e.what()) + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } } if (overlap_removed_points.size() == 1) { - const auto error_message{"Same points are given."}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Lateral offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -637,18 +654,24 @@ double calcLateralOffset( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error( + std::string(e.what()) + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } } if (overlap_removed_points.size() == 1) { - const auto error_message{"Same points are given."}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + ": Lateral offset calculation is not supported for the same points."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return std::nan(""); } @@ -684,7 +707,7 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -727,7 +750,7 @@ std::vector calcSignedArcLengthPartialSum( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -779,7 +802,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -822,7 +845,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -861,7 +884,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -901,7 +924,7 @@ double calcArcLength(const T & points) try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } @@ -1007,7 +1030,7 @@ std::optional calcDistanceToForwardStopPoint( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1041,17 +1064,22 @@ std::optional calcLongitudinalOffsetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } if (points.size() - 1 < src_idx) { - const auto error_message{"Invalid source index"}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " error: The given source index is out of the points size. Failed to calculate longitudinal " + "offset."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return NaN since no_throw option is enabled. The maintainer must check the code."); return {}; } @@ -1118,7 +1146,7 @@ std::optional calcLongitudinalOffsetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); return {}; } @@ -1166,21 +1194,25 @@ std::optional calcLongitudinalOffsetPose( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); return {}; } if (points.size() - 1 < src_idx) { - const auto error_message{"Invalid source index"}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " error: The given source index is out of the points size. Failed to calculate longitudinal " + "offset."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::out_of_range(error_message); } - std::cerr << error_message << std::endl; + log_error(error_message); return {}; } if (points.size() == 1) { + log_error("Failed to calculate longitudinal offset: points size is one."); return {}; } @@ -1265,7 +1297,7 @@ std::optional calcLongitudinalOffsetPose( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1311,7 +1343,7 @@ std::optional insertTargetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -1326,7 +1358,7 @@ std::optional insertTargetPoint( try { validateNonSharpAngle(p_front, p_target, p_back); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return {}; } @@ -2192,7 +2224,7 @@ std::optional calcDistanceToForwardStopPoint( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error("Failed to calculate stop distance" + std::string(e.what())); return {}; } @@ -2331,9 +2363,7 @@ T cropPoints( cropped_forward_points, target_pos, modified_target_seg_idx, backward_length); if (cropped_points.size() < 2) { - RCLCPP_ERROR( - rclcpp::get_logger("obstacle_avoidance_planner.trajectory_utils"), - ". Return original points since cropped_points size is less than 2."); + log_error("Return original points since cropped_points size is less than 2."); return points; } @@ -2378,18 +2408,22 @@ double calcYawDeviation( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; + log_error(e.what()); return 0.0; } } if (overlap_removed_points.size() <= 1) { - const auto error_message{"points size is less than 2"}; + const std::string error_message( + "[motion_utils] " + std::string(__func__) + + " Given points size is less than 2. Failed to calculate yaw deviation."); tier4_autoware_utils::print_backtrace(); if (throw_exception) { throw std::runtime_error(error_message); } - std::cerr << error_message << std::endl; + log_error( + error_message + + " Return 0 since no_throw option is enabled. The maintainer must check the code."); return 0.0; } diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 22567b569d0ad..6664e6154bdfd 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -1883,7 +1883,7 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -1896,7 +1896,7 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -1909,7 +1909,7 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -2310,7 +2310,7 @@ TEST(trajectory, insertTargetPoint_Length) const auto p_target = createPoint(4.0, 10.0, 0.0); const auto insert_idx = insertTargetPoint(4.0, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4317,7 +4317,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4330,7 +4330,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4343,7 +4343,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); + EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } From 9761d21de6afbbbb0b9a24a219cc79cd588d3a16 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 20 Dec 2023 13:01:05 +0900 Subject: [PATCH 4/8] feat(intersection): disable stuck detection against private lane (#5910) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 3 +-- .../src/manager.cpp | 9 +++------ .../src/scene_intersection.cpp | 10 ++++++---- .../src/scene_intersection.hpp | 10 +++------- 4 files changed, 13 insertions(+), 19 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 997addd48d7f8..6fc136c512689 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -23,8 +23,7 @@ stuck_vehicle_velocity_threshold: 0.833 # enable_front_car_decel_prediction: false # assumed_front_car_decel: 1.0 - timeout_private_area: 3.0 - enable_private_area_stuck_disregard: false + disable_against_private_lane: true yield_stuck: turn_direction: diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 789708abe98f8..e859b15b345b8 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -74,10 +74,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); ip.stuck_vehicle.stuck_vehicle_velocity_threshold = getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity_threshold"); - ip.stuck_vehicle.timeout_private_area = - getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); - ip.stuck_vehicle.enable_private_area_stuck_disregard = - getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard"); + ip.stuck_vehicle.disable_against_private_lane = + getOrDeclareParameter(node, ns + ".stuck_vehicle.disable_against_private_lane"); ip.yield_stuck.turn_direction.left = getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.left"); @@ -201,7 +199,6 @@ void IntersectionModuleManager::launchNewModules( } const std::string location = ll.attributeOr("location", "else"); - const bool is_private_area = (location.compare("private") == 0); const auto associative_ids = planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); bool has_traffic_light = false; @@ -213,7 +210,7 @@ void IntersectionModuleManager::launchNewModules( } const auto new_module = std::make_shared( module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, - has_traffic_light, enable_occlusion_detection, is_private_area, node_, + has_traffic_light, enable_occlusion_detection, node_, logger_.get_child("intersection_module"), clock_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index e036fac13fd3b..5909d607f6351 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -97,8 +97,8 @@ IntersectionModule::IntersectionModule( [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, - const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) + const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), node_(node), lane_id_(lane_id), @@ -107,7 +107,6 @@ IntersectionModule::IntersectionModule( has_traffic_light_(has_traffic_light), enable_occlusion_detection_(enable_occlusion_detection), occlusion_attention_divisions_(std::nullopt), - is_private_area_(is_private_area), occlusion_uuid_(tier4_autoware_utils::generateUUID()) { velocity_factor_.init(PlanningBehavior::INTERSECTION); @@ -1056,8 +1055,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); + const bool is_first_conflicting_lane_private = + (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); if (stuck_detected) { - if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { + if (!(is_first_conflicting_lane_private && + planner_param_.stuck_vehicle.disable_against_private_lane)) { } else { std::optional stopline_idx = std::nullopt; if (stuck_stopline_idx_opt) { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 7366bdc1bc0e6..2f8c296a348b9 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -74,8 +74,7 @@ class IntersectionModule : public SceneModuleInterface bool use_stuck_stopline; double stuck_vehicle_detect_dist; double stuck_vehicle_velocity_threshold; - double timeout_private_area; - bool enable_private_area_stuck_disregard; + bool disable_against_private_lane; } stuck_vehicle; struct YieldStuck @@ -263,8 +262,8 @@ class IntersectionModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, - const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path @@ -314,9 +313,6 @@ class IntersectionModule : public SceneModuleInterface // vehicles are very slow std::optional initial_green_light_observed_time_{std::nullopt}; - // for stuck vehicle detection - const bool is_private_area_; - // for RTC const UUID occlusion_uuid_; bool occlusion_safety_{true}; From 4225f4e794bba4c52b33bbd5484ec1f6926ee903 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Wed, 20 Dec 2023 14:01:47 +0900 Subject: [PATCH 5/8] fix(run_out): do not insert stop point when stopped in Object method (#5911) Signed-off-by: Tomohito Ando --- .../src/scene.cpp | 21 +++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index 92516e7b4424b..b14152863d8d0 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -615,16 +615,25 @@ std::optional RunOutModule::calcStopPoint( // vehicle have to decelerate if there is not enough distance with deceleration_jerk const bool deceleration_needed = *stop_dist > dist_to_collision - planner_param_.run_out.stop_margin; - // avoid acceleration when ego is decelerating - // TODO(Tomohito Ando): replace with more appropriate method - constexpr float epsilon = 1.0e-2; - constexpr float stopping_vel_mps = 2.5 / 3.6; - const bool is_stopping = current_vel < stopping_vel_mps && current_acc < epsilon; - if (!deceleration_needed && !is_stopping) { + const auto & detection_method = planner_param_.run_out.detection_method; + + if (!deceleration_needed && detection_method == "Object") { debug_ptr_->setAccelReason(RunOutDebug::AccelReason::LOW_JERK); return {}; } + // If the detection method assumes running out, avoid acceleration when the ego is decelerating. + // TODO(Tomohito Ando): replace with more appropriate way + if (!deceleration_needed && detection_method != "Object") { + constexpr float epsilon = 1.0e-2; + constexpr float stopping_vel_mps = 2.5 / 3.6; + const bool is_stopping = current_vel < stopping_vel_mps && current_acc < epsilon; + if (!is_stopping) { + debug_ptr_->setAccelReason(RunOutDebug::AccelReason::LOW_JERK); + return {}; + } + } + // calculate the stop point for base link const float base_to_collision_point = planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front; From 0dee51263a607ea19f99f9de34e26668368547a1 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 20 Dec 2023 14:03:09 +0900 Subject: [PATCH 6/8] fix(start_planner): refine shift pull out path to start pose (#5874) * fix(start_planner): refine shift pull out path to start pose Signed-off-by: kosuke55 * fix typo Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../shift_pull_out.hpp | 4 ++ .../src/shift_pull_out.cpp | 54 +++++++++++++++++++ 2 files changed, 58 insertions(+) diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index 9012504ed2e7a..2d50ae189dc13 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp @@ -46,6 +46,10 @@ class ShiftPullOut : public PullOutPlannerBase double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr); + bool refineShiftedPathToStartPose( + ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose, + const double longitudinal_acc, const double lateral_acc); + std::shared_ptr lane_departure_checker_; private: diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index 73952ca02f558..35281d5bedbcf 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -158,6 +158,58 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos return std::nullopt; } +bool ShiftPullOut::refineShiftedPathToStartPose( + ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose, + const double longitudinal_acc, const double lateral_acc) +{ + constexpr double TOLERANCE = 0.01; + constexpr size_t MAX_ITERATION = 100; + + // Lambda to check if change is above tolerance + auto is_within_tolerance = + [](const auto & prev_val, const auto & current_val, const auto & tolerance) { + return std::abs(current_val - prev_val) < tolerance; + }; + + size_t iteration = 0; + while (iteration < MAX_ITERATION) { + const double lateral_offset = + motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position); + + PathShifter path_shifter; + path_shifter.setPath(shifted_path.path); + ShiftLine shift_line{}; + shift_line.start = start_pose; + shift_line.end = end_pose; + shift_line.end_shift_length = lateral_offset; + path_shifter.addShiftLine(shift_line); + path_shifter.setVelocity(0.0); + path_shifter.setLongitudinalAcceleration(longitudinal_acc); + path_shifter.setLateralAccelerationLimit(lateral_acc); + + if (!path_shifter.generate(&shifted_path, false)) { + RCLCPP_WARN( + rclcpp::get_logger("ShiftPullOut:refineShiftedPathToStartPose()"), + "Failed to generate shifted path"); + return false; + } + + if (is_within_tolerance( + lateral_offset, + motion_utils::calcLateralOffset(shifted_path.path.points, start_pose.position), + TOLERANCE)) { + return true; + } + + iteration++; + } + + RCLCPP_WARN( + rclcpp::get_logger("ShiftPullOut:refineShiftedPathToStartPose()"), + "Failed to converge lateral offset until max iteration"); + return false; +} + std::vector ShiftPullOut::calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, const Pose & start_pose, const Pose & goal_pose) @@ -303,6 +355,8 @@ std::vector ShiftPullOut::calcPullOutPaths( if (!path_shifter.generate(&shifted_path, offset_back)) { continue; } + refineShiftedPathToStartPose( + shifted_path, start_pose, *shift_end_pose_ptr, longitudinal_acc, lateral_acc); // set velocity const size_t pull_out_end_idx = From 8781d1d783a65a4b8ba2a85d552384f7f85a8214 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 20 Dec 2023 18:09:09 +0900 Subject: [PATCH 7/8] refactor(traffic_light_module): boost::optional to std::optional (#5912) Signed-off-by: Muhammad Zulfaqar --- .../behavior_velocity_traffic_light_module/package.xml | 1 - .../behavior_velocity_traffic_light_module/src/debug.cpp | 2 -- .../behavior_velocity_traffic_light_module/src/manager.cpp | 1 - .../behavior_velocity_traffic_light_module/src/manager.hpp | 3 ++- .../behavior_velocity_traffic_light_module/src/scene.cpp | 7 ++----- .../behavior_velocity_traffic_light_module/src/scene.hpp | 7 +++---- 6 files changed, 7 insertions(+), 14 deletions(-) diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index c658f0890b986..39c454e59dcb2 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -24,7 +24,6 @@ eigen geometry_msgs lanelet2_extension - libboost-dev motion_utils pluginlib rclcpp diff --git a/planning/behavior_velocity_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_traffic_light_module/src/debug.cpp index 9a0ba4f37c3c0..b3dedaa29d6ad 100644 --- a/planning/behavior_velocity_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/debug.cpp @@ -26,8 +26,6 @@ namespace behavior_velocity_planner { -using tier4_autoware_utils::appendMarkerArray; - visualization_msgs::msg::MarkerArray TrafficLightModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 908627540bdcc..fa1a516c107b0 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index c36c6af1128ce..87213d8a5ed3f 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -26,6 +26,7 @@ #include #include +#include namespace behavior_velocity_planner { @@ -57,7 +58,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC // Debug rclcpp::Publisher::SharedPtr pub_tl_state_; - boost::optional first_ref_stop_path_point_index_; + std::optional first_ref_stop_path_point_index_; }; class TrafficLightModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index a74ff8c0cb5e8..d2d18d74a5c1a 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -19,7 +19,6 @@ #include #include -#include // To be replaced by std::optional in C++17 #include @@ -31,9 +30,7 @@ #include #include -#include #include -#include #include namespace behavior_velocity_planner @@ -52,14 +49,14 @@ bool getBackwardPointFromBasePoint( return true; } -boost::optional findNearestCollisionPoint( +std::optional findNearestCollisionPoint( const LineString2d & line1, const LineString2d & line2, const Point2d & origin) { std::vector collision_points; bg::intersection(line1, line2, collision_points); if (collision_points.empty()) { - return boost::none; + return std::nullopt; } // check nearest collision point diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 6e46709267427..c7b2472289bf5 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -17,7 +17,6 @@ #include #include -#include #include #include @@ -81,7 +80,7 @@ class TrafficLightModule : public SceneModuleInterface inline State getTrafficLightModuleState() const { return state_; } - inline boost::optional getFirstRefStopPathPointIndex() const + inline std::optional getFirstRefStopPathPointIndex() const { return first_ref_stop_path_point_index_; } @@ -130,9 +129,9 @@ class TrafficLightModule : public SceneModuleInterface // prevent stop chattering std::unique_ptr