diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index db998fae00..746d6bd3ba 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -22,7 +22,7 @@ jobs: build-depends-repos: build_depends.repos steps: - name: Cancel previous runs - uses: styfle/cancel-workflow-action@0.9.1 + uses: styfle/cancel-workflow-action@0.10.0 - name: Check out repository uses: actions/checkout@v3 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index ede7eb7ef7..9f08293e43 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -4,7 +4,7 @@ ci: repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.2.0 + rev: v4.3.0 hooks: - id: check-json - id: check-merge-conflict @@ -24,7 +24,7 @@ repos: args: [-c, .markdownlint.yaml, --fix] - repo: https://github.com/pre-commit/mirrors-prettier - rev: v2.6.2 + rev: v2.7.1 hooks: - id: prettier @@ -46,7 +46,7 @@ repos: - id: shellcheck - repo: https://github.com/scop/pre-commit-shfmt - rev: v3.5.0-1 + rev: v3.5.1-1 hooks: - id: shfmt args: [-w, -s, -i=4] @@ -57,7 +57,7 @@ repos: - id: isort - repo: https://github.com/psf/black - rev: 22.3.0 + rev: 22.6.0 hooks: - id: black args: [--line-length=100] diff --git a/README.md b/README.md index b1ff82b201..b22428a818 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # autoware_launcher -## Getting Started +## Getting started ### Using real vehicle @@ -14,7 +14,7 @@ ros2 launch autoware_launch autoware.launch.xml map_path:=/path/to/map_folder ve ros2 launch autoware_launch planning_simulator.launch.xml map_path:=/path/to/map_folder vehicle_model:=[vehicle_name] sensor_model:=[sensor_name] ``` -## Directory Structure +## Directory structure - [autoware_launch](./autoware_launch) - [control_launch](./control_launch) diff --git a/autoware_api_launch/launch/include/external_api_adaptor.launch.py b/autoware_api_launch/launch/include/external_api_adaptor.launch.py index bd9e64b4a0..0ee9778270 100644 --- a/autoware_api_launch/launch/include/external_api_adaptor.launch.py +++ b/autoware_api_launch/launch/include/external_api_adaptor.launch.py @@ -29,6 +29,7 @@ def _create_api_node(node_name, class_name, **kwargs): def generate_launch_description(): components = [ + _create_api_node("calibration_status", "CalibrationStatus"), _create_api_node("cpu_usage", "CpuUsage"), _create_api_node("diagnostics", "Diagnostics"), _create_api_node("door", "Door"), diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index c2c862c6c8..46cca66416 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -7,6 +7,7 @@ + @@ -59,6 +60,7 @@ + @@ -70,8 +72,7 @@ - - + diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml index 0596343e5c..c143f2c51b 100644 --- a/autoware_launch/launch/logging_simulator.launch.xml +++ b/autoware_launch/launch/logging_simulator.launch.xml @@ -19,12 +19,12 @@ + - @@ -75,6 +75,7 @@ + @@ -86,8 +87,7 @@ - - + @@ -98,7 +98,6 @@ - @@ -107,7 +106,6 @@ - diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index e3eec0ebcb..805153370b 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -724,6 +724,7 @@ Visualization Manager: Display Twist: true Display UUID: true Display Velocity: true + Line Width: 0.03 Enabled: true MOTORCYCLE: Alpha: 0.999 @@ -771,6 +772,7 @@ Visualization Manager: Display Twist: true Display UUID: true Display Velocity: true + Line Width: 0.03 Enabled: true MOTORCYCLE: Alpha: 0.999 @@ -818,6 +820,7 @@ Visualization Manager: Display Twist: false Display UUID: true Display Velocity: true + Line Width: 0.03 Enabled: true MOTORCYCLE: Alpha: 0.999 @@ -843,6 +846,18 @@ Visualization Manager: Alpha: 0.999 Color: 255; 255; 255 Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Maneuver + Namespaces: + maneuver: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /perception/object_recognition/prediction/maneuver + Value: true Enabled: true Name: Prediction - Class: rviz_common/Group @@ -1018,6 +1033,18 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Walkway) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/walkway + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true Name: VirtualWall (DetectionArea) @@ -1155,6 +1182,18 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Walkway + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/walkway + Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false Name: Intersection diff --git a/control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml b/control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml new file mode 100644 index 0000000000..d42e2392be --- /dev/null +++ b/control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + frequency_hz: 10.0 + engage_acceptable_limits: + allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. + dist_threshold: 1.5 + yaw_threshold: 0.524 + speed_upper_threshold: 10.0 + speed_lower_threshold: -10.0 + acc_threshold: 1.5 + lateral_acc_threshold: 1.0 + lateral_acc_diff_threshold: 0.5 + stable_check: + duration: 0.1 + dist_threshold: 1.5 + speed_upper_threshold: 2.0 + speed_lower_threshold: -2.0 + yaw_threshold: 0.262 diff --git a/control_launch/config/trajectory_follower/1/lateral_controller.param.yaml b/control_launch/config/trajectory_follower/1/lateral_controller.param.yaml index f112edb09b..a92f958bd8 100644 --- a/control_launch/config/trajectory_follower/1/lateral_controller.param.yaml +++ b/control_launch/config/trajectory_follower/1/lateral_controller.param.yaml @@ -1,8 +1,6 @@ /**: ros__parameters: - # -- system -- - ctrl_period: 0.03 # control period [s] traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value @@ -40,6 +38,7 @@ mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics @@ -54,9 +53,13 @@ steering_lpf_cutoff_hz: 10.0 # cutoff frequency of lowpass filter for steering command [Hz] error_deriv_lpf_cutoff_hz: 5.0 - # stop state + # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.2 stop_state_entry_target_speed: 0.2 + converged_steer_rad: 0.1 + keep_steer_control_until_converged: false + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 # vehicle parameters vehicle: diff --git a/control_launch/config/trajectory_follower/1/longitudinal_controller.param.yaml b/control_launch/config/trajectory_follower/1/longitudinal_controller.param.yaml index d5e67c2cca..571d66174f 100644 --- a/control_launch/config/trajectory_follower/1/longitudinal_controller.param.yaml +++ b/control_launch/config/trajectory_follower/1/longitudinal_controller.param.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: - longitudinal_ctrl_period: 0.03 delay_compensation_time: 0.40 enable_smooth_stop: true enable_overshoot_emergency: false enable_large_tracking_error_emergency: false enable_slope_compensation: true + enable_keep_stopped_until_steer_convergence: false # state transition drive_state_stop_dist: 0.5 diff --git a/control_launch/config/trajectory_follower/10/lateral_controller.param.yaml b/control_launch/config/trajectory_follower/10/lateral_controller.param.yaml index f112edb09b..a92f958bd8 100644 --- a/control_launch/config/trajectory_follower/10/lateral_controller.param.yaml +++ b/control_launch/config/trajectory_follower/10/lateral_controller.param.yaml @@ -1,8 +1,6 @@ /**: ros__parameters: - # -- system -- - ctrl_period: 0.03 # control period [s] traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value @@ -40,6 +38,7 @@ mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics @@ -54,9 +53,13 @@ steering_lpf_cutoff_hz: 10.0 # cutoff frequency of lowpass filter for steering command [Hz] error_deriv_lpf_cutoff_hz: 5.0 - # stop state + # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.2 stop_state_entry_target_speed: 0.2 + converged_steer_rad: 0.1 + keep_steer_control_until_converged: false + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 # vehicle parameters vehicle: diff --git a/control_launch/config/trajectory_follower/10/longitudinal_controller.param.yaml b/control_launch/config/trajectory_follower/10/longitudinal_controller.param.yaml index 4c63f2a86d..1c71fa9b09 100644 --- a/control_launch/config/trajectory_follower/10/longitudinal_controller.param.yaml +++ b/control_launch/config/trajectory_follower/10/longitudinal_controller.param.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: - longitudinal_ctrl_period: 0.03 delay_compensation_time: 0.40 enable_smooth_stop: true enable_overshoot_emergency: false enable_large_tracking_error_emergency: false enable_slope_compensation: true + enable_keep_stopped_until_steer_convergence: false # state transition drive_state_stop_dist: 0.5 diff --git a/control_launch/config/trajectory_follower/2/lateral_controller.param.yaml b/control_launch/config/trajectory_follower/2/lateral_controller.param.yaml index f112edb09b..a92f958bd8 100644 --- a/control_launch/config/trajectory_follower/2/lateral_controller.param.yaml +++ b/control_launch/config/trajectory_follower/2/lateral_controller.param.yaml @@ -1,8 +1,6 @@ /**: ros__parameters: - # -- system -- - ctrl_period: 0.03 # control period [s] traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value @@ -40,6 +38,7 @@ mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics @@ -54,9 +53,13 @@ steering_lpf_cutoff_hz: 10.0 # cutoff frequency of lowpass filter for steering command [Hz] error_deriv_lpf_cutoff_hz: 5.0 - # stop state + # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.2 stop_state_entry_target_speed: 0.2 + converged_steer_rad: 0.1 + keep_steer_control_until_converged: false + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 # vehicle parameters vehicle: diff --git a/control_launch/config/trajectory_follower/2/longitudinal_controller.param.yaml b/control_launch/config/trajectory_follower/2/longitudinal_controller.param.yaml index 4c63f2a86d..1c71fa9b09 100644 --- a/control_launch/config/trajectory_follower/2/longitudinal_controller.param.yaml +++ b/control_launch/config/trajectory_follower/2/longitudinal_controller.param.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: - longitudinal_ctrl_period: 0.03 delay_compensation_time: 0.40 enable_smooth_stop: true enable_overshoot_emergency: false enable_large_tracking_error_emergency: false enable_slope_compensation: true + enable_keep_stopped_until_steer_convergence: false # state transition drive_state_stop_dist: 0.5 diff --git a/control_launch/config/trajectory_follower/3/lateral_controller.param.yaml b/control_launch/config/trajectory_follower/3/lateral_controller.param.yaml index f112edb09b..a92f958bd8 100644 --- a/control_launch/config/trajectory_follower/3/lateral_controller.param.yaml +++ b/control_launch/config/trajectory_follower/3/lateral_controller.param.yaml @@ -1,8 +1,6 @@ /**: ros__parameters: - # -- system -- - ctrl_period: 0.03 # control period [s] traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value @@ -40,6 +38,7 @@ mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics @@ -54,9 +53,13 @@ steering_lpf_cutoff_hz: 10.0 # cutoff frequency of lowpass filter for steering command [Hz] error_deriv_lpf_cutoff_hz: 5.0 - # stop state + # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.2 stop_state_entry_target_speed: 0.2 + converged_steer_rad: 0.1 + keep_steer_control_until_converged: false + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 # vehicle parameters vehicle: diff --git a/control_launch/config/trajectory_follower/3/longitudinal_controller.param.yaml b/control_launch/config/trajectory_follower/3/longitudinal_controller.param.yaml index 3731ccf9a2..9cefc359d6 100644 --- a/control_launch/config/trajectory_follower/3/longitudinal_controller.param.yaml +++ b/control_launch/config/trajectory_follower/3/longitudinal_controller.param.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: - longitudinal_ctrl_period: 0.03 delay_compensation_time: 0.40 enable_smooth_stop: true enable_overshoot_emergency: false enable_large_tracking_error_emergency: false enable_slope_compensation: true + enable_keep_stopped_until_steer_convergence: false # state transition drive_state_stop_dist: 0.5 diff --git a/control_launch/config/trajectory_follower/4/lateral_controller.param.yaml b/control_launch/config/trajectory_follower/4/lateral_controller.param.yaml index 3f1aaf99f5..b979878ab3 100644 --- a/control_launch/config/trajectory_follower/4/lateral_controller.param.yaml +++ b/control_launch/config/trajectory_follower/4/lateral_controller.param.yaml @@ -1,8 +1,6 @@ /**: ros__parameters: - # -- system -- - ctrl_period: 0.03 # control period [s] traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value @@ -40,6 +38,7 @@ mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics @@ -54,9 +53,13 @@ steering_lpf_cutoff_hz: 10.0 # cutoff frequency of lowpass filter for steering command [Hz] error_deriv_lpf_cutoff_hz: 5.0 - # stop state + # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.2 stop_state_entry_target_speed: 0.2 + converged_steer_rad: 0.1 + keep_steer_control_until_converged: false + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 # vehicle parameters vehicle: diff --git a/control_launch/config/trajectory_follower/4/longitudinal_controller.param.yaml b/control_launch/config/trajectory_follower/4/longitudinal_controller.param.yaml index b70ab79b50..7848f47c4f 100644 --- a/control_launch/config/trajectory_follower/4/longitudinal_controller.param.yaml +++ b/control_launch/config/trajectory_follower/4/longitudinal_controller.param.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: - longitudinal_ctrl_period: 0.03 delay_compensation_time: 0.40 enable_smooth_stop: true enable_overshoot_emergency: false enable_large_tracking_error_emergency: false enable_slope_compensation: true + enable_keep_stopped_until_steer_convergence: false # state transition drive_state_stop_dist: 0.5 diff --git a/control_launch/config/trajectory_follower/5/lateral_controller.param.yaml b/control_launch/config/trajectory_follower/5/lateral_controller.param.yaml index d331b723d2..440ef62427 100644 --- a/control_launch/config/trajectory_follower/5/lateral_controller.param.yaml +++ b/control_launch/config/trajectory_follower/5/lateral_controller.param.yaml @@ -1,8 +1,6 @@ /**: ros__parameters: - # -- system -- - ctrl_period: 0.03 # control period [s] traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value @@ -40,6 +38,7 @@ mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics @@ -54,9 +53,13 @@ steering_lpf_cutoff_hz: 10.0 # cutoff frequency of lowpass filter for steering command [Hz] error_deriv_lpf_cutoff_hz: 5.0 - # stop state + # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.2 stop_state_entry_target_speed: 0.2 + converged_steer_rad: 0.1 + keep_steer_control_until_converged: false + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 # vehicle parameters vehicle: diff --git a/control_launch/config/trajectory_follower/5/longitudinal_controller.param.yaml b/control_launch/config/trajectory_follower/5/longitudinal_controller.param.yaml index 4c63f2a86d..1c71fa9b09 100644 --- a/control_launch/config/trajectory_follower/5/longitudinal_controller.param.yaml +++ b/control_launch/config/trajectory_follower/5/longitudinal_controller.param.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: - longitudinal_ctrl_period: 0.03 delay_compensation_time: 0.40 enable_smooth_stop: true enable_overshoot_emergency: false enable_large_tracking_error_emergency: false enable_slope_compensation: true + enable_keep_stopped_until_steer_convergence: false # state transition drive_state_stop_dist: 0.5 diff --git a/control_launch/config/trajectory_follower/6/lateral_controller.param.yaml b/control_launch/config/trajectory_follower/6/lateral_controller.param.yaml index f112edb09b..a92f958bd8 100644 --- a/control_launch/config/trajectory_follower/6/lateral_controller.param.yaml +++ b/control_launch/config/trajectory_follower/6/lateral_controller.param.yaml @@ -1,8 +1,6 @@ /**: ros__parameters: - # -- system -- - ctrl_period: 0.03 # control period [s] traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value @@ -40,6 +38,7 @@ mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics @@ -54,9 +53,13 @@ steering_lpf_cutoff_hz: 10.0 # cutoff frequency of lowpass filter for steering command [Hz] error_deriv_lpf_cutoff_hz: 5.0 - # stop state + # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.2 stop_state_entry_target_speed: 0.2 + converged_steer_rad: 0.1 + keep_steer_control_until_converged: false + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 # vehicle parameters vehicle: diff --git a/control_launch/config/trajectory_follower/6/longitudinal_controller.param.yaml b/control_launch/config/trajectory_follower/6/longitudinal_controller.param.yaml index d5e67c2cca..571d66174f 100644 --- a/control_launch/config/trajectory_follower/6/longitudinal_controller.param.yaml +++ b/control_launch/config/trajectory_follower/6/longitudinal_controller.param.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: - longitudinal_ctrl_period: 0.03 delay_compensation_time: 0.40 enable_smooth_stop: true enable_overshoot_emergency: false enable_large_tracking_error_emergency: false enable_slope_compensation: true + enable_keep_stopped_until_steer_convergence: false # state transition drive_state_stop_dist: 0.5 diff --git a/control_launch/config/trajectory_follower/7/lateral_controller.param.yaml b/control_launch/config/trajectory_follower/7/lateral_controller.param.yaml index f112edb09b..a92f958bd8 100644 --- a/control_launch/config/trajectory_follower/7/lateral_controller.param.yaml +++ b/control_launch/config/trajectory_follower/7/lateral_controller.param.yaml @@ -1,8 +1,6 @@ /**: ros__parameters: - # -- system -- - ctrl_period: 0.03 # control period [s] traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value @@ -40,6 +38,7 @@ mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics @@ -54,9 +53,13 @@ steering_lpf_cutoff_hz: 10.0 # cutoff frequency of lowpass filter for steering command [Hz] error_deriv_lpf_cutoff_hz: 5.0 - # stop state + # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.2 stop_state_entry_target_speed: 0.2 + converged_steer_rad: 0.1 + keep_steer_control_until_converged: false + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 # vehicle parameters vehicle: diff --git a/control_launch/config/trajectory_follower/7/longitudinal_controller.param.yaml b/control_launch/config/trajectory_follower/7/longitudinal_controller.param.yaml index 4c63f2a86d..1c71fa9b09 100644 --- a/control_launch/config/trajectory_follower/7/longitudinal_controller.param.yaml +++ b/control_launch/config/trajectory_follower/7/longitudinal_controller.param.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: - longitudinal_ctrl_period: 0.03 delay_compensation_time: 0.40 enable_smooth_stop: true enable_overshoot_emergency: false enable_large_tracking_error_emergency: false enable_slope_compensation: true + enable_keep_stopped_until_steer_convergence: false # state transition drive_state_stop_dist: 0.5 diff --git a/control_launch/config/trajectory_follower/9/lateral_controller.param.yaml b/control_launch/config/trajectory_follower/9/lateral_controller.param.yaml index f112edb09b..a92f958bd8 100644 --- a/control_launch/config/trajectory_follower/9/lateral_controller.param.yaml +++ b/control_launch/config/trajectory_follower/9/lateral_controller.param.yaml @@ -1,8 +1,6 @@ /**: ros__parameters: - # -- system -- - ctrl_period: 0.03 # control period [s] traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value @@ -40,6 +38,7 @@ mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics @@ -54,9 +53,13 @@ steering_lpf_cutoff_hz: 10.0 # cutoff frequency of lowpass filter for steering command [Hz] error_deriv_lpf_cutoff_hz: 5.0 - # stop state + # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.2 stop_state_entry_target_speed: 0.2 + converged_steer_rad: 0.1 + keep_steer_control_until_converged: false + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 # vehicle parameters vehicle: diff --git a/control_launch/config/trajectory_follower/9/longitudinal_controller.param.yaml b/control_launch/config/trajectory_follower/9/longitudinal_controller.param.yaml index 5bc0997d94..032e9d7d25 100644 --- a/control_launch/config/trajectory_follower/9/longitudinal_controller.param.yaml +++ b/control_launch/config/trajectory_follower/9/longitudinal_controller.param.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: - longitudinal_ctrl_period: 0.03 delay_compensation_time: 0.40 enable_smooth_stop: true enable_overshoot_emergency: false enable_large_tracking_error_emergency: false enable_slope_compensation: true + enable_keep_stopped_until_steer_convergence: false # state transition drive_state_stop_dist: 0.5 diff --git a/control_launch/config/trajectory_follower/default/lateral_controller.param.yaml b/control_launch/config/trajectory_follower/default/lateral_controller.param.yaml index 68cbabedae..133b794b39 100644 --- a/control_launch/config/trajectory_follower/default/lateral_controller.param.yaml +++ b/control_launch/config/trajectory_follower/default/lateral_controller.param.yaml @@ -1,8 +1,6 @@ /**: ros__parameters: - # -- system -- - ctrl_period: 0.03 # control period [s] traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value @@ -40,6 +38,7 @@ mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics @@ -54,9 +53,13 @@ steering_lpf_cutoff_hz: 10.0 # cutoff frequency of lowpass filter for steering command [Hz] error_deriv_lpf_cutoff_hz: 5.0 - # stop state + # stop state: steering command is kept in the previous value in the stop state. stop_state_entry_ego_speed: 0.2 stop_state_entry_target_speed: 0.2 + converged_steer_rad: 0.1 + keep_steer_control_until_converged: false + new_traj_duration_time: 1.0 + new_traj_end_dist: 0.3 # vehicle parameters vehicle: diff --git a/control_launch/config/trajectory_follower/default/longitudinal_controller.param.yaml b/control_launch/config/trajectory_follower/default/longitudinal_controller.param.yaml index 4c63f2a86d..1c71fa9b09 100644 --- a/control_launch/config/trajectory_follower/default/longitudinal_controller.param.yaml +++ b/control_launch/config/trajectory_follower/default/longitudinal_controller.param.yaml @@ -1,12 +1,12 @@ /**: ros__parameters: - longitudinal_ctrl_period: 0.03 delay_compensation_time: 0.40 enable_smooth_stop: true enable_overshoot_emergency: false enable_large_tracking_error_emergency: false enable_slope_compensation: true + enable_keep_stopped_until_steer_convergence: false # state transition drive_state_stop_dist: 0.5 diff --git a/control_launch/config/trajectory_follower/latlon_muxer.param.yaml b/control_launch/config/trajectory_follower/latlon_muxer.param.yaml deleted file mode 100644 index 371bb99787..0000000000 --- a/control_launch/config/trajectory_follower/latlon_muxer.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - timeout_thr_sec: 0.5 # [s] Time limit after which input messages are discarded. diff --git a/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml index 0ad4631c6d..4876699351 100644 --- a/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml +++ b/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml @@ -6,8 +6,15 @@ stop_hold_acceleration: -1.5 emergency_acceleration: -2.4 stopped_state_entry_duration_time: 0.1 - vel_lim: 25.0 - lon_acc_lim: 5.0 - lon_jerk_lim: 5.0 - lat_acc_lim: 5.0 - lat_jerk_lim: 5.0 + nominal: + vel_lim: 25.0 + lon_acc_lim: 5.0 + lon_jerk_lim: 5.0 + lat_acc_lim: 5.0 + lat_jerk_lim: 5.0 + on_transition: + vel_lim: 50.0 + lon_acc_lim: 1.0 + lon_jerk_lim: 0.5 + lat_acc_lim: 1.2 + lat_jerk_lim: 0.75 diff --git a/control_launch/launch/control.launch.py b/control_launch/launch/control.launch.py index ab920ddad8..8e6f77b058 100644 --- a/control_launch/launch/control.launch.py +++ b/control_launch/launch/control.launch.py @@ -19,13 +19,11 @@ from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition -from launch.conditions import LaunchConfigurationEquals from launch.conditions import UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import EnvironmentVariable from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import LoadComposableNodes from launch_ros.actions import PushRosNamespace from launch_ros.descriptions import ComposableNode from launch_ros.substitutions import FindPackageShare @@ -39,10 +37,6 @@ def launch_setup(context, *args, **kwargs): lon_controller_param_path = LaunchConfiguration("lon_controller_param_path").perform(context) with open(lon_controller_param_path, "r") as f: lon_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] - latlon_muxer_param_path = LaunchConfiguration("latlon_muxer_param_path").perform(context) - with open(latlon_muxer_param_path, "r") as f: - latlon_muxer_param = yaml.safe_load(f)["/**"]["ros__parameters"] - vehicle_cmd_gate_param_path = LaunchConfiguration("vehicle_cmd_gate_param_path").perform( context ) @@ -54,73 +48,34 @@ def launch_setup(context, *args, **kwargs): with open(lane_departure_checker_param_path, "r") as f: lane_departure_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] - # lateral controller - mpc_follower_component = ComposableNode( + operation_mode_transition_manager_param_path = LaunchConfiguration( + "operation_mode_transition_manager_param_path" + ).perform(context) + with open(operation_mode_transition_manager_param_path, "r") as f: + operation_mode_transition_manager_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + controller_component = ComposableNode( package="trajectory_follower_nodes", - plugin="autoware::motion::control::trajectory_follower_nodes::LateralController", - name="lateral_controller_node_exe", + plugin="autoware::motion::control::trajectory_follower_nodes::Controller", + name="controller_node_exe", namespace="trajectory_follower", remappings=[ ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), ("~/input/current_odometry", "/localization/kinematic_state"), ("~/input/current_steering", "/vehicle/status/steering_status"), - ("~/output/control_cmd", "lateral/control_cmd"), ("~/output/predicted_trajectory", "lateral/predicted_trajectory"), - ("~/output/diagnostic", "lateral/diagnostic"), - ], - parameters=[ - lat_controller_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - pure_pursuit_component = ComposableNode( - package="pure_pursuit", - plugin="pure_pursuit::PurePursuitNode", - name="pure_pursuit_node_exe", - namespace="trajectory_follower", - remappings=[ - ("input/reference_trajectory", "/planning/scenario_planning/trajectory"), - ("input/current_odometry", "/localization/kinematic_state"), - ("output/control_raw", "lateral/control_cmd"), - ], - parameters=[ - lat_controller_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # longitudinal controller - lon_controller_component = ComposableNode( - package="trajectory_follower_nodes", - plugin="autoware::motion::control::trajectory_follower_nodes::LongitudinalController", - name="longitudinal_controller_node_exe", - namespace="trajectory_follower", - remappings=[ - ("~/input/current_trajectory", "/planning/scenario_planning/trajectory"), - ("~/input/current_odometry", "/localization/kinematic_state"), - ("~/output/control_cmd", "longitudinal/control_cmd"), + ("~/output/lateral_diagnostic", "lateral/diagnostic"), ("~/output/slope_angle", "longitudinal/slope_angle"), - ("~/output/diagnostic", "longitudinal/diagnostic"), - ], - parameters=[ - lon_controller_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # latlon muxer - latlon_muxer_component = ComposableNode( - package="trajectory_follower_nodes", - plugin="autoware::motion::control::trajectory_follower_nodes::LatLonMuxer", - name="latlon_muxer_node_exe", - namespace="trajectory_follower", - remappings=[ - ("~/input/lateral/control_cmd", "lateral/control_cmd"), - ("~/input/longitudinal/control_cmd", "longitudinal/control_cmd"), + ("~/output/longitudinal_diagnostic", "longitudinal/diagnostic"), ("~/output/control_cmd", "control_cmd"), ], parameters=[ - latlon_muxer_param, + { + "ctrl_period": 0.03, + "lateral_controller_mode": LaunchConfiguration("lateral_controller_mode"), + }, + lon_controller_param, + lat_controller_param, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -165,6 +120,7 @@ def launch_setup(context, *args, **kwargs): remappings=[ ("input/emergency_state", "/system/emergency/emergency_state"), ("input/steering", "/vehicle/status/steering_status"), + ("input/operation_mode", "/control/operation_mode"), ("input/auto/control_cmd", "/control/trajectory_follower/control_cmd"), ("input/auto/turn_indicators_cmd", "/planning/turn_indicators_cmd"), ("input/auto/hazard_lights_cmd", "/planning/hazard_lights_cmd"), @@ -186,6 +142,7 @@ def launch_setup(context, *args, **kwargs): ("output/gate_mode", "/control/current_gate_mode"), ("output/engage", "/api/autoware/get/engage"), ("output/external_emergency", "/api/autoware/get/emergency"), + ("output/operation_mode", "/control/vehicle_cmd_gate/operation_mode"), ("~/service/engage", "/api/autoware/set/engage"), ("~/service/external_emergency", "/api/autoware/set/emergency"), # TODO(Takagi, Isamu): deprecated @@ -204,6 +161,30 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # operation mode transition manager + operation_mode_transition_manager_component = ComposableNode( + package="operation_mode_transition_manager", + plugin="operation_mode_transition_manager::OperationModeTransitionManager", + name="operation_mode_transition_manager", + remappings=[ + # input + ("kinematics", "/localization/kinematic_state"), + ("steering", "/vehicle/status/steering_status"), + ("trajectory", "/planning/scenario_planning/trajectory"), + ("control_cmd", "/control/command/control_cmd"), + ("control_mode_report", "/vehicle/status/control_mode"), + ("gate_operation_mode", "/control/vehicle_cmd_gate/operation_mode"), + ("operation_mode_request", "/system/operation_mode_request"), + # output + ("is_autonomous_available", "/control/is_autonomous_available"), + ("operation_mode", "/control/operation_mode"), + ("control_mode_request", "/control/control_mode_request"), + ], + parameters=[ + operation_mode_transition_manager_param, + ], + ) + # external cmd selector external_cmd_selector_loader = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -227,56 +208,26 @@ def launch_setup(context, *args, **kwargs): ], ) - # set container to run all required components in the same process - mpc_follower_container = ComposableNodeContainer( + container = ComposableNodeContainer( name="control_container", namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), composable_node_descriptions=[ - lon_controller_component, - latlon_muxer_component, + controller_component, lane_departure_component, shift_decider_component, vehicle_cmd_gate_component, + operation_mode_transition_manager_component, ], - condition=LaunchConfigurationEquals("lateral_controller_mode", "mpc_follower"), - ) - pure_pursuit_container = ComposableNodeContainer( - name="control_container", - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[ - lon_controller_component, - latlon_muxer_component, - shift_decider_component, - vehicle_cmd_gate_component, - ], - condition=LaunchConfigurationEquals("lateral_controller_mode", "pure_pursuit"), - ) - - # lateral controller is separated since it may be another controller (e.g. pure pursuit) - mpc_follower_loader = LoadComposableNodes( - composable_node_descriptions=[mpc_follower_component], - target_container=mpc_follower_container, - condition=LaunchConfigurationEquals("lateral_controller_mode", "mpc_follower"), - ) - pure_pursuit_loader = LoadComposableNodes( - composable_node_descriptions=[pure_pursuit_component], - target_container=pure_pursuit_container, - condition=LaunchConfigurationEquals("lateral_controller_mode", "pure_pursuit"), ) group = GroupAction( [ PushRosNamespace("control"), - mpc_follower_container, - pure_pursuit_container, + container, external_cmd_selector_loader, external_cmd_converter_loader, - mpc_follower_loader, - pure_pursuit_loader, ] ) @@ -319,14 +270,6 @@ def add_launch_arg(name: str, default_value=None, description=None): ], "path to the parameter file of longitudinal controller", ) - add_launch_arg( - "latlon_muxer_param_path", - [ - FindPackageShare("control_launch"), - "/config/trajectory_follower/latlon_muxer.param.yaml", - ], - "path to the parameter file of latlon muxer", - ) add_launch_arg( "vehicle_cmd_gate_param_path", [ @@ -339,6 +282,14 @@ def add_launch_arg(name: str, default_value=None, description=None): "lane_departure_checker_param_path", [FindPackageShare("lane_departure_checker"), "/config/lane_departure_checker.param.yaml"], ) + add_launch_arg( + "operation_mode_transition_manager_param_path", + [ + FindPackageShare("control_launch"), + "/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml", + ], + "path to the parameter file of vehicle_cmd_gate", + ) # vehicle cmd gate add_launch_arg("use_emergency_handling", "false", "use emergency handling") diff --git a/control_launch/launch/control.launch.xml b/control_launch/launch/control.launch.xml index 3ca1a2d47b..0284192cdd 100644 --- a/control_launch/launch/control.launch.xml +++ b/control_launch/launch/control.launch.xml @@ -9,6 +9,7 @@ + @@ -16,6 +17,7 @@ + diff --git a/localization_launch/config/localization_error_monitor.param.yaml b/localization_launch/config/localization_error_monitor.param.yaml new file mode 100644 index 0000000000..026daf0532 --- /dev/null +++ b/localization_launch/config/localization_error_monitor.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + scale: 3.0 + error_ellipse_size: 1.0 + warn_ellipse_size: 0.8 + error_ellipse_size_lateral_direction: 0.3 + warn_ellipse_size_lateral_direction: 0.2 diff --git a/localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml b/localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml index d2ea9ced7b..ee6047281d 100644 --- a/localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml +++ b/localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml @@ -2,10 +2,6 @@ - - - - - + diff --git a/localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index be07542d16..016838dd0d 100644 --- a/localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -3,7 +3,7 @@ - + @@ -16,8 +16,6 @@ - - diff --git a/localization_launch/launch/twist_estimator/twist_estimator.launch.xml b/localization_launch/launch/twist_estimator/twist_estimator.launch.xml index fd7854a7e4..155521a0ed 100644 --- a/localization_launch/launch/twist_estimator/twist_estimator.launch.xml +++ b/localization_launch/launch/twist_estimator/twist_estimator.launch.xml @@ -1,12 +1,8 @@ - - - - - - - + + + diff --git a/perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml b/perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml new file mode 100644 index 0000000000..dfdee95642 --- /dev/null +++ b/perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + filter_target_label: + UNKNOWN : true + CAR : false + TRUCK : false + BUS : false + TRAILER : false + MOTORCYCLE : false + BICYCLE : false + PEDESTRIAN : false diff --git a/perception_launch/config/object_recognition/detection/object_position_filter.param.yaml b/perception_launch/config/object_recognition/detection/object_position_filter.param.yaml new file mode 100644 index 0000000000..70afd9d31b --- /dev/null +++ b/perception_launch/config/object_recognition/detection/object_position_filter.param.yaml @@ -0,0 +1,16 @@ +/**: + ros__parameters: + filter_target_label: + UNKNOWN : true + CAR : false + TRUCK : false + BUS : false + TRAILER : false + MOTORCYCLE : false + BICYCLE : false + PEDESTRIAN : false + + upper_bound_x: 100.0 + lower_bound_x: 0.0 + upper_bound_y: 10.0 + lower_bound_y: -10.0 diff --git a/perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml b/perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml new file mode 100644 index 0000000000..a07a9416c2 --- /dev/null +++ b/perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + use_down_sample_filter: False + down_sample_voxel_size: 0.1 + distance_threshold: 0.5 diff --git a/perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml index 6e7ac3e9ef..6fb495c258 100644 --- a/perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml +++ b/perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml @@ -1,19 +1,19 @@ /**: ros__parameters: can_assign_matrix: - #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE PEDESTRIAN - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN - 1, 1, 1, 1, 1, 0, 0, 0, #CAR - 1, 1, 1, 1, 1, 0, 0, 0, #TRUCK - 1, 1, 1, 1, 1, 0, 0, 0, #BUS - 1, 1, 1, 1, 1, 0, 0, 0, #TRAILER + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN <-Measurement + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN <-Tracker + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN max_dist_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN + [4.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS @@ -24,11 +24,11 @@ max_area_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN [10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, #UNKNOWN - 12.10, 12.10, 19.75, 32.40, 32.40, 10000.00, 10000.00, 10000.00, #CAR - 19.75, 12.10, 19.75, 32.40, 32.40, 10000.00, 10000.00, 10000.00, #TRUCK - 32.40, 12.10, 19.75, 32.40, 32.40, 10000.00, 10000.00, 10000.00, #BUS - 32.40, 12.10, 19.75, 32.40, 32.40, 10000.00, 10000.00, 10000.00, #TRAILER - 3.00, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE + 12.10, 12.10, 19.75, 40.00, 40.00, 10000.00, 10000.00, 10000.00, #CAR + 19.75, 12.10, 19.75, 40.00, 40.00, 10000.00, 10000.00, 10000.00, #TRUCK + 40.00, 12.10, 19.75, 40.00, 40.00, 10000.00, 10000.00, 10000.00, #BUS + 40.00, 12.10, 19.75, 40.00, 40.00, 10000.00, 10000.00, 10000.00, #TRAILER + 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN min_area_matrix: @@ -52,13 +52,13 @@ 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN - min_iou_matrix: + min_iou_matrix: # If value is negative, it will be ignored. #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN - 0.1, 0.3, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR - 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK - 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS - 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER - 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE - 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE - 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] #PEDESTRIAN + [0.0001, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, 0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.0001] #PEDESTRIAN diff --git a/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 093c6b7322..0031f3663b 100644 --- a/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -1,7 +1,8 @@ - + + @@ -21,8 +22,9 @@ - + + @@ -58,6 +61,16 @@ + + + + + + + + + + @@ -88,13 +101,13 @@ - - + + - - + + @@ -107,8 +120,7 @@ - - + @@ -116,7 +128,7 @@ - + @@ -133,8 +145,8 @@ - - + + @@ -143,7 +155,7 @@ - + @@ -151,20 +163,30 @@ - - - - - - - + + + + + + + + + + + + + + + + + + - - - - + + + + - diff --git a/perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml new file mode 100644 index 0000000000..d2a59a4a45 --- /dev/null +++ b/perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception_launch/launch/object_recognition/detection/detection.launch.xml b/perception_launch/launch/object_recognition/detection/detection.launch.xml index b248a2786e..a17341857b 100644 --- a/perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -1,10 +1,16 @@ + + + - - + + + + + @@ -22,9 +28,34 @@ - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -53,6 +84,16 @@ + + + + + + + + + + @@ -63,7 +104,11 @@ - - + + + + + + diff --git a/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index cde5fc181f..da55d0b355 100644 --- a/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -2,10 +2,12 @@ + - + + @@ -29,13 +31,21 @@ + + + + + + + + + - - + @@ -61,8 +71,8 @@ - - + + @@ -71,7 +81,7 @@ - + @@ -80,10 +90,26 @@ + + + + + + + - + - + + + + + + + + + + diff --git a/perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml new file mode 100644 index 0000000000..a0eef1825b --- /dev/null +++ b/perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py new file mode 100644 index 0000000000..afcf9021c0 --- /dev/null +++ b/perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -0,0 +1,168 @@ +# Copyright 2022 TIER IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +import os + +from ament_index_python.packages import get_package_share_directory +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +import yaml + + +class PointcloudMapFilterPipeline: + def __init__(self, context): + pointcloud_map_filter_param_path = os.path.join( + get_package_share_directory("perception_launch"), + "config/object_recognition/detection/pointcloud_map_filter.param.yaml", + ) + with open(pointcloud_map_filter_param_path, "r") as f: + self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"] + self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"] + self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"] + self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"] + + def create_pipeline(self): + if self.use_down_sample_filter: + return self.create_down_sample_pipeline() + else: + return self.create_normal_pipeline() + + def create_normal_pipeline(self): + components = [] + components.append( + ComposableNode( + package="compare_map_segmentation", + plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", + name="voxel_based_compare_map_filter", + remappings=[ + ("input", LaunchConfiguration("input_topic")), + ("map", "/map/pointcloud_map"), + ("output", LaunchConfiguration("output_topic")), + ], + parameters=[ + { + "distance_threshold": self.distance_threshold, + } + ], + extra_arguments=[ + {"use_intra_process_comms": False}, + ], + ) + ) + return components + + def create_down_sample_pipeline(self): + components = [] + down_sample_topic = ( + "/perception/obstacle_segmentation/pointcloud_map_filtered/downsampled/pointcloud" + ) + components.append( + ComposableNode( + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", + name="voxel_grid_downsample_filter", + remappings=[ + ("input", LaunchConfiguration("input_topic")), + ("output", down_sample_topic), + ], + parameters=[ + { + "voxel_size_x": self.voxel_size, + "voxel_size_y": self.voxel_size, + "voxel_size_z": self.voxel_size, + } + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ), + ) + components.append( + ComposableNode( + package="compare_map_segmentation", + plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", + name="voxel_based_compare_map_filter", + remappings=[ + ("input", down_sample_topic), + ("map", "/map/pointcloud_map"), + ("output", LaunchConfiguration("output_topic")), + ], + parameters=[ + { + "distance_threshold": self.distance_threshold, + } + ], + extra_arguments=[ + {"use_intra_process_comms": False}, + ], + ) + ) + return components + + +def launch_setup(context, *args, **kwargs): + pipeline = PointcloudMapFilterPipeline(context) + components = [] + components.extend(pipeline.create_pipeline()) + individual_container = ComposableNodeContainer( + name=LaunchConfiguration("container_name"), + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=components, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + output="screen", + ) + pointcloud_container_loader = LoadComposableNodes( + composable_node_descriptions=components, + target_container=LaunchConfiguration("container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + ) + return [individual_container, pointcloud_container_loader] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None): + launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value)) + + add_launch_arg("input_topic", "") + add_launch_arg("output_topic", "") + add_launch_arg("use_multithread", "False") + add_launch_arg("use_intra_process", "True") + add_launch_arg("use_pointcloud_container", "False") + add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container") + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + return launch.LaunchDescription( + launch_arguments + + [set_container_executable, set_container_mt_executable] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml new file mode 100644 index 0000000000..6fb6184d6a --- /dev/null +++ b/perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/perception_launch/launch/perception.launch.xml b/perception_launch/launch/perception.launch.xml index 9b525a1dcb..ab932d1796 100644 --- a/perception_launch/launch/perception.launch.xml +++ b/perception_launch/launch/perception.launch.xml @@ -2,8 +2,9 @@ - - + + + @@ -67,6 +68,7 @@ + diff --git a/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index 64252eaf7c..ae79eae500 100644 --- a/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -17,6 +17,7 @@ + @@ -37,6 +38,7 @@ + diff --git a/perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py b/perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py index 3f16c9a043..8827dfbb98 100644 --- a/perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py +++ b/perception_launch/launch/traffic_light_recognition/traffic_light_node_container.launch.py @@ -67,6 +67,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("input_h", "224") add_launch_arg("input_w", "224") + add_launch_arg("use_crosswalk_traffic_light_estimator", "True") add_launch_arg("use_intra_process", "False") add_launch_arg("use_multithread", "False") @@ -101,7 +102,7 @@ def create_parameter_dict(*args): remappings=[ ("~/input/image", LaunchConfiguration("input/image")), ("~/input/rois", "rois"), - ("~/output/traffic_signals", "traffic_signals"), + ("~/output/traffic_signals", "classified/traffic_signals"), ], extra_arguments=[ {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} @@ -130,6 +131,46 @@ def create_parameter_dict(*args): output="both", ) + estimator_loader = LoadComposableNodes( + composable_node_descriptions=[ + ComposableNode( + package="crosswalk_traffic_light_estimator", + plugin="traffic_light::CrosswalkTrafficLightEstimatorNode", + name="crosswalk_traffic_light_estimator", + remappings=[ + ("~/input/vector_map", "/map/vector_map"), + ("~/input/route", "/planning/mission_planning/route"), + ("~/input/classified/traffic_signals", "classified/traffic_signals"), + ("~/output/traffic_signals", "traffic_signals"), + ], + extra_arguments=[{"use_intra_process_comms": False}], + ), + ], + target_container=container, + condition=IfCondition(LaunchConfiguration("use_crosswalk_traffic_light_estimator")), + ) + + relay_loader = LoadComposableNodes( + composable_node_descriptions=[ + ComposableNode( + package="topic_tools", + plugin="topic_tools::RelayNode", + name="classified_signals_relay", + namespace="", + parameters=[ + {"input_topic": "classified/traffic_signals"}, + {"output_topic": "traffic_signals"}, + {"type": "autoware_auto_perception_msgs/msg/TrafficSignalArray"}, + ], + extra_arguments=[ + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} + ], + ) + ], + target_container=container, + condition=UnlessCondition(LaunchConfiguration("use_crosswalk_traffic_light_estimator")), + ) + decompressor_loader = LoadComposableNodes( composable_node_descriptions=[ ComposableNode( @@ -205,5 +246,7 @@ def create_parameter_dict(*args): container, decompressor_loader, fine_detector_loader, + estimator_loader, + relay_loader, ] ) diff --git a/perception_launch/package.xml b/perception_launch/package.xml index 4c5673c04f..a49fa414d0 100644 --- a/perception_launch/package.xml +++ b/perception_launch/package.xml @@ -11,6 +11,7 @@ ament_cmake_auto compare_map_segmentation + crosswalk_traffic_light_estimator euclidean_cluster ground_segmentation image_projection_based_fusion diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index d691228589..1b820b2c2b 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -24,7 +24,7 @@ min_nominal_avoidance_speed: 7.0 # [m/s] min_sharp_avoidance_speed: 1.0 # [m/s] - road_shoulder_safety_margin: 0.5 # [m] + road_shoulder_safety_margin: 0.0 # [m] max_right_shift_length: 5.0 max_left_shift_length: 5.0 @@ -32,7 +32,7 @@ nominal_lateral_jerk: 0.2 # [m/s3] max_lateral_jerk: 1.0 # [m/s3] - object_hold_max_count: 20 + object_last_seen_threshold: 2.0 # For prevention of large acceleration while avoidance min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml index 92e8ba6801..4ea3229d46 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml @@ -1,14 +1,43 @@ /**: ros__parameters: crosswalk: - crosswalk: - stop_line_distance: 1.5 # make stop line away from crosswalk when no explicit stop line exists - stop_margin: 1.0 - slow_margin: 2.0 - slow_velocity: 2.76 # 2.76 m/s = 10.0 kmph - stop_predicted_object_prediction_time_margin: 3.0 - - walkway: - stop_line_distance: 1.5 # make stop line away from walkway when no explicit stop line exists - stop_margin: 1.0 - stop_duration_sec: 1.0 + show_processing_time: false # [-] whether to show processing time + + # param for stop position + stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists + stop_line_margin: 10.0 # [m] if objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_margin meters before the object) + stop_margin: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin + stop_position_threshold: 1.0 # [m] threshold for check whether the vehicle stop in front of crosswalk + + # param for ego velocity + slow_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph) + max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake + max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake + no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph) + + # param for stuck vehicle + stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck + max_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked + stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk + + # param for pass judge logic + ego_pass_first_margin: 6.0 # [s] time margin for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_later_margin: 10.0 # [s] time margin for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) + min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) + max_yield_timeout: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + + # param for input data + tl_state_timeout: 3.0 # [s] timeout threshold for traffic light signal + + # param for target area & object + crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk + target_object: + unknown: true # [-] whether to look and stop by UNKNOWN objects + bicycle: true # [-] whether to look and stop by BICYCLE objects + motorcycle: true # [-] whether to look and stop by MOTORCYCLE objects (tmp: currently it is difficult for perception modules to detect bicycles and motorcycles separately.) + pedestrian: true # [-] whether to look and stop by PEDESTRIAN objects + + walkway: + stop_duration_sec: 1.0 # [s] stop time at stop position + stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index bfcaeb8201..828298fc17 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -2,7 +2,6 @@ ros__parameters: intersection: state_transit_margin_time: 0.5 - decel_velocity: 8.33 # 8.33m/s = 30.0km/h stop_line_margin: 3.0 stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml index 8818ed0199..527c00ccf3 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml @@ -12,13 +12,9 @@ detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision - # rectangle detection area for Points method - # this will be replaced with more appropriate detection area - detection_area_size: - dist_ahead: 50.0 # [m] ahead distance from ego position - dist_behind: 5.0 # [m] behind distance from ego position - dist_right: 7.0 # [m] right distance from ego position - dist_left: 7.0 # [m] left distance from ego position + detection_area: + margin_behind: 0.5 # [m] ahead margin for detection area length + margin_ahead: 1.0 # [m] behind margin for detection area length # parameter to create abstracted dynamic obstacles dynamic_obstacle: diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml index 79ae337b8e..6df9e2af49 100644 --- a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml @@ -58,7 +58,8 @@ option: steer_limit_constraint: true fix_points_around_ego: true - # plan_from_ego: false + plan_from_ego: false + max_plan_from_ego_length: 10.0 visualize_sampling_num: 1 enable_manual_warm_start: true enable_warm_start: true # false diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 708c23f5ba..c23ce72f3b 100644 --- a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -3,10 +3,10 @@ common: planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" - is_showing_debug_info: true + is_showing_debug_info: false # longitudinal info - idling_time: 4.0 # idling time to detect front vehicle starting deceleration [s] + idling_time: 1.5 # idling time to detect front vehicle starting deceleration [s] min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] @@ -17,6 +17,7 @@ nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] cruise_obstacle_type: unknown: true @@ -39,7 +40,7 @@ pedestrian: true obstacle_filtering: - rough_detection_area_expand_width : 5.0 # rough lateral margin for rough detection area expansion [m] + rough_detection_area_expand_width : 3.0 # rough lateral margin for rough detection area expansion [m] detection_area_expand_width : 0.0 # lateral margin for precise detection area expansion decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking @@ -48,11 +49,14 @@ crossing_obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop collision_time_margin : 8.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] + outside_rough_detection_area_expand_width : 0.5 # rough lateral margin for rough detection area expansion for obstacles outside the trajectory [m] ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego + stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle + ignored_outside_obstacle_type: - unknown: false + unknown: true car: false truck: false bus: false @@ -65,17 +69,19 @@ # use_predicted_obstacle_pose: false # PID gains to keep safe distance with the front vehicle - kp: 0.6 + kp: 2.5 ki: 0.0 - kd: 0.5 + kd: 2.3 min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] - output_ratio_during_accel: 2.5 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] - vel_to_acc_weight: 8.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + vel_to_acc_weight: 3.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] + lpf_cruise_gain: 0.2 + optimization_based_planner: resampling_s_interval: 2.0 dense_resampling_time_interval: 0.1 diff --git a/planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml b/planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml index 72648a9dfd..d6152c4f56 100644 --- a/planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml +++ b/planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml @@ -8,6 +8,7 @@ th_stopped_time_sec: 1.0 th_stopped_velocity_mps: 0.01 th_course_out_distance_m: 1.0 + vehicle_shape_margin_m: 1.0 replan_when_obstacle_found: true replan_when_course_out: true