From 3b7fd5866ba5f9dad842a00cc6236f6b1fa2f78a Mon Sep 17 00:00:00 2001 From: Yuxin Wang <87698138+Ariiees@users.noreply.github.com> Date: Thu, 22 Aug 2024 10:29:33 -0400 Subject: [PATCH] refactor(autoware_obstacle_stop_planner): rework parameters (#7795) Signed-off-by: Ariiees --- .../autoware_obstacle_stop_planner/README.md | 6 + .../adaptive_cruise_control.schema.json | 216 ++++++++++++ .../schema/common.schema.json | 85 +++++ .../schema/obstacle_stop_planner.schema.json | 322 ++++++++++++++++++ 4 files changed, 629 insertions(+) create mode 100644 planning/autoware_obstacle_stop_planner/schema/adaptive_cruise_control.schema.json create mode 100644 planning/autoware_obstacle_stop_planner/schema/common.schema.json create mode 100644 planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json diff --git a/planning/autoware_obstacle_stop_planner/README.md b/planning/autoware_obstacle_stop_planner/README.md index 10870222aee09..d3965192cd4c3 100644 --- a/planning/autoware_obstacle_stop_planner/README.md +++ b/planning/autoware_obstacle_stop_planner/README.md @@ -31,6 +31,8 @@ ### Common Parameter +{{ json_to_markdown("planning/autoware_obstacle_stop_planner/schema/common.schema.json") }} + | Parameter | Type | Description | | -------------------------------------- | ------ | ----------------------------------------------------------------------------------------- | | `enable_slow_down` | bool | enable slow down planner [-] | @@ -103,6 +105,8 @@ stopped due to other factors. ### Parameters +{{ json_to_markdown("planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json") }} + #### Stop position | Parameter | Type | Description | @@ -186,6 +190,8 @@ down section. ### Parameters +{{ json_to_markdown("planning/autoware_obstacle_stop_planner/schema/adaptive_cruise_control.schema.json") }} + #### Slow down section | Parameter | Type | Description | diff --git a/planning/autoware_obstacle_stop_planner/schema/adaptive_cruise_control.schema.json b/planning/autoware_obstacle_stop_planner/schema/adaptive_cruise_control.schema.json new file mode 100644 index 0000000000000..25a6a9338987b --- /dev/null +++ b/planning/autoware_obstacle_stop_planner/schema/adaptive_cruise_control.schema.json @@ -0,0 +1,216 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameter for adaptive cruise control", + "type": "object", + "definitions": { + "adaptive_cruise_control": { + "type": "object", + "properties": { + "adaptive_cruise_control": { + "type": "object", + "properties": { + "use_object_to_estimate_vel": { + "type": "boolean", + "description": "use tracking objects for estimating object velocity or not", + "default": "true" + }, + "use_pcl_to_estimate_vel": { + "type": "boolean", + "description": "use pcl for estimating object velocity or not", + "default": "true" + }, + "consider_obj_velocity": { + "type": "boolean", + "description": "consider forward vehicle velocity to ACC or not", + "default": "true" + }, + "obstacle_velocity_thresh_to_start_acc": { + "type": "number", + "description": "start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s]", + "default": "1.5" + }, + "obstacle_velocity_thresh_to_stop_acc": { + "type": "number", + "description": "stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s]", + "default": "1.0" + }, + "emergency_stop_acceleration": { + "type": "number", + "description": "supposed minimum acceleration (deceleration) in emergency stop [m/ss]", + "default": "-5.0" + }, + "emergency_stop_idling_time": { + "type": "number", + "description": "supposed idling time to start emergency stop [s]", + "default": "0.5" + }, + "min_dist_stop": { + "type": "number", + "description": "minimum distance of emergency stop [m]", + "default": "4.0" + }, + "obstacle_emergency_stop_acceleration": { + "type": "number", + "description": "supposed minimum acceleration (deceleration) in emergency stop [m/ss]", + "default": "-5.0" + }, + "max_standard_acceleration": { + "type": "number", + "description": "supposed maximum acceleration in active cruise control [m/ss]", + "default": "0.5" + }, + "min_standard_acceleration": { + "type": "number", + "description": "supposed minimum acceleration (deceleration) in active cruise control", + "default": "-1.0" + }, + "standard_idling_time": { + "type": "number", + "description": "supposed idling time to react object in active cruise control [s]", + "default": "0.5" + }, + "min_dist_standard": { + "type": "number", + "description": "minimum distance in active cruise control [m]", + "default": "4.0" + }, + "obstacle_min_standard_acceleration": { + "type": "number", + "description": "supposed minimum acceleration of forward obstacle [m/ss]", + "default": "-1.5" + }, + "margin_rate_to_change_vel": { + "type": "number", + "description": "margin to insert upper velocity [-]", + "default": "0.3" + }, + "use_time_compensation_to_calc_distance": { + "type": "boolean", + "description": "use time-compensation to calculate distance to forward vehicle", + "default": "true" + }, + "p_coefficient_positive": { + "type": "number", + "description": "coefficient P in PID control (used when target dist -current_dist >=0) [-]", + "default": "0.1" + }, + "p_coefficient_negative": { + "type": "number", + "description": "coefficient P in PID control (used when target dist -current_dist <0) [-]", + "default": "0.3" + }, + "d_coefficient_positive": { + "type": "number", + "description": "coefficient D in PID control (used when delta_dist >=0) [-]", + "default": "0.0" + }, + "d_coefficient_negative": { + "type": "number", + "description": "coefficient D in PID control (used when delta_dist <0) [-]", + "default": "0.2" + }, + "object_polygon_length_margin": { + "type": "number", + "description": "The distance to extend the polygon length the object in pointcloud-object matching [m]", + "default": "2.0" + }, + "object_polygon_width_margin": { + "type": "number", + "description": "The distance to extend the polygon width the object in pointcloud-object matching [m]", + "default": "0.5" + }, + "valid_estimated_vel_diff_time": { + "type": "number", + "description": "Maximum time difference treated as continuous points in speed estimation using a point cloud [s]", + "default": "1.0" + }, + "valid_vel_que_time": { + "type": "number", + "description": "Time width of information used for speed estimation in speed estimation using a point cloud [s]", + "default": "0.5" + }, + "valid_estimated_vel_max": { + "type": "number", + "description": "Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s]", + "default": "20.0" + }, + "valid_estimated_vel_min": { + "type": "number", + "description": "Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s]", + "default": "-20.0" + }, + "thresh_vel_to_stop": { + "type": "number", + "description": "Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s]", + "default": "1.5" + }, + "lowpass_gain_of_upper_velocity": { + "type": "number", + "description": "Lowpass-gain of upper velocity", + "default": "0.75" + }, + "use_rough_velocity_estimation": { + "type": "boolean", + "description": "Use rough estimated velocity if the velocity estimation is failed (#### If this parameter is true, the vehicle may collide with the front car. Be careful. ####)", + "default": "false" + }, + "rough_velocity_rate": { + "type": "number", + "description": "In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value", + "default": "0.9" + } + }, + "required": [ + "use_object_to_estimate_vel", + "use_pcl_to_estimate_vel", + "consider_obj_velocity", + "obstacle_velocity_thresh_to_start_acc", + "obstacle_velocity_thresh_to_stop_acc", + "emergency_stop_acceleration", + "emergency_stop_idling_time", + "min_dist_stop", + "obstacle_emergency_stop_acceleration", + "max_standard_acceleration", + "min_standard_acceleration", + "standard_idling_time", + "min_dist_standard", + "obstacle_min_standard_acceleration", + "margin_rate_to_change_vel", + "use_time_compensation_to_calc_distance", + "p_coefficient_positive", + "p_coefficient_negative", + "d_coefficient_positive", + "d_coefficient_negative", + "object_polygon_length_margin", + "object_polygon_width_margin", + "valid_estimated_vel_diff_time", + "valid_vel_que_time", + "valid_estimated_vel_max", + "valid_estimated_vel_min", + "thresh_vel_to_stop", + "lowpass_gain_of_upper_velocity", + "use_rough_velocity_estimation", + "rough_velocity_rate" + ], + "additionalProperties": false + } + }, + "required": ["adaptive_cruise_control"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/adaptive_cruise_control" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/autoware_obstacle_stop_planner/schema/common.schema.json b/planning/autoware_obstacle_stop_planner/schema/common.schema.json new file mode 100644 index 0000000000000..d8360f48c1617 --- /dev/null +++ b/planning/autoware_obstacle_stop_planner/schema/common.schema.json @@ -0,0 +1,85 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameter for common in autoware_obstacle_stop_planner", + "type": "object", + "definitions": { + "common": { + "type": "object", + "properties": { + "max_vel": { + "type": "number", + "description": "max velocity limit [m/s]", + "default": "11.1" + }, + "normal": { + "type": "object", + "properties": { + "min_acc": { + "type": "number", + "description": "min deceleration [m/ss]", + "default": "-0.5" + }, + "max_acc": { + "type": "number", + "description": "max acceleration [m/ss]", + "default": "1.0" + }, + "min_jerk": { + "type": "number", + "description": "min jerk [m/sss]", + "default": "-0.5" + }, + "max_jerk": { + "type": "number", + "description": "max jerk [m/sss]", + "default": "1.0" + } + }, + "required": ["min_acc", "max_acc", "min_jerk", "max_jerk"] + }, + "limit": { + "type": "object", + "properties": { + "min_acc": { + "type": "number", + "description": "min deceleration limit [m/ss]", + "default": "-2.5" + }, + "max_acc": { + "type": "number", + "description": "max acceleration [m/ss]", + "default": "1.0" + }, + "min_jerk": { + "type": "number", + "description": "min jerk [m/sss]", + "default": "-1.5" + }, + "max_jerk": { + "type": "number", + "description": "max jerk [m/sss]", + "default": "1.5" + } + }, + "required": ["min_acc", "max_acc", "min_jerk", "max_jerk"] + } + }, + "required": ["max_vel", "normal", "limit"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/common" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json b/planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json new file mode 100644 index 0000000000000..868f669ad2004 --- /dev/null +++ b/planning/autoware_obstacle_stop_planner/schema/obstacle_stop_planner.schema.json @@ -0,0 +1,322 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameter for obstacle stop planner", + "type": "object", + "definitions": { + "obstacle_stop_planner": { + "type": "object", + "properties": { + "chattering_threshold": { + "type": "number", + "description": "even if the obstacle disappears, the stop judgment continues for chattering_threshold [s]", + "default": "0.5" + }, + "max_velocity": { + "type": "number", + "description": "max velocity [m/s]", + "default": "20.0" + }, + "ego_nearest_dist_threshold": { + "type": "number", + "description": "[m]", + "default": "3.0" + }, + "ego_nearest_yaw_threshold": { + "type": "number", + "description": "[rad] = 60 [deg]", + "default": "1.046" + }, + "enable_slow_down": { + "type": "boolean", + "description": "whether to use slow down planner [-]", + "default": "false" + }, + "enable_z_axis_obstacle_filtering": { + "type": "boolean", + "description": "filter obstacles in z axis (height) [-]", + "default": "true" + }, + "z_axis_filtering_buffer": { + "type": "number", + "description": "additional buffer for z axis filtering [m]", + "default": "0.0" + }, + "voxel_grid_x": { + "type": "number", + "description": "voxel grid x parameter for filtering pointcloud [m]", + "default": "0.05" + }, + "voxel_grid_y": { + "type": "number", + "description": "voxel grid y parameter for filtering pointcloud [m]", + "default": "0.05" + }, + "voxel_grid_z": { + "type": "number", + "description": "voxel grid z parameter for filtering pointcloud [m]", + "default": "100000.0" + }, + "use_predicted_objects": { + "type": "boolean", + "description": "whether to use predicted objects [-]", + "default": "false" + }, + "publish_obstacle_polygon": { + "type": "boolean", + "description": "whether to publish obstacle polygon [-]", + "default": "false" + }, + "predicted_object_filtering_threshold": { + "type": "number", + "description": "threshold for filtering predicted objects (valid only publish_obstacle_polygon true) [m]", + "default": "1.5" + }, + "stop_planner": { + "type": "object", + "properties": { + "stop_position": { + "type": "object", + "properties": { + "max_longitudinal_margin": { + "type": "number", + "description": "stop margin distance from obstacle on the path [m]", + "default": "5.0" + }, + "max_longitudinal_margin_behind_goal": { + "type": "number", + "description": "stop margin distance from obstacle behind the goal on the path [m]", + "default": "3.0" + }, + "min_longitudinal_margin": { + "type": "number", + "description": "stop margin distance when any other stop point is inserted in stop margin [m]", + "default": "2.0" + }, + "hold_stop_margin_distance": { + "type": "number", + "description": "the ego keeps stopping if the ego is in this margin [m]", + "default": "0.0" + } + }, + "required": [ + "max_longitudinal_margin", + "max_longitudinal_margin_behind_goal", + "min_longitudinal_margin", + "hold_stop_margin_distance" + ] + }, + "detection_area": { + "type": "object", + "properties": { + "lateral_margin": { + "type": "number", + "description": "margin [m]", + "default": "0.0" + }, + "vehicle_lateral_margin": { + "type": "number", + "description": "margin of vehicle footprint [m]", + "default": "0.0" + }, + "pedestrian_lateral_margin": { + "type": "number", + "description": "margin of pedestrian footprint [m]", + "default": "0.0" + }, + "unknown_lateral_margin": { + "type": "number", + "description": "margin of unknown footprint [m]", + "default": "0.0" + }, + "step_length": { + "type": "number", + "description": "step length for pointcloud search range [m]", + "default": "1.0" + }, + "enable_stop_behind_goal_for_obstacle": { + "type": "boolean", + "description": "enable extend trajectory after goal lane for obstacle detection", + "default": "true" + } + }, + "required": [ + "lateral_margin", + "vehicle_lateral_margin", + "pedestrian_lateral_margin", + "unknown_lateral_margin", + "step_length", + "enable_stop_behind_goal_for_obstacle" + ] + } + }, + "required": ["stop_position", "detection_area"] + }, + "slow_down_planner": { + "type": "object", + "properties": { + "slow_down_section": { + "type": "object", + "properties": { + "longitudinal_forward_margin": { + "type": "number", + "description": "margin distance from slow down point to vehicle front [m]", + "default": "5.0" + }, + "longitudinal_backward_margin": { + "type": "number", + "description": "margin distance from slow down point to vehicle rear [m]", + "default": "5.0" + }, + "longitudinal_margin_span": { + "type": "number", + "description": "fineness param for relaxing slow down margin (use this param if consider_constraints is True) [m/s]", + "default": "-0.1" + }, + "min_longitudinal_forward_margin": { + "type": "number", + "description": "min margin for relaxing slow down margin (use this param if consider_constraints is True) [m/s]", + "default": "1.0" + } + }, + "required": [ + "longitudinal_forward_margin", + "longitudinal_backward_margin", + "longitudinal_margin_span", + "min_longitudinal_forward_margin" + ] + }, + "detection_area": { + "type": "object", + "properties": { + "lateral_margin": { + "type": "number", + "description": "offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]", + "default": "1.0" + }, + "vehicle_lateral_margin": { + "type": "number", + "description": "offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]", + "default": "1.0" + }, + "pedestrian_lateral_margin": { + "type": "number", + "description": "offset from pedestrian side edge for expanding the search area of the surrounding point cloud [m]", + "default": "1.0" + }, + "unknown_lateral_margin": { + "type": "number", + "description": "offset from unknown side edge for expanding the search area of the surrounding point cloud [m]", + "default": "1.0" + } + }, + "required": [ + "lateral_margin", + "vehicle_lateral_margin", + "pedestrian_lateral_margin", + "unknown_lateral_margin" + ] + }, + "target_velocity": { + "type": "object", + "properties": { + "max_slow_down_velocity": { + "type": "number", + "description": "max slow down velocity (use this param if consider_constraints is False)[m/s]", + "default": "1.38" + }, + "min_slow_down_velocity": { + "type": "number", + "description": "offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]", + "default": "0.28" + }, + "slow_down_velocity": { + "type": "number", + "description": "target slow down velocity (use this param if consider_constraints is True)[m/s]", + "default": "1.38" + } + }, + "required": ["max_slow_down_velocity", "min_slow_down_velocity", "slow_down_velocity"] + }, + "constraints": { + "type": "object", + "properties": { + "jerk_min_slow_down": { + "type": "number", + "description": "min slow down jerk constraint [m/sss]", + "default": "-0.3" + }, + "jerk_span": { + "type": "number", + "description": "fineness param for planning deceleration jerk [m/sss]", + "default": "-0.01" + }, + "jerk_start": { + "type": "number", + "description": "init jerk used for deceleration planning [m/sss]", + "default": "-0.1" + } + }, + "required": ["jerk_min_slow_down", "jerk_span", "jerk_start"] + }, + "consider_constraints": { + "type": "boolean", + "description": "set 'True', if no decel plan found under jerk/dec constrains, relax target slow down vel [-]", + "default": "false" + }, + "velocity_threshold_decel_complete": { + "type": "number", + "description": "use for judge whether the ego velocity converges the target slow down velocity [m/s]", + "default": "0.2" + }, + "acceleration_threshold_decel_complete": { + "type": "number", + "description": "use for judge whether the ego velocity converges the target slow down velocity [m/ss]", + "default": "0.1" + } + }, + "required": [ + "slow_down_section", + "detection_area", + "target_velocity", + "constraints", + "consider_constraints", + "velocity_threshold_decel_complete", + "acceleration_threshold_decel_complete" + ] + } + }, + "required": [ + "chattering_threshold", + "max_velocity", + "ego_nearest_dist_threshold", + "ego_nearest_yaw_threshold", + "enable_slow_down", + "enable_z_axis_obstacle_filtering", + "z_axis_filtering_buffer", + "voxel_grid_x", + "voxel_grid_y", + "voxel_grid_z", + "use_predicted_objects", + "publish_obstacle_polygon", + "predicted_object_filtering_threshold", + "stop_planner", + "slow_down_planner" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/obstacle_stop_planner" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +}