diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml index a298967a31af9..f54d991a9d852 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -37,6 +37,8 @@ Planning: behavior_path_planner_avoidance: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance.utils behavior_path_planner_goal_planner: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner diff --git a/planning/behavior_path_avoidance_module/README.md b/planning/behavior_path_avoidance_module/README.md index dc6ab0ec4b6e1..aa7a3a42efdb4 100644 --- a/planning/behavior_path_avoidance_module/README.md +++ b/planning/behavior_path_avoidance_module/README.md @@ -760,176 +760,7 @@ WIP The avoidance specific parameter configuration file can be located at `src/autoware/launcher/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml`. -### General parameters - -namespace: `avoidance.` - -| Name | Unit | Type | Description | Default value | -| :------------------------------------ | :--- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| resample_interval_for_planning | [m] | double | Path resample interval for avoidance planning path. | 0.3 | -| resample_interval_for_output | [m] | double | Path resample interval for output path. Too short interval increases computational cost for latter modules. | 4.0 | -| detection_area_right_expand_dist | [m] | double | Lanelet expand length for right side to find avoidance target vehicles. | 0.0 | -| detection_area_left_expand_dist | [m] | double | Lanelet expand length for left side to find avoidance target vehicles. | 1.0 | -| enable_cancel_maneuver | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | -| enable_yield_maneuver | [-] | bool | Flag to enable yield maneuver. | false | -| enable_yield_maneuver_during_shifting | [-] | bool | Flag to enable yield maneuver during shifting. | false | - -| Name | Unit | Type | Description | Default value | -| :------------------------ | ---- | ---- | ----------------------------------------------------------------------------------------------------------------------- | ------------- | -| enable_bound_clipping | [-] | bool | Enable clipping left and right bound of drivable area when obstacles are in the drivable area | false | -| use_adjacent_lane | [-] | bool | Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane. | true | -| use_opposite_lane | [-] | bool | Extend avoidance trajectory to opposite direction lane. `use_adjacent_lane` must be `true` to take effects | true | -| use_intersection_areas | [-] | bool | Extend drivable to intersection area. | false | -| use_hatched_road_markings | [-] | bool | Extend drivable to hatched road marking area. | false | - -| Name | Unit | Type | Description | Default value | -| :------------------ | ---- | ---- | --------------------------------------------------------------------------------------- | ------------- | -| output_debug_marker | [-] | bool | Flag to publish debug marker (set `false` as default since it takes considerable cost). | false | -| output_debug_info | [-] | bool | Flag to print debug info (set `false` as default since it takes considerable cost). | false | - -### Avoidance target filtering parameters - -namespace: `avoidance.target_object.` - -This module supports all object classes, and it can set following parameters independently. - -```yaml -car: - is_target: true # [-] - moving_speed_threshold: 1.0 # [m/s] - moving_time_threshold: 1.0 # [s] - max_expand_ratio: 0.0 # [-] - envelope_buffer_margin: 0.3 # [m] - avoid_margin_lateral: 1.0 # [m] - safety_buffer_lateral: 0.7 # [m] - safety_buffer_longitudinal: 0.0 # [m] -``` - -| Name | Unit | Type | Description | Default value | -| :------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------- | ------------- | -| is_target | [-] | bool | By setting this flag `true`, this module avoid those class objects. | false | -| moving_speed_threshold | [m/s] | double | Objects with speed greater than this will be judged as moving ones. | 1.0 | -| moving_time_threshold | [s] | double | Objects keep moving longer duration than this will be excluded from avoidance target. | 1.0 | -| envelope_buffer_margin | [m] | double | The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation. | 0.3 | -| avoid_margin_lateral | [m] | double | The lateral distance between ego and avoidance targets. | 1.0 | -| safety_buffer_lateral | [m] | double | Creates an additional lateral gap that will prevent the vehicle from getting to near to the obstacle. | 0.5 | -| safety_buffer_longitudinal | [m] | double | Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle. | 0.0 | - -Parameters for the logic to compensate perception noise of the far objects. - -| Name | Unit | Type | Description | Default value | -| :----------------------------------- | ---- | ------ | ------------------------------------------------------------------------------------------------------------ | ------------- | -| max_expand_ratio | [-] | double | This value will be applied `envelope_buffer_margin` according to the distance between the ego and object. | 0.0 | -| lower_distance_for_polygon_expansion | [-] | double | If the distance between the ego and object is less than this, the expand ratio will be zero. | 30.0 | -| upper_distance_for_polygon_expansion | [-] | double | If the distance between the ego and object is larger than this, the expand ratio will be `max_expand_ratio`. | 100.0 | - -namespace: `avoidance.target_filtering.` - -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| threshold_distance_object_is_on_center | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 1.0 | -| object_ignore_section_traffic_light_in_front_distance | [m] | double | If the distance between traffic light and vehicle is less than this parameter, this module will ignore it. | 30.0 | -| object_ignore_section_crosswalk_in_front_distance | [m] | double | If the front distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | -| object_ignore_section_crosswalk_behind_distance | [m] | double | If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | -| object_check_forward_distance | [m] | double | Forward distance to search the avoidance target. | 150.0 | -| object_check_backward_distance | [m] | double | Backward distance to search the avoidance target. | 2.0 | -| object_check_goal_distance | [m] | double | If the distance between object and goal position is less than this parameter, the module do not return center line. | 20.0 | -| object_check_return_pose_distance | [m] | double | If the distance between object and return position is less than this parameter, the module do not return center line. | 20.0 | -| object_check_shiftable_ratio | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.6 | -| object_check_min_road_shoulder_width | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder target. | 0.5 | -| object_last_seen_threshold | [s] | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0 | - -### Safety check parameters - -namespace: `avoidance.safety_check.` - -| Name | Unit | Type | Description | Default value | -| :----------------------------- | ---- | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- | -| enable | [-] | bool | Enable to use safety check feature. | true | -| check_current_lane | [-] | bool | Check objects on current driving lane. | false | -| check_shift_side_lane | [-] | bool | Check objects on shift side lane. | true | -| check_other_side_lane | [-] | bool | Check objects on other side lane. | false | -| check_unavoidable_object | [-] | bool | Check collision between ego and unavoidable objects. | false | -| check_other_object | [-] | bool | Check collision between ego and non avoidance target objects. | false | -| check_all_predicted_path | [-] | bool | Check all prediction path of safety check target objects. | false | -| time_horizon | [s] | double | Time horizon to check lateral/longitudinal margin is enough or not. | 10.0 | -| time_resolution | [s] | double | Time resolution to check lateral/longitudinal margin is enough or not. | 0.5 | -| safety_check_backward_distance | [m] | double | Backward distance to search the dynamic objects. | 50.0 | -| safety_check_hysteresis_factor | [-] | double | Hysteresis factor that be used for chattering prevention. | 2.0 | -| safety_check_ego_offset | [m] | double | Output new avoidance path **only when** the offset between ego and previous output path is less than this. | 1.0 | - -### Avoidance maneuver parameters - -namespace: `avoidance.avoidance.lateral.` - -| Name | Unit | Type | Description | Default value | -| :---------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------------------- | :------------ | -| road_shoulder_safety_margin | [m] | double | Prevents the generated path to come too close to the road shoulders. | 0.0 | -| lateral_execution_threshold | [m] | double | The lateral distance deviation threshold between the current path and suggested avoidance point to execute avoidance. (\*2) | 0.499 | -| lateral_small_shift_threshold | [m] | double | The shift lines whose lateral offset is less than this will be applied with other ones. | 0.501 | -| max_right_shift_length | [m] | double | Maximum shift length for right direction | 5.0 | -| max_left_shift_length | [m] | double | Maximum shift length for left direction | 5.0 | - -namespace: `avoidance.avoidance.longitudinal.` - -| Name | Unit | Type | Description | Default value | -| :----------------------------------- | :---- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | -| prepare_time | [s] | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change. | 2.0 | -| longitudinal_collision_safety_buffer | [s] | double | Longitudinal collision buffer between target object and shift line. | 0.0 | -| min_prepare_distance | [m] | double | Minimum distance for "prepare_time" x "ego_speed". | 1.0 | -| min_avoidance_distance | [m] | double | Minimum distance of avoidance path (i.e. this distance is needed even if its lateral jerk is very low) | 10.0 | -| min_nominal_avoidance_speed | [m/s] | double | Minimum speed for jerk calculation in a nominal situation (\*1). | 7.0 | -| min_sharp_avoidance_speed | [m/s] | double | Minimum speed for jerk calculation in a sharp situation (\*1). | 1.0 | -| min_slow_down_speed | [m/s] | double | Minimum slow speed for avoidance prepare section. | 1.38 (5km/h) | -| buf_slow_down_speed | [m/s] | double | Buffer for controller tracking error. Basically, vehicle always cannot follow velocity profile precisely. Therefore, the module inserts lower speed than target speed that satisfies conditions to avoid object within accel/jerk constraints so that the avoidance path always can be output even if the current speed is a little bit higher than target speed. | 0.57 (2.0km/h) | - -### Yield maneuver parameters - -namespace: `avoidance.yield.` - -| Name | Unit | Type | Description | Default value | -| :------------- | :---- | :----- | :------------------------------------------------------------ | :------------ | -| yield_velocity | [m/s] | double | The ego will decelerate yield velocity in the yield maneuver. | 2.78 | - -### Stop maneuver parameters - -namespace: `avoidance.stop.` - -| Name | Unit | Type | Description | Default value | -| :----------- | :--- | :----- | :---------------------------------------------------------------------------------------------------- | :------------ | -| min_distance | [m] | double | Minimum stop distance in the situation where avoidance maneuver is not approved or in yield maneuver. | 10.0 | -| max_distance | [m] | double | Maximum stop distance in the situation where avoidance maneuver is not approved or in yield maneuver. | 20.0 | -| stop_buffer | [m] | double | Buffer distance in the situation where avoidance maneuver is not approved or in yield maneuver. | 1.0 | - -### Constraints parameters - -namespace: `avoidance.constraints.` - -| Name | Unit | Type | Description | Default value | -| :------------------------ | :--- | :--- | :--------------------------------------------------------------------------------------- | :------------ | -| use_constraints_for_decel | [-] | bool | Flag to decel under longitudinal constraints. `TRUE: allow to control breaking mildness` | false | - -namespace: `avoidance.constraints.lateral.` - -| a Name | Unit | Type | Description | Default value | -| :----------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------- | :------------ | -| prepare_time | [s] | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change. | 2.0 | -| min_prepare_distance | [m] | double | Minimum distance for "prepare_time" x "ego_speed". | 1.0 | -| nominal_lateral_jerk | [m/s3] | double | Avoidance path is generated with this jerk when there is enough distance from ego. | 0.2 | -| max_lateral_jerk | [m/s3] | double | Avoidance path gets sharp up to this jerk limit when there is not enough distance from ego. | 1.0 | -| max_lateral_acceleration | [m/s3] | double | Avoidance path gets sharp up to this accel limit when there is not enough distance from ego. | 0.5 | - -namespace: `avoidance.constraints.longitudinal.` - -| Name | Unit | Type | Description | Default value | -| :------------------- | :------ | :----- | :------------------------------------- | :------------ | -| nominal_deceleration | [m/ss] | double | Nominal deceleration limit. | -1.0 | -| nominal_jerk | [m/sss] | double | Nominal jerk limit. | 0.5 | -| max_deceleration | [m/ss] | double | Max decelerate limit. | -2.0 | -| max_jerk | [m/sss] | double | Max jerk limit. | 1.0 | -| max_acceleration | [m/ss] | double | Maximum acceleration during avoidance. | 1.0 | - -(\*2) If there are multiple vehicles in a row to be avoided, no new avoidance path will be generated unless their lateral margin difference exceeds this value. +{{ json_to_markdown("planning/behavior_path_avoidance_module/schema/avoidance.schema.json") }} ## Future extensions / Unimplemented parts diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 92f533bc63872..dc42af0412254 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -28,7 +28,6 @@ hard_margin_for_parked_vehicle: 0.7 # [m] max_expand_ratio: 0.0 # [-] FOR DEVELOPER envelope_buffer_margin: 0.5 # [m] FOR DEVELOPER - use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: th_moving_speed: 1.0 th_moving_time: 2.0 @@ -39,7 +38,6 @@ hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - use_conservative_buffer_longitudinal: true bus: th_moving_speed: 1.0 th_moving_time: 2.0 @@ -50,7 +48,6 @@ hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - use_conservative_buffer_longitudinal: true trailer: th_moving_speed: 1.0 th_moving_time: 2.0 @@ -61,7 +58,6 @@ hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - use_conservative_buffer_longitudinal: true unknown: th_moving_speed: 0.28 th_moving_time: 1.0 @@ -72,7 +68,6 @@ hard_margin_for_parked_vehicle: -0.2 max_expand_ratio: 0.0 envelope_buffer_margin: 0.1 - use_conservative_buffer_longitudinal: true bicycle: th_moving_speed: 0.28 th_moving_time: 1.0 @@ -83,7 +78,6 @@ hard_margin_for_parked_vehicle: 0.5 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - use_conservative_buffer_longitudinal: true motorcycle: th_moving_speed: 1.0 th_moving_time: 1.0 @@ -94,7 +88,6 @@ hard_margin_for_parked_vehicle: 0.3 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - use_conservative_buffer_longitudinal: true pedestrian: th_moving_speed: 0.28 th_moving_time: 1.0 @@ -105,7 +98,6 @@ hard_margin_for_parked_vehicle: 0.5 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - use_conservative_buffer_longitudinal: true lower_distance_for_polygon_expansion: 30.0 # [m] FOR DEVELOPER upper_distance_for_polygon_expansion: 100.0 # [m] FOR DEVELOPER @@ -140,6 +132,10 @@ th_shiftable_ratio: 0.8 # [-] min_road_shoulder_width: 0.5 # [m] FOR DEVELOPER + # for merging/deviating vehicle + merging_vehicle: + th_overhang_distance: 0.0 # [m] + # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. avoidance_for_ambiguous_vehicle: enable: true # [-] @@ -222,6 +218,8 @@ min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] FOR DEVELOPER nominal_avoidance_speed: 8.33 # [m/s] FOR DEVELOPER + consider_front_overhang: true # [-] + consider_rear_overhang: true # [-] # return dead line return_dead_line: goal: @@ -296,4 +294,3 @@ # for debug debug: marker: false - console: false diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index bc66598d3ee90..58ada91df9e96 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -76,8 +76,6 @@ struct ObjectParameter double lateral_hard_margin_for_parked_vehicle{1.0}; double longitudinal_margin{0.0}; - - bool use_conservative_buffer_longitudinal{true}; }; struct AvoidanceParameters @@ -191,13 +189,8 @@ struct AvoidanceParameters double time_threshold_for_ambiguous_vehicle{0.0}; double distance_threshold_for_ambiguous_vehicle{0.0}; - // when complete avoidance motion, there is a distance margin with the object - // for longitudinal direction - double longitudinal_collision_margin_min_distance{0.0}; - - // when complete avoidance motion, there is a time margin with the object - // for longitudinal direction - double longitudinal_collision_margin_time{0.0}; + // for merging/deviating vehicle + double th_overhang_distance{0.0}; // parameters for safety check area bool enable_safety_check{false}; @@ -219,6 +212,9 @@ struct AvoidanceParameters size_t hysteresis_factor_safe_count; double hysteresis_factor_expand_rate{0.0}; + bool consider_front_overhang{true}; + bool consider_rear_overhang{true}; + // maximum stop distance double stop_max_distance{0.0}; @@ -329,7 +325,6 @@ struct AvoidanceParameters // debug bool publish_debug_marker = false; - bool print_debug_info = false; }; struct ObjectData // avoidance target diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 2e481e7c98e44..8d95e724045ba 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -148,16 +148,19 @@ class AvoidanceHelper { const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front - : 0.0; - return object_parameter.longitudinal_margin + additional_buffer_longitudinal; + if (!parameters_->consider_front_overhang) { + return object_parameter.longitudinal_margin; + } + return object_parameter.longitudinal_margin + data_->parameters.base_link2front; } double getRearConstantDistance(const ObjectData & object) const { const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); + if (!parameters_->consider_rear_overhang) { + return object_parameter.longitudinal_margin; + } return object_parameter.longitudinal_margin + data_->parameters.base_link2rear + object.length; } 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 index 5b8963c7ca22d..25101537850ae 100644 --- 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 @@ -70,8 +70,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node) param.lateral_hard_margin_for_parked_vehicle = getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); param.longitudinal_margin = getOrDeclareParameter(*node, ns + "longitudinal_margin"); - param.use_conservative_buffer_longitudinal = - getOrDeclareParameter(*node, ns + "use_conservative_buffer_longitudinal"); return param; }; @@ -133,6 +131,11 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "min_road_shoulder_width"); } + { + const std::string ns = "avoidance.target_filtering.merging_vehicle."; + p.th_overhang_distance = getOrDeclareParameter(*node, ns + "th_overhang_distance"); + } + { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); @@ -270,6 +273,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); p.nominal_avoidance_speed = getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); + p.consider_front_overhang = getOrDeclareParameter(*node, ns + "consider_front_overhang"); + p.consider_rear_overhang = getOrDeclareParameter(*node, ns + "consider_rear_overhang"); } // avoidance maneuver (return shift dead line) @@ -379,7 +384,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { const std::string ns = "avoidance.debug."; p.publish_debug_marker = getOrDeclareParameter(*node, ns + "marker"); - p.print_debug_info = getOrDeclareParameter(*node, ns + "console"); } return p; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 899ec99cb0d3b..298406c44cf27 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -424,9 +424,6 @@ class AvoidanceModule : public SceneModuleInterface // TODO(Satoshi OTA) create detected object manager. ObjectDataArray registered_objects_; - // TODO(Satoshi OTA) remove mutable. - mutable ObjectDataArray detected_objects_; - // TODO(Satoshi OTA) remove this variable. mutable ObjectDataArray ego_stopped_objects_; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index 3f1d3495e51cb..d120aab4d6943 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -31,6 +31,9 @@ using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPoly using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; +static constexpr const char * logger_namespace = + "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance.utils"; + bool isOnRight(const ObjectData & obj); double calcShiftLength( @@ -112,8 +115,6 @@ void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters); -void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects); - void updateRegisteredObject( ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, const std::shared_ptr & parameters); diff --git a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json new file mode 100644 index 0000000000000..7c0205d2473e5 --- /dev/null +++ b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json @@ -0,0 +1,1445 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for behavior_path_avoidance_module", + "type": "object", + "definitions": { + "behavior_path_avoidance_module": { + "type": "object", + "properties": { + "resample_interval_for_planning": { + "type": "number", + "description": "Path resample interval for avoidance planning path.", + "default": "0.3" + }, + "resample_interval_for_output": { + "type": "number", + "description": "Path resample interval for output path. Too short interval increases computational cost for latter modules.", + "default": "4.0" + }, + "enable_bound_clipping": { + "type": "boolean", + "description": "Enable clipping left and right bound of drivable area when obstacles are in the drivable area.", + "default": "false" + }, + "disable_path_update": { + "type": "boolean", + "description": "Disable path update.", + "default": "false" + }, + "use_adjacent_lane": { + "type": "boolean", + "description": "Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane.", + "default": "true" + }, + "use_opposite_lane": { + "type": "boolean", + "description": "Extend avoidance trajectory to opposite direction lane. `use_adjacent_lane` must be `true` to take effects.", + "default": "true" + }, + "use_hatched_road_markings": { + "type": "boolean", + "description": "Extend drivable to hatched road marking area.", + "default": "true" + }, + "use_intersection_areas": { + "type": "boolean", + "description": "Extend drivable to intersection area.", + "default": "true" + }, + "use_freespace_areas": { + "type": "boolean", + "description": "Extend drivable to freespace area.", + "default": "true" + }, + "target_object": { + "type": "object", + "properties": { + "car": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.3 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.2 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.5 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "truck": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.3 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.2 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.5 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "bus": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.3 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.2 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.5 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "trailer": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.3 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.2 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.5 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "unknown": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": -0.2 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": -0.2 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.1 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "motorcycle": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.5 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.5 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.5 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "bicycle": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.3 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.3 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.5 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "pedestrian": { + "type": "object", + "properties": { + "th_moving_speed": { + "type": "number", + "description": "Objects with speed greater than this will be judged as moving ones.", + "default": 1.0, + "minimum": 0.0 + }, + "th_moving_time": { + "type": "number", + "description": "Objects keep moving longer duration than this will be excluded from avoidance target.", + "default": 1.0 + }, + "longitudinal_margin": { + "type": "number", + "description": "Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle.", + "default": 0.0 + }, + "lateral_margin": { + "type": "object", + "properties": { + "soft_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.7 + }, + "hard_margin": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.5 + }, + "hard_margin_for_parked_vehicle": { + "type": "number", + "description": "Lateral distance between ego and avoidance targets.", + "default": 0.5 + } + }, + "required": ["soft_margin", "hard_margin", "hard_margin_for_parked_vehicle"], + "additionalProperties": false + }, + "envelope_buffer_margin": { + "type": "number", + "description": "The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation.", + "default": 0.5 + }, + "max_expand_ratio": { + "type": "number", + "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", + "default": 0.0 + } + }, + "required": [ + "th_moving_speed", + "th_moving_time", + "longitudinal_margin", + "lateral_margin", + "envelope_buffer_margin", + "max_expand_ratio" + ], + "additionalProperties": false + }, + "lower_distance_for_polygon_expansion": { + "type": "number", + "description": "If the distance between the ego and object is less than this, the expand ratio will be zero.", + "default": 30.0 + }, + "upper_distance_for_polygon_expansion": { + "type": "number", + "description": "If the distance between the ego and object is larger than this, the expand ratio will be max_expand_ratio.", + "default": 100.0 + } + }, + "required": [ + "car", + "truck", + "bus", + "trailer", + "unknown", + "bicycle", + "motorcycle", + "pedestrian", + "lower_distance_for_polygon_expansion", + "upper_distance_for_polygon_expansion" + ], + "additionalProperties": false + }, + "target_filtering": { + "type": "object", + "properties": { + "target_type": { + "type": "object", + "properties": { + "car": { + "type": "boolean", + "description": "Enable avoidance maneuver for CAR.", + "default": "true" + }, + "truck": { + "type": "boolean", + "description": "Enable avoidance maneuver for TRUCK.", + "default": "true" + }, + "bus": { + "type": "boolean", + "description": "Enable avoidance maneuver for BUS.", + "default": "true" + }, + "trailer": { + "type": "boolean", + "description": "Enable avoidance maneuver for TRAILER.", + "default": "true" + }, + "unknown": { + "type": "boolean", + "description": "Enable avoidance maneuver for UNKNOWN.", + "default": "true" + }, + "bicycle": { + "type": "boolean", + "description": "Enable avoidance maneuver for BICYCLE.", + "default": "true" + }, + "motorcycle": { + "type": "boolean", + "description": "Enable avoidance maneuver for MOTORCYCLE.", + "default": "true" + }, + "pedestrian": { + "type": "boolean", + "description": "Enable avoidance maneuver for PEDESTRIAN.", + "default": "true" + } + }, + "required": [ + "car", + "truck", + "bus", + "trailer", + "unknown", + "bicycle", + "motorcycle", + "pedestrian" + ], + "additionalProperties": false + }, + "object_check_goal_distance": { + "type": "number", + "description": "If the distance between object and goal position is less than this parameter, the module do not return center line.", + "default": 20.0 + }, + "object_check_return_pose_distance": { + "type": "number", + "description": "If the distance between object and return position is less than this parameter, the module do not return center line.", + "default": 20.0 + }, + "max_compensation_time": { + "type": "number", + "description": "For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit.", + "default": 2.0 + }, + "detection_area": { + "type": "object", + "properties": { + "static": { + "type": "boolean", + "description": "If true, the detection area longitudinal range is calculated based on current ego speed.", + "default": "false" + }, + "min_forward_distance": { + "type": "number", + "description": "Minimum forward distance to search the avoidance target.", + "default": 50.0 + }, + "max_forward_distance": { + "type": "number", + "description": "Maximum forward distance to search the avoidance target.", + "default": 150.0 + }, + "backward_distance": { + "type": "number", + "description": "Backward distance to search the avoidance target.", + "default": 10.0 + } + }, + "required": [ + "static", + "min_forward_distance", + "max_forward_distance", + "backward_distance" + ], + "additionalProperties": false + }, + "merging_vehicle": { + "type": "object", + "properties": { + "th_overhang_distance": { + "type": "number", + "description": "Distance threshold to ignore merging/deviating vehicle to/from ego driving lane. The distance represents how the object polygon overlaps ego lane, and it's calculated from polygon overhang point and lane centerline. If the distance is more than this param, the module never avoid the object. (Basically, the ego stops behind of it.)", + "default": 0.5 + } + }, + "required": ["th_overhang_distance"], + "additionalProperties": false + }, + "parked_vehicle": { + "type": "object", + "properties": { + "th_offset_from_centerline": { + "type": "number", + "description": "Vehicles around the center line within this distance will be excluded from avoidance target.", + "default": 1.0 + }, + "th_shiftable_ratio": { + "type": "number", + "description": "Vehicles around the center line within this distance will be excluded from avoidance target.", + "default": 0.8, + "minimum": 0.0, + "maximum": 1.0 + }, + "min_road_shoulder_width": { + "type": "number", + "description": "Width considered as a road shoulder if the lane does not have a road shoulder target.", + "default": 0.5 + } + }, + "required": [ + "th_offset_from_centerline", + "th_shiftable_ratio", + "min_road_shoulder_width" + ], + "additionalProperties": false + }, + "avoidance_for_ambiguous_vehicle": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "Enable avoidance maneuver for ambiguous vehicles.", + "default": "true" + }, + "closest_distance_to_wait_and_see": { + "type": "number", + "description": "Start avoidance maneuver after the distance to ambiguous vehicle is less than this param.", + "default": 10.0 + }, + "condition": { + "type": "object", + "properties": { + "th_stopped_time": { + "type": "number", + "description": "Never avoid object whose stopped time is less than this param.", + "default": 3.0 + }, + "th_moving_distance": { + "type": "number", + "description": "Never avoid object which moves more than this param.", + "default": 1.0 + } + }, + "required": ["th_stopped_time", "th_moving_distance"], + "additionalProperties": false + }, + "ignore_area": { + "type": "object", + "properties": { + "traffic_light": { + "type": "object", + "properties": { + "front_distance": { + "type": "number", + "description": "If the distance between traffic light and vehicle is less than this parameter, this module will ignore it.", + "default": 100.0 + } + }, + "required": ["front_distance"], + "additionalProperties": false + }, + "crosswalk": { + "type": "object", + "properties": { + "front_distance": { + "type": "number", + "description": "If the front distance between crosswalk and vehicle is less than this parameter, this module will ignore it.", + "default": 30.0 + }, + "behind_distance": { + "type": "number", + "description": "If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it.", + "default": 30.0 + } + }, + "required": ["front_distance", "behind_distance"], + "additionalProperties": false + } + }, + "required": ["traffic_light", "crosswalk"], + "additionalProperties": false + } + }, + "required": [ + "enable", + "closest_distance_to_wait_and_see", + "condition", + "ignore_area" + ], + "additionalProperties": false + }, + "intersection": { + "type": "object", + "properties": { + "yaw_deviation": { + "type": "number", + "description": "Yaw deviation threshold param to judge if the object is not merging or deviating vehicle.", + "default": 0.349 + } + }, + "required": ["yaw_deviation"], + "additionalProperties": false + } + }, + "required": [ + "target_type", + "object_check_goal_distance", + "object_check_return_pose_distance", + "max_compensation_time", + "detection_area", + "merging_vehicle", + "parked_vehicle", + "avoidance_for_ambiguous_vehicle", + "intersection" + ], + "additionalProperties": false + }, + "safety_check": { + "type": "object", + "properties": { + "target_type": { + "type": "object", + "properties": { + "car": { + "type": "boolean", + "description": "Enable safety_check for CAR.", + "default": "true" + }, + "truck": { + "type": "boolean", + "description": "Enable safety_check for TRUCK.", + "default": "true" + }, + "bus": { + "type": "boolean", + "description": "Enable safety_check for BUS.", + "default": "true" + }, + "trailer": { + "type": "boolean", + "description": "Enable safety_check for TRAILER.", + "default": "true" + }, + "unknown": { + "type": "boolean", + "description": "Enable safety_check for UNKNOWN.", + "default": "false" + }, + "bicycle": { + "type": "boolean", + "description": "Enable safety_check for BICYCLE.", + "default": "true" + }, + "motorcycle": { + "type": "boolean", + "description": "Enable safety_check for MOTORCYCLE.", + "default": "true" + }, + "pedestrian": { + "type": "boolean", + "description": "Enable safety_check for PEDESTRIAN.", + "default": "true" + } + }, + "required": [ + "car", + "truck", + "bus", + "trailer", + "unknown", + "bicycle", + "motorcycle", + "pedestrian" + ], + "additionalProperties": false + }, + "enable": { + "type": "boolean", + "description": "Enable to use safety check feature.", + "default": "true" + }, + "check_current_lane": { + "type": "boolean", + "description": "Check objects on current driving lane.", + "default": "true" + }, + "check_shift_side_lane": { + "type": "boolean", + "description": "Check objects on shift side lane.", + "default": "true" + }, + "check_other_side_lane": { + "type": "boolean", + "description": "Check objects on other side lane.", + "default": "true" + }, + "check_unavoidable_object": { + "type": "boolean", + "description": "Check collision between ego and unavoidable objects.", + "default": "true" + }, + "check_other_object": { + "type": "boolean", + "description": "Check collision between ego and non avoidance target objects.", + "default": "true" + }, + "check_all_predicted_path": { + "type": "boolean", + "description": "Check all prediction path of safety check target objects.", + "default": "true" + }, + "safety_check_backward_distance": { + "type": "number", + "description": "Backward distance to search the dynamic objects.", + "default": 100.0 + }, + "hysteresis_factor_expand_rate": { + "type": "number", + "description": "Hysteresis factor that be used for chattering prevention.", + "default": 2.0 + }, + "hysteresis_factor_safe_count": { + "type": "integer", + "description": "Hysteresis count that be used for chattering prevention.", + "default": 10 + }, + "min_velocity": { + "type": "number", + "description": "Minimum velocity of the ego vehicle's predicted path.", + "default": 1.38 + }, + "max_velocity": { + "type": "number", + "description": "Maximum velocity of the ego vehicle's predicted path.", + "default": 50.0 + }, + "time_resolution": { + "type": "number", + "description": "Time resolution for the ego vehicle's predicted path.", + "default": 0.5 + }, + "time_horizon_for_front_object": { + "type": "number", + "description": "Time horizon for predicting front objects.", + "default": 3.0 + }, + "time_horizon_for_rear_object": { + "type": "number", + "description": "Time horizon for predicting rear objects.", + "default": 10.0 + }, + "delay_until_departure": { + "type": "number", + "description": "Delay until the ego vehicle departs.", + "default": 1.0 + }, + "extended_polygon_policy": { + "type": "string", + "enum": ["rectangle", "along_path"], + "description": "See https://github.com/autowarefoundation/autoware.universe/pull/6336.", + "default": "along_path" + }, + "expected_front_deceleration": { + "type": "number", + "description": "The front object's maximum deceleration when the front vehicle perform sudden braking.", + "default": -1.0 + }, + "expected_rear_deceleration": { + "type": "number", + "description": "The rear object's maximum deceleration when the rear vehicle perform sudden braking.", + "default": -1.0 + }, + "rear_vehicle_reaction_time": { + "type": "number", + "description": "The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake.", + "default": 2.0 + }, + "rear_vehicle_safety_time_margin": { + "type": "number", + "description": "The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking.", + "default": 1.0 + }, + "lateral_distance_max_threshold": { + "type": "number", + "description": "The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe.", + "default": 2.0 + }, + "longitudinal_distance_min_threshold": { + "type": "number", + "description": "The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe.", + "default": 3.0 + }, + "longitudinal_velocity_delta_time": { + "type": "number", + "description": "The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance)", + "default": 0.0 + } + }, + "required": [ + "target_type", + "enable", + "check_current_lane", + "check_shift_side_lane", + "check_other_side_lane", + "check_unavoidable_object", + "check_other_object", + "check_all_predicted_path", + "safety_check_backward_distance", + "hysteresis_factor_expand_rate", + "hysteresis_factor_safe_count", + "min_velocity", + "max_velocity", + "time_resolution", + "time_horizon_for_front_object", + "time_horizon_for_rear_object", + "delay_until_departure", + "extended_polygon_policy", + "expected_front_deceleration", + "expected_rear_deceleration", + "rear_vehicle_reaction_time", + "rear_vehicle_safety_time_margin", + "lateral_distance_max_threshold", + "longitudinal_distance_min_threshold", + "longitudinal_velocity_delta_time" + ], + "additionalProperties": false + }, + "avoidance": { + "type": "object", + "properties": { + "lateral": { + "type": "object", + "properties": { + "th_avoid_execution": { + "type": "number", + "description": "The lateral distance deviation threshold between the current path and suggested avoidance point to execute avoidance.", + "default": 0.09 + }, + "th_small_shift_length": { + "type": "number", + "description": "The shift lines whose lateral offset is less than this will be applied with other ones.", + "default": 0.101 + }, + "soft_drivable_bound_margin": { + "type": "number", + "description": "Keep distance to drivable bound.", + "default": 0.3 + }, + "hard_drivable_bound_margin": { + "type": "number", + "description": "Keep distance to drivable bound.", + "default": 0.3 + }, + "max_right_shift_length": { + "type": "number", + "description": "Maximum shift length for right direction", + "default": 5.0 + }, + "max_left_shift_length": { + "type": "number", + "description": "Maximum shift length for left direction.", + "default": 5.0 + }, + "max_deviation_from_lane": { + "type": "number", + "description": "Use in validation phase to check if the avoidance path is in drivable area.", + "default": 0.2 + }, + "ratio_for_return_shift_approval": { + "type": "number", + "description": "This parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear.", + "default": 0.5, + "minimum": 0.0, + "maximum": 1.0 + } + }, + "required": [ + "th_avoid_execution", + "th_small_shift_length", + "soft_drivable_bound_margin", + "hard_drivable_bound_margin", + "max_deviation_from_lane", + "ratio_for_return_shift_approval" + ], + "additionalProperties": false + }, + "longitudinal": { + "type": "object", + "properties": { + "min_prepare_time": { + "type": "number", + "description": "Avoidance shift starts from point ahead of this time x ego speed at least.", + "default": 1.0 + }, + "max_prepare_time": { + "type": "number", + "description": "Avoidance shift starts from point ahead of this time x ego speed if possible.", + "default": 2.0 + }, + "min_prepare_distance": { + "type": "number", + "description": "Minimum prepare distance.", + "default": 1.0 + }, + "min_slow_down_speed": { + "type": "number", + "description": "Minimum slow speed for avoidance prepare section.", + "default": 1.38 + }, + "buf_slow_down_speed": { + "type": "number", + "description": "Buffer for controller tracking error. Basically, vehicle always cannot follow velocity profile precisely. Therefore, the module inserts lower speed than target speed that satisfies conditions to avoid object within accel/jerk constraints so that the avoidance path always can be output even if the current speed is a little bit higher than target speed.", + "default": 0.56 + }, + "nominal_avoidance_speed": { + "type": "number", + "description": "Nominal avoidance speed.", + "default": 8.33 + }, + "consider_front_overhang": { + "type": "boolean", + "description": "Flag to consider vehicle front overhang in shift line generation logic.", + "default": true + }, + "consider_rear_overhang": { + "type": "boolean", + "description": "Flag to consider vehicle rear overhang in shift line generation logic.", + "default": true + } + }, + "required": [ + "min_prepare_time", + "max_prepare_time", + "min_prepare_distance", + "min_slow_down_speed", + "buf_slow_down_speed", + "nominal_avoidance_speed", + "consider_front_overhang", + "consider_rear_overhang" + ], + "additionalProperties": false + }, + "return_dead_line": { + "type": "object", + "properties": { + "goal": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "Insert stop point in order to return original lane before reaching goal.", + "default": "true" + }, + "buffer": { + "type": "number", + "description": "Buffer distance to return original lane before reaching goal.", + "default": 3.0 + } + }, + "required": ["enable", "buffer"], + "additionalProperties": false + }, + "traffic_light": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "Insert stop point in order to return original lane before reaching traffic light.", + "default": "true" + }, + "buffer": { + "type": "number", + "description": "Buffer distance to return original lane before reaching traffic light.", + "default": 3.0 + } + }, + "required": ["enable", "buffer"], + "additionalProperties": false + } + }, + "required": ["goal", "traffic_light"], + "additionalProperties": false + } + }, + "required": ["lateral", "longitudinal", "return_dead_line"], + "additionalProperties": false + }, + "stop": { + "type": "object", + "properties": { + "max_distance": { + "type": "number", + "description": "Maximum stop distance in the situation where avoidance maneuver is not approved or in yield maneuver.", + "default": 20.0 + }, + "stop_buffer": { + "type": "number", + "description": "Buffer distance in the situation where avoidance maneuver is not approved or in yield maneuver.", + "default": 1.0 + } + }, + "required": ["max_distance", "stop_buffer"], + "additionalProperties": false + }, + "yield": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "Flag to enable yield maneuver.", + "default": "true" + }, + "enable_during_shifting": { + "type": "boolean", + "description": "Flag to enable yield maneuver during shifting.", + "default": "false" + } + }, + "required": ["enable", "enable_during_shifting"], + "additionalProperties": false + }, + "cancel": { + "type": "object", + "properties": { + "enable": { + "type": "boolean", + "description": "Flag to enable cancel maneuver.", + "default": "true" + } + }, + "required": ["enable"], + "additionalProperties": false + }, + "policy": { + "type": "object", + "properties": { + "make_approval_request": { + "type": "string", + "enum": ["per_shift_line", "per_avoidance_maneuver"], + "description": "policy for rtc request. select `per_shift_line` or `per_avoidance_maneuver`. `per_shift_line`: request approval for each shift line. `per_avoidance_maneuver`: request approval for avoidance maneuver (avoid + return).", + "default": "per_shift_line" + }, + "deceleration": { + "type": "string", + "enum": ["reliable", "best_effort"], + "description": "policy for vehicle slow down behavior. select `best_effort` or `reliable`. `best_effort`: slow down deceleration & jerk are limited by constraints but there is a possibility that the vehicle can't stop in front of the vehicle. `reliable`: insert stop or slow down point with ignoring decel/jerk constraints. make it possible to increase chance to avoid but uncomfortable deceleration maybe happen.", + "default": "best_effort" + }, + "lateral_margin": { + "type": "string", + "enum": ["reliable", "best_effort"], + "description": "policy for voidance lateral margin. select `best_effort` or `reliable`. `best_effort`: output avoidance path with shorten lateral margin when there is no enough longitudinal margin to avoid. `reliable`: module output avoidance path with safe (rtc cooperate) state only when the vehicle can avoid with expected lateral margin.", + "default": "best_effort" + }, + "use_shorten_margin_immediately": { + "type": "boolean", + "description": "if true, module doesn't wait deceleration and outputs avoidance path by best effort margin.", + "default": "true" + } + }, + "required": ["make_approval_request"], + "additionalProperties": false + }, + "constraints": { + "type": "object", + "properties": { + "lateral": { + "type": "object", + "properties": { + "velocity": { + "type": "array", + "description": "Velocity array to decide current lateral accel/jerk limit.", + "default": [1.0, 1.38, 11.1] + }, + "max_accel_values": { + "type": "array", + "description": "Avoidance path gets sharp up to this accel limit when there is not enough distance from ego.", + "default": [0.5, 0.5, 0.5] + }, + "min_jerk_values": { + "type": "array", + "description": "Avoidance path is generated with this jerk when there is enough distance from ego.", + "default": [0.2, 0.2, 0.2] + }, + "max_jerk_values": { + "type": "array", + "description": "Avoidance path gets sharp up to this jerk limit when there is not enough distance from ego.", + "default": [1.0, 1.0, 1.0] + } + }, + "required": ["velocity", "max_accel_values", "min_jerk_values", "max_jerk_values"], + "additionalProperties": false + }, + "longitudinal": { + "type": "object", + "properties": { + "nominal_deceleration": { + "type": "number", + "description": "Nominal deceleration limit.", + "default": -1.0 + }, + "nominal_jerk": { + "type": "number", + "description": "Nominal jerk limit.", + "default": 0.5 + }, + "max_deceleration": { + "type": "number", + "description": "Max deceleration limit.", + "default": -1.5 + }, + "max_jerk": { + "type": "number", + "description": "Max jerk limit.", + "default": 1.0 + }, + "max_acceleration": { + "type": "number", + "description": "Maximum acceleration during avoidance.", + "default": 0.5 + }, + "min_velocity_to_limit_max_acceleration": { + "type": "number", + "description": "If the ego speed is faster than this param, the module applies acceleration limit `max_acceleration`.", + "default": 2.78 + } + }, + "required": [ + "nominal_deceleration", + "nominal_jerk", + "max_deceleration", + "max_jerk", + "max_acceleration", + "min_velocity_to_limit_max_acceleration" + ], + "additionalProperties": false + } + }, + "required": ["lateral", "longitudinal"], + "additionalProperties": false + }, + "shift_line_pipeline": { + "type": "object", + "properties": { + "trim": { + "type": "object", + "properties": { + "quantize_size": { + "type": "number", + "description": "Lateral shift length quantize size.", + "default": 0.1 + }, + "th_similar_grad_1": { + "type": "number", + "description": "Lateral shift length threshold to merge similar gradient shift lines.", + "default": 0.1 + }, + "th_similar_grad_2": { + "type": "number", + "description": "Lateral shift length threshold to merge similar gradient shift lines.", + "default": 0.2 + }, + "th_similar_grad_3": { + "type": "number", + "description": "Lateral shift length threshold to merge similar gradient shift lines.", + "default": 0.5 + } + }, + "required": [ + "quantize_size", + "th_similar_grad_1", + "th_similar_grad_2", + "th_similar_grad_3" + ], + "additionalProperties": false + } + }, + "required": ["trim"], + "additionalProperties": false + }, + "debug": { + "type": "object", + "properties": { + "marker": { + "type": "boolean", + "description": "Publish debug marker.", + "default": "false" + } + }, + "required": ["marker"], + "additionalProperties": false + } + }, + "required": [ + "resample_interval_for_planning", + "resample_interval_for_output", + "enable_bound_clipping", + "use_adjacent_lane", + "use_opposite_lane", + "use_hatched_road_markings", + "use_intersection_areas", + "use_freespace_areas", + "target_object", + "target_filtering", + "safety_check", + "avoidance", + "stop", + "yield", + "cancel", + "policy", + "constraints", + "shift_line_pipeline", + "debug" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "properties": { + "ros__parameters": { + "type": "object", + "properties": { + "avoidance": { + "$ref": "#/definitions/behavior_path_avoidance_module" + } + }, + "required": ["avoidance"], + "additionalProperties": false + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 7d029277fcbc4..4595a77a84b3b 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -578,6 +578,7 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, std::string("TooNearToGoal")); addObjects(data.other_objects, std::string("ParallelToEgoLane")); addObjects(data.other_objects, std::string("MergingToEgoLane")); + addObjects(data.other_objects, std::string("DeviatingFromEgoLane")); addObjects(data.other_objects, std::string("UnstableObject")); addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle")); addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle(wait-and-see)")); diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index 3261214d1b9b6..7772bd9c2ad35 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -59,9 +59,6 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "longitudinal_margin", config.longitudinal_margin); - updateParam( - parameters, ns + "use_conservative_buffer_longitudinal", - config.use_conservative_buffer_longitudinal); }; { @@ -127,6 +124,11 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "backward_distance", p->object_check_backward_distance); } + { + const std::string ns = "avoidance.target_filtering.merging_vehicle."; + updateParam(parameters, ns + "th_overhang_distance", p->th_overhang_distance); + } + { const std::string ns = "avoidance.avoidance.lateral.avoidance_for_ambiguous_vehicle."; updateParam(parameters, ns + "enable", p->enable_avoidance_for_ambiguous_vehicle); @@ -170,6 +172,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "min_prepare_distance", p->min_prepare_distance); updateParam(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed); updateParam(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed); + updateParam(parameters, ns + "consider_front_overhang", p->consider_front_overhang); + updateParam(parameters, ns + "consider_rear_overhang", p->consider_rear_overhang); } { @@ -258,7 +262,6 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "marker", p->publish_debug_marker); - updateParam(parameters, ns + "console", p->print_debug_info); } std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index b16395f825385..b70b606d8f279 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -40,12 +40,6 @@ #include #include -// set as macro so that calling function name will be printed. -// debug print is heavy. turn on only when debugging. -#define DEBUG_PRINT(...) \ - RCLCPP_DEBUG_EXPRESSION(getLogger(), parameters_->print_debug_info, __VA_ARGS__) -#define printShiftLines(p, msg) DEBUG_PRINT("[%s] %s", msg, toStrInfo(p).c_str()) - namespace behavior_path_planner { @@ -109,7 +103,7 @@ AvoidanceModule::AvoidanceModule( bool AvoidanceModule::isExecutionRequested() const { - DEBUG_PRINT("AVOIDANCE isExecutionRequested"); + RCLCPP_DEBUG(getLogger(), "AVOIDANCE isExecutionRequested"); // Check ego is in preferred lane updateInfoMarker(avoid_data_); @@ -132,7 +126,11 @@ bool AvoidanceModule::isExecutionRequested() const bool AvoidanceModule::isExecutionReady() const { - DEBUG_PRINT("AVOIDANCE isExecutionReady"); + RCLCPP_DEBUG_STREAM(getLogger(), "---Avoidance GO/NO-GO status---"); + RCLCPP_DEBUG_STREAM(getLogger(), std::boolalpha << "SAFE:" << avoid_data_.safe); + RCLCPP_DEBUG_STREAM(getLogger(), std::boolalpha << "COMFORTABLE:" << avoid_data_.comfortable); + RCLCPP_DEBUG_STREAM(getLogger(), std::boolalpha << "VALID:" << avoid_data_.valid); + RCLCPP_DEBUG_STREAM(getLogger(), std::boolalpha << "READY:" << avoid_data_.ready); return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready; } @@ -412,9 +410,6 @@ ObjectData AvoidanceModule::createObjectData( // Calc moving time. utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, parameters_); - // Fill init pose. - utils::avoidance::fillInitialPose(object_data, detected_objects_); - // Calc lateral deviation from path to target object. object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 ? Direction::LEFT @@ -1139,7 +1134,7 @@ void AvoidanceModule::addNewShiftLines( const auto new_shift_length = front_new_shift_line.end_shift_length; const auto new_shift_end_idx = front_new_shift_line.end_idx; - DEBUG_PRINT("min_start_idx = %lu", min_start_idx); + RCLCPP_DEBUG(getLogger(), "min_start_idx = %lu", min_start_idx); // Remove shift points that starts later than the new_shift_line from path_shifter. // @@ -1152,8 +1147,9 @@ void AvoidanceModule::addNewShiftLines( // farther avoidance. for (const auto & sl : current_shift_lines) { if (sl.start_idx >= min_start_idx) { - DEBUG_PRINT( - "sl.start_idx = %lu, this sl starts after new proposal. remove this one.", sl.start_idx); + RCLCPP_DEBUG( + getLogger(), "sl.start_idx = %lu, this sl starts after new proposal. remove this one.", + sl.start_idx); continue; } @@ -1171,7 +1167,7 @@ void AvoidanceModule::addNewShiftLines( } } - DEBUG_PRINT("sl.start_idx = %lu, no conflict. keep this one.", sl.start_idx); + RCLCPP_DEBUG(getLogger(), "sl.start_idx = %lu, no conflict. keep this one.", sl.start_idx); future.push_back(sl); } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index c97d21ca552eb..b711dcb91236d 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -418,33 +418,41 @@ bool isParkedVehicle( bool is_left_side_parked_vehicle = false; if (!isOnRight(object)) { - auto [object_shiftable_distance, sub_type] = [&]() { - const auto most_left_road_lanelet = - route_handler->getMostLeftLanelet(object.overhang_lanelet); - const auto most_left_lanelet_candidates = - route_handler->getLaneletMapPtr()->laneletLayer.findUsages( - most_left_road_lanelet.leftBound()); - - lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute sub_type = - most_left_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_left_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_left_lanelet = ll; - } + const auto most_left_lanelet = [&]() { + auto same_direction_lane = + route_handler->getMostLeftLanelet(object.overhang_lanelet, true, true); + const lanelet::Attribute & sub_type = + same_direction_lane.attribute(lanelet::AttributeName::Subtype); + if (sub_type == "road_shoulder") { + return same_direction_lane; } - const auto center_to_left_boundary = distance2d( - to2D(most_left_lanelet.leftBound().basicLineString()), - to2D(toLaneletPoint(centerline_pos)).basicPoint()); + const auto opposite_lanes = route_handler->getLeftOppositeLanelets(same_direction_lane); + if (opposite_lanes.empty()) { + return same_direction_lane; + } - return std::make_pair( - center_to_left_boundary - 0.5 * object.object.shape.dimensions.y, sub_type); + return static_cast(opposite_lanes.front().invert()); }(); - if (sub_type.value() != "road_shoulder") { + const auto center_to_left_boundary = distance2d( + to2D(most_left_lanelet.leftBound().basicLineString()), + to2D(toLaneletPoint(centerline_pos)).basicPoint()); + + double object_shiftable_distance = + center_to_left_boundary - 0.5 * object.object.shape.dimensions.y; + + const lanelet::Attribute & sub_type = + most_left_lanelet.attribute(lanelet::AttributeName::Subtype); + if (sub_type == "road_shoulder") { + // assuming it's parked vehicle if its CoG is within road shoulder lanelet. + if (boost::geometry::within( + to2D(toLaneletPoint(object_pos)).basicPoint(), + most_left_lanelet.polygon2d().basicPolygon())) { + return true; + } + } else { + // assuming there is 0.5m road shoulder even if it's not defined explicitly in HDMap. object_shiftable_distance += parameters->object_check_min_road_shoulder_width; } @@ -458,33 +466,41 @@ bool isParkedVehicle( bool is_right_side_parked_vehicle = false; if (isOnRight(object)) { - auto [object_shiftable_distance, sub_type] = [&]() { - const auto most_right_road_lanelet = - route_handler->getMostRightLanelet(object.overhang_lanelet); - const auto most_right_lanelet_candidates = - route_handler->getLaneletMapPtr()->laneletLayer.findUsages( - most_right_road_lanelet.rightBound()); - - lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute sub_type = - most_right_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_right_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_right_lanelet = ll; - } + const auto most_right_lanelet = [&]() { + auto same_direction_lane = + route_handler->getMostRightLanelet(object.overhang_lanelet, true, true); + const lanelet::Attribute & sub_type = + same_direction_lane.attribute(lanelet::AttributeName::Subtype); + if (sub_type == "road_shoulder") { + return same_direction_lane; } - const auto center_to_right_boundary = distance2d( - to2D(most_right_lanelet.rightBound().basicLineString()), - to2D(toLaneletPoint(centerline_pos)).basicPoint()); + const auto opposite_lanes = route_handler->getRightOppositeLanelets(same_direction_lane); + if (opposite_lanes.empty()) { + return same_direction_lane; + } - return std::make_pair( - center_to_right_boundary - 0.5 * object.object.shape.dimensions.y, sub_type); + return static_cast(opposite_lanes.front().invert()); }(); - if (sub_type.value() != "road_shoulder") { + const auto center_to_right_boundary = distance2d( + to2D(most_right_lanelet.rightBound().basicLineString()), + to2D(toLaneletPoint(centerline_pos)).basicPoint()); + + double object_shiftable_distance = + center_to_right_boundary - 0.5 * object.object.shape.dimensions.y; + + const lanelet::Attribute & sub_type = + most_right_lanelet.attribute(lanelet::AttributeName::Subtype); + if (sub_type == "road_shoulder") { + // assuming it's parked vehicle if its CoG is within road shoulder lanelet. + if (boost::geometry::within( + to2D(toLaneletPoint(object_pos)).basicPoint(), + most_right_lanelet.polygon2d().basicPolygon())) { + return true; + } + } else { + // assuming there is 0.5m road shoulder even if it's not defined explicitly in HDMap. object_shiftable_distance += parameters->object_check_min_road_shoulder_width; } @@ -551,25 +567,58 @@ bool isNeverAvoidanceTarget( const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - const auto is_moving_distance_longer_than_threshold = - tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) > - parameters->distance_threshold_for_ambiguous_vehicle; - if (is_moving_distance_longer_than_threshold) { - object.reason = AvoidanceDebugFactor::MOVING_OBJECT; - return true; - } - if (object.is_within_intersection) { if (object.behavior == ObjectData::Behavior::NONE) { object.reason = "ParallelToEgoLane"; - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it."); + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object belongs to ego lane. never avoid it."); return true; } if (object.behavior == ObjectData::Behavior::MERGING) { object.reason = "MergingToEgoLane"; - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it."); + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object belongs to ego lane. never avoid it."); + return true; + } + } + + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; + if ( + isOnRight(object) && !object.is_parked && + object.overhang_points.front().first > parameters->th_overhang_distance) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), + "merging vehicle. but overhang distance is larger than threshold."); + return true; + } + if ( + !isOnRight(object) && !object.is_parked && + object.overhang_points.front().first < -1.0 * parameters->th_overhang_distance) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), + "merging vehicle. but overhang distance is larger than threshold."); + return true; + } + } + + if (object.behavior == ObjectData::Behavior::DEVIATING) { + object.reason = "DeviatingFromEgoLane"; + if ( + isOnRight(object) && !object.is_parked && + object.overhang_points.front().first > parameters->th_overhang_distance) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), + "deviating vehicle. but overhang distance is larger than threshold."); + return true; + } + if ( + !isOnRight(object) && !object.is_parked && + object.overhang_points.front().first < -1.0 * parameters->th_overhang_distance) { + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), + "deviating vehicle. but overhang distance is larger than threshold."); return true; } } @@ -581,7 +630,8 @@ bool isNeverAvoidanceTarget( planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, false); if (right_lane.has_value() && left_lane.has_value()) { object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane. never avoid it."); + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it."); return true; } } @@ -589,7 +639,8 @@ bool isNeverAvoidanceTarget( if (isCloseToStopFactor(object, data, planner_data, parameters)) { if (object.is_on_ego_lane && !object.is_parked) { object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is close to stop factor. never avoid it."); + RCLCPP_DEBUG( + rclcpp::get_logger(logger_namespace), "object is close to stop factor. never avoid it."); return true; } } @@ -604,12 +655,12 @@ bool isObviousAvoidanceTarget( { if (!object.is_within_intersection) { if (object.is_parked && object.behavior == ObjectData::Behavior::NONE) { - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is obvious parked vehicle."); + RCLCPP_DEBUG(rclcpp::get_logger(logger_namespace), "object is obvious parked vehicle."); return true; } if (!object.is_on_ego_lane && object.behavior == ObjectData::Behavior::NONE) { - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is adjacent vehicle."); + RCLCPP_DEBUG(rclcpp::get_logger(logger_namespace), "object is adjacent vehicle."); return true; } } @@ -748,6 +799,15 @@ bool isSatisfiedWithVehicleCondition( return false; } + const auto & current_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto is_moving_distance_longer_than_threshold = + calcDistance2d(object.init_pose, current_pose) > + parameters->distance_threshold_for_ambiguous_vehicle; + if (is_moving_distance_longer_than_threshold) { + object.reason = "AmbiguousStoppedVehicle"; + return false; + } + if (object.is_within_intersection) { if (object.behavior == ObjectData::Behavior::DEVIATING) { object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; @@ -874,8 +934,7 @@ double getRoadShoulderDistance( } { - const auto p2 = - calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -100.0 : 100.0), 0.0).position; + const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -1.0 : 1.0), 0.0).position; const auto opt_intersect = tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); @@ -1381,6 +1440,7 @@ void fillObjectMovingTime( object_data.last_stop = now; object_data.move_time = 0.0; if (is_new_object) { + object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; object_data.stop_time = 0.0; object_data.last_move = now; stopped_objects.push_back(object_data); @@ -1389,11 +1449,13 @@ void fillObjectMovingTime( same_id_obj->last_stop = now; same_id_obj->move_time = 0.0; object_data.stop_time = same_id_obj->stop_time; + object_data.init_pose = same_id_obj->init_pose; } return; } if (is_new_object) { + object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; object_data.move_time = std::numeric_limits::infinity(); object_data.stop_time = 0.0; object_data.last_move = now; @@ -1403,6 +1465,7 @@ void fillObjectMovingTime( object_data.last_stop = same_id_obj->last_stop; object_data.move_time = (now - same_id_obj->last_stop).seconds(); object_data.stop_time = 0.0; + object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; if (object_data.move_time > object_parameter.moving_time_threshold) { stopped_objects.erase(same_id_obj); @@ -1449,22 +1512,6 @@ void fillAvoidanceNecessity( object_data.avoid_required = check_necessity(parameters->hysteresis_factor_expand_rate); } -void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects) -{ - const auto id = object_data.object.object_id; - const auto same_id_obj = std::find_if( - detected_objects.begin(), detected_objects.end(), - [&id](const auto & o) { return o.object.object_id == id; }); - - if (same_id_obj != detected_objects.end()) { - object_data.init_pose = same_id_obj->init_pose; - return; - } - - object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; - detected_objects.push_back(object_data); -} - void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters) @@ -2188,8 +2235,7 @@ DrivableLanes generateExpandedDrivableLanes( } if (i == max_recursive_search_num - 1) { RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), - "Drivable area expansion reaches max iteration."); + rclcpp::get_logger(logger_namespace), "Drivable area expansion reaches max iteration."); } } }; diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index f3f6870085e02..2c181489ba4c2 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -1,6 +1,5 @@ /**: ros__parameters: - verbose: false max_iteration_num: 100 traffic_light_signal_timeout: 1.0 planning_hz: 10.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 9054435a33912..51a4cddabb941 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -96,7 +96,7 @@ struct SceneModuleStatus class PlannerManager { public: - PlannerManager(rclcpp::Node & node, const size_t max_iteration_num, const bool verbose); + PlannerManager(rclcpp::Node & node, const size_t max_iteration_num); /** * @brief run all candidate and approved modules. @@ -471,8 +471,6 @@ class PlannerManager mutable std::shared_ptr debug_msg_ptr_; size_t max_iteration_num_{100}; - - bool verbose_{false}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 6490380565b53..044a77f200d7f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -128,7 +128,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const std::lock_guard lock(mutex_manager_); // for planner_manager_ const auto & p = planner_data_->parameters; - planner_manager_ = std::make_shared(*this, p.max_iteration_num, p.verbose); + planner_manager_ = std::make_shared(*this, p.max_iteration_num); for (const auto & name : declare_parameter>("launch_modules")) { // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. @@ -203,7 +203,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() { BehaviorPathPlannerParameters p{}; - p.verbose = declare_parameter("verbose"); p.max_iteration_num = declare_parameter("max_iteration_num"); p.traffic_light_signal_timeout = declare_parameter("traffic_light_signal_timeout"); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 6ac67605839e1..418a70cb269d6 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -30,13 +30,11 @@ namespace behavior_path_planner { -PlannerManager::PlannerManager( - rclcpp::Node & node, const size_t max_iteration_num, const bool verbose) +PlannerManager::PlannerManager(rclcpp::Node & node, const size_t max_iteration_num) : plugin_loader_("behavior_path_planner", "behavior_path_planner::SceneModuleManagerInterface"), logger_(node.get_logger().get_child("planner_manager")), clock_(*node.get_clock()), - max_iteration_num_{max_iteration_num}, - verbose_{verbose} + max_iteration_num_{max_iteration_num} { processing_time_.emplace("total_time", 0.0); debug_publisher_ptr_ = std::make_unique(&node, "~/debug"); @@ -946,11 +944,7 @@ void PlannerManager::print() const state_publisher_ptr_->publish("internal_state", string_stream.str()); - if (!verbose_) { - return; - } - - RCLCPP_INFO_STREAM(logger_, string_stream.str()); + RCLCPP_DEBUG_STREAM(logger_, string_stream.str()); } void PlannerManager::publishProcessingTime() const diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index a2481040f7548..4947a1b544302 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -141,7 +141,13 @@ class SceneModuleInterface virtual BehaviorModuleOutput run() { updateData(); - return isWaitingApproval() ? planWaitingApproval() : plan(); + const auto output = isWaitingApproval() ? planWaitingApproval() : plan(); + try { + motion_utils::validateNonEmpty(output.path.points); + } catch (const std::exception & ex) { + throw std::invalid_argument("[" + name_ + "]" + ex.what()); + } + return output; } /** diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index 1eae4a1c4c345..1241f98daa747 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp @@ -30,7 +30,6 @@ struct ModuleConfigParameters struct BehaviorPathPlannerParameters { - bool verbose; size_t max_iteration_num{100}; double traffic_light_signal_timeout{1.0};