From 38702a37e3e37f2949b42c21656ca63b53f4527b Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 13 Dec 2021 12:13:09 +0900 Subject: [PATCH] feat: add control_launch package (#157) * release v0.4.0 * load velocity_controller_param.yaml from launcher (#52) Signed-off-by: Shinnosuke Hirakawa * Include enable_slope_compensation to yaml file (#61) * Sync mpc param (#74) Signed-off-by: wep21 * add stop dist param (#73) * add stop dist param * add keep stopping dist param * removed ROS1 package Signed-off-by: mitsudome-r * Revert "removed ROS1 package" This reverts commit 85eb34207aa7b90314d93a6eb370213e10fcd7bd. * add COLCON_IGNORE file to all ROS1 packages Signed-off-by: mitsudome-r * rename *.launch files to *.launch.xml Signed-off-by: mitsudome-r * port control launch (#7) * port control launch Signed-off-by: Takamasa Horibe * fix license Signed-off-by: Takamasa Horibe * ROS2 Linting: control_launch (#29) * Add linter tests and completepackage dependencies list * Reorder alphabetically * Comment out pure_pursuit dependency - not ported yet * Add back pure_pursuit package post port * Ros2 port autoware launch (#35) * [autoware_launch] port CMakelists.txt and remove COLCON_IGNORE Signed-off-by: mitsudome-r * [autoware_launch] fix planning_simulator.launch.xml Signed-off-by: mitsudome-r * [autoware_launch] add rviz config Signed-off-by: mitsudome-r * [autoware_launch] first port of autoware_launch Signed-off-by: mitsudome-r * lanuch rviz with config file Signed-off-by: kosuke murakami * modify launch file for making psim work Signed-off-by: kosuke murakami * add vehicle model in launch Signed-off-by: kosuke murakami * change from vehicle_model to vehicle_param_file Signed-off-by: kosuke murakami * [autoware_launch] add autoware_web_controller and system launch Signed-off-by: mitsudome-r * add rosbrdige_suite to build_depends.repos Signed-off-by: mitsudome-r * Update rviz Signed-off-by: wep21 * update autoware.rviz Signed-off-by: mitsudome-r * remove autoware_ros2.rviz Signed-off-by: mitsudome-r Co-authored-by: kosuke murakami Co-authored-by: wep21 * Ros2 v0.8.0 control launch (#53) * restore file name for v0.8.0 update Signed-off-by: Takamasa Horibe * Add use_emergency_handling arg to vehicle_cmd_gate.launch (#83) Signed-off-by: Kenji Miyake * Add parameter for using steer prediction (#88) * change stop dist param (#85) * mpc: change param name steer_rate_lim_deg -> steer_rate_lim_dps (#79) Signed-off-by: Takamasa Horibe * add mpc parameter (#105) * Add parameters for stop state (#126) * Add parameters for stop state * Change default value * Add vehicle cmd gate config (#136) * Add config_file of vehicle_cmd_gate Signed-off-by: Kenji Miyake * Add use_emergency_stop Signed-off-by: Kenji Miyake * Rename emergency_stop to external_emergency_stop Signed-off-by: Kenji Miyake * Fix command_gate diag Signed-off-by: Kenji Miyake * Add lane_departure_checker (#123) Signed-off-by: Kenji Miyake * Revert "restore file name for v0.8.0 update" This reverts commit 516b366819f7f9d69b9bc3e2de180d4523794bcd. * fix control launch Signed-off-by: Takamasa Horibe * fix args Signed-off-by: Takamasa Horibe * fix param type Signed-off-by: Takamasa Horibe * fix lane departure checker Signed-off-by: Takamasa Horibe Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Fumiya Watanabe Co-authored-by: tkimura4 * [velocity controller]: Fix parameter type (#62) Signed-off-by: wep21 * Rename ROS-related .yaml to .param.yaml (#65) * Rename ROS-related .yaml to .param.yaml Signed-off-by: Kenji Miyake * Add missing '--' Signed-off-by: Kenji Miyake * Rename vehicle_info.yaml to vehicle_info.param.yaml Signed-off-by: Kenji Miyake * Fix livox param name Signed-off-by: Kenji Miyake * Ros2 update v0.9.0 (#67) * Add pose history into rviz config Signed-off-by: wep21 * add blocked by obstacle option (#164) * fix tab name (#166) * disenable ndt visualization (#169) * disenable ndt visualization * change alpha * dont visualize position covariance Signed-off-by: wep21 * change global frame to map (#171) * add param (#156) Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: Taichi Higashide Co-authored-by: Yukihiro Saito Co-authored-by: tkimura4 * Fix typo in control launch (#91) * Fix typo in control module * change admissible_yaw_error -> admissible_yaw_error_rad * Fix various typos in launch files (#97) * add use_sim-time option (#99) * Replace doc by description (#182) Signed-off-by: Kenji Miyake * Sync public repo (#185) * add tier4 usbcam (#104) * add tier4 usbcam * change version * tier4/ros2 * Ros2 vehicle info param server (#96) * add vehicle info param server * delete vehicle param file Co-authored-by: taikitanaka * Ros2 fix topic name part2 (#89) * Fix topic name for traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix topic name for traffic_light_visualization Signed-off-by: Takagi, Isamu * Fix topic name for traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: tkimura4 Co-authored-by: taikitanaka Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> * Use set_parameter for use_sim_time (#198) * Use set_parameter for use_sim_time Signed-off-by: wep21 * Add default parameter for scenario simulator Signed-off-by: wep21 * Add control composed launch (#201) Signed-off-by: wep21 * Fix mistakes in control.launch.py (#209) * Fix typo in control.launch.py Signed-off-by: Kenji Miyake * Add remote_cmd_converter_component Signed-off-by: Kenji Miyake * Feature/add external cmd selector (#206) * Add remote_cmd_selector to control.launch (#172) * Add remote_cmd_selector to control.launch Signed-off-by: Kenji Miyake * Rename remote_cmd_selector to external_cmd_selector Signed-off-by: Kenji Miyake * Change default external cmd source (#173) Signed-off-by: Kenji Miyake * modify to use control.launch.xml * add external_cmd_selector to control.launch.py * Fix remapping in control.launch.py Signed-off-by: Kenji Miyake * Fix remapping in control.launch.py again Signed-off-by: Kenji Miyake Co-authored-by: Keisuke Shima * Format launch files (#228) * Format launch files Signed-off-by: Kenji Miyake * Format launch.py Signed-off-by: Kenji Miyake * Fix lint Signed-off-by: Kenji Miyake * add external commands (#210) * add remote commands * fix topic * ToExternalCmd * fix topic in remote cmd converter * fix ament * add description for control_launch (#330) * add description * fix sentence * use of * add README.md and svg files (#328) * add md and svg * fix typo * fix typo * fix word * fix typo * add lack of things * Update README * fix depending packages * fix word * Feature/expand footprint launcher (#318) * fix typo in control.launch.py #363 * Fix description in control launch arg (#364) * Add autoware api (#376) * Add external api adaptor (#267) * Add external api adaptor * Add api adaptor to logging simulator Signed-off-by: Takagi, Isamu * Add engage status output Signed-off-by: Takagi, Isamu * Add internal api adaptor (#273) Signed-off-by: Takagi, Isamu * Add map hash generator (#319) Signed-off-by: Takagi, Isamu * Add autoware api launch (#326) * Add autoware api launch Signed-off-by: Takagi, Isamu * Apply autoware api launch Signed-off-by: Takagi, Isamu * Add deprecated comment Signed-off-by: Takagi, Isamu * Remove unused parameter (#325) Signed-off-by: Takagi, Isamu * Add api parameter (#341) Signed-off-by: Takagi, Isamu * Add start request API (#321) * Add use start request option Signed-off-by: Takagi, Isamu * Fix lint Signed-off-by: Takagi, Isamu * Feature external cmd selector heartbeat (#356) * Rename external command topic Signed-off-by: Takagi, Isamu * Modify command topic name Signed-off-by: Takagi, Isamu * Fix topic name Signed-off-by: Takagi, Isamu * Rename remote_cmd_converter Signed-off-by: Takagi, Isamu * Remove gate mode from external command Signed-off-by: Takagi, Isamu * Fix latest external command name (#361) Signed-off-by: Takagi, Isamu * Fix merge conflict Signed-off-by: Takagi, Isamu * change external traffic light topic name in behavior velocity planner (#310) * fix topic * change internal topic name Co-authored-by: yabuta * Move launch file of external_cmd_selector (#386) * Move launch file of external_cmd_selector Signed-off-by: Takagi, Isamu * Add utility function to include launch.py Signed-off-by: Takagi, Isamu * Add missing parameter Signed-off-by: Takagi, Isamu * Fix external command api name Signed-off-by: Takagi, Isamu * move common parameters Signed-off-by: Takagi, Isamu * Fix format Signed-off-by: Takagi, Isamu * Use EmergencyState instead of deprecated EmergencyMode (#390) * Use EmergencyState instead of deprecated EmergencyMode Signed-off-by: Kenji Miyake * Update control.launch.py * Add selected external command API (#397) * Fix pre-commit (#407) * Fix pre-commit errors Signed-off-by: Kenji Miyake * Fix package.xml Signed-off-by: Kenji Miyake * Add emergency status API (#439) * remove yaw-recalc param in mpc (#190) (#294) Co-authored-by: tkimura4 * Master sync for velocity controller (#297) * update velocity_controller_param.yaml for refactoring velocity_controller (#238) * update velocity_controller_param.yaml (#247) Co-authored-by: Takayuki Murooka * Fix parameter type (#299) Signed-off-by: wep21 * add smoothing_num parameter (#263) (#301) * add smoothing_num parameter * change param Co-authored-by: tkimura4 * add stopped_jerk (#276) (#315) * add stopped_jerk * set stopped_jerk -5.0 Co-authored-by: Takayuki Murooka * Rename param from smooth_stop_max_fast_vel to smooth_stop_min_fast_vel (#351) * Feature/vehicle gate parametrize acc (#320) * [vehicle_cmd_gate] add acc parameters Signed-off-by: TakaHoribe * fix typo Signed-off-by: TakaHoribe * Update control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml Co-authored-by: tkimura4 Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: tkimura4 * Change formatter to black (#488) * Update pre-commit settings * Apply Black * Replace ament_lint_common with autoware_lint_common * Update build_depends.repos * Fix build_depends * twist -> odometry (#109) Co-authored-by: Takayuki Murooka * remove unused depend/launcher (#112) * Auto/control psim (#113) * succeeded in launching control nodes * fix input/output of control_launch * Remove multiple package * Add newline * Update control_launch/launch/control.launch.py Co-authored-by: tkimura4 * Apply pre-commit Co-authored-by: Takayuki Murooka Co-authored-by: tkimura4 * Fix remapping in control.launch.py (#115) * Fix remappings * Add comment * Fix remappings (#116) * fix topics * add external hazard indicator * add param (#126) * fix yaml (#127) * remove unused files Signed-off-by: tomoya.kimura * add default parameter of vehicle information Signed-off-by: tomoya.kimura * ci(pre-commit): autofix Co-authored-by: mitsudome-r Co-authored-by: shin <8327162+0x126@users.noreply.github.com> Co-authored-by: Fumiya Watanabe Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: Jilada Eccleston Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: kosuke murakami Co-authored-by: wep21 Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: Taichi Higashide Co-authored-by: Yukihiro Saito Co-authored-by: Kazuki Miyahara Co-authored-by: taikitanaka Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Keisuke Shima Co-authored-by: Hiroki OTA Co-authored-by: yabuta Co-authored-by: Takayuki Murooka Co-authored-by: Maxime CLEMENT Co-authored-by: Kenji Miyake Co-authored-by: Takayuki Murooka Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- launch/control_launch/CMakeLists.txt | 23 + launch/control_launch/README.md | 24 + .../lateral_controller.param.yaml | 71 +++ .../latlon_muxer.param.yaml | 3 + .../longitudinal_controller.param.yaml | 84 ++++ .../vehicle_cmd_gate.param.yaml | 12 + .../control_launch/control_launch.drawio.svg | 421 ++++++++++++++++++ .../control_launch/launch/control.launch.py | 356 +++++++++++++++ launch/control_launch/package.xml | 27 ++ 9 files changed, 1021 insertions(+) create mode 100644 launch/control_launch/CMakeLists.txt create mode 100644 launch/control_launch/README.md create mode 100644 launch/control_launch/config/trajectory_follower/lateral_controller.param.yaml create mode 100644 launch/control_launch/config/trajectory_follower/latlon_muxer.param.yaml create mode 100644 launch/control_launch/config/trajectory_follower/longitudinal_controller.param.yaml create mode 100644 launch/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml create mode 100644 launch/control_launch/control_launch.drawio.svg create mode 100644 launch/control_launch/launch/control.launch.py create mode 100644 launch/control_launch/package.xml diff --git a/launch/control_launch/CMakeLists.txt b/launch/control_launch/CMakeLists.txt new file mode 100644 index 0000000000000..206bb55e108fe --- /dev/null +++ b/launch/control_launch/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.5) +project(control_launch) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wno-unused-parameter -Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/launch/control_launch/README.md b/launch/control_launch/README.md new file mode 100644 index 0000000000000..f18eedc3c9cf4 --- /dev/null +++ b/launch/control_launch/README.md @@ -0,0 +1,24 @@ +# control_launch + +## Structure + +![control_launch](./control_launch.drawio.svg) + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +You can include as follows in `*.launch.xml` to use `control.launch.py`. + +```xml + + + + +``` + +## Notes + +For reducing processing load, we use the [Component](https://docs.ros.org/en/galactic/Concepts/About-Composition.html) feature in ROS2 (similar to Nodelet in ROS1 ) diff --git a/launch/control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/control_launch/config/trajectory_follower/lateral_controller.param.yaml new file mode 100644 index 0000000000000..d2928e02dd6ec --- /dev/null +++ b/launch/control_launch/config/trajectory_follower/lateral_controller.param.yaml @@ -0,0 +1,71 @@ +/**: + 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 + admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value + + # -- path smoothing -- + enable_path_smoothing: false # flag for path smoothing + path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing + curvature_smoothing_num_traj: 1 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) + curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) + + # -- mpc optimization -- + qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) + mpc_prediction_horizon: 50 # prediction horizon step + mpc_prediction_dt: 0.1 # prediction horizon period [s] + mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q + mpc_weight_heading_error: 0.0 # heading error weight in matrix Q + mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q + mpc_weight_steering_input: 1.0 # steering error weight in matrix R + mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R + mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R + mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R + mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R + mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point + mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point + mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point + mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point + mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) + mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability + mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability + 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 + + # -- vehicle model -- + vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics + input_delay: 0.24 # steering input delay time for delay compensation + vehicle_model_steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s] + steer_lim_deg: 20.0 # steering angle limit [deg] + steer_rate_lim_dps: 600.0 # steering angle rate limit [deg/s] + acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] + velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] + + # -- lowpass filter for noise reduction -- + steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz] + error_deriv_lpf_cutoff_hz: 5.0 + + # stop state + stop_state_entry_ego_speed: 0.2 + stop_state_entry_target_speed: 0.5 + + # vehicle parameters + vehicle: + cg_to_front_m: 1.228 + cg_to_rear_m: 1.5618 + mass_kg: 2400.0 + mass_fl: 600.0 + mass_fr: 600.0 + mass_rl: 600.0 + mass_rr: 600.0 + cf: 155494.663 + cr: 155494.663 diff --git a/launch/control_launch/config/trajectory_follower/latlon_muxer.param.yaml b/launch/control_launch/config/trajectory_follower/latlon_muxer.param.yaml new file mode 100644 index 0000000000000..371bb99787559 --- /dev/null +++ b/launch/control_launch/config/trajectory_follower/latlon_muxer.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + timeout_thr_sec: 0.5 # [s] Time limit after which input messages are discarded. diff --git a/launch/control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/launch/control_launch/config/trajectory_follower/longitudinal_controller.param.yaml new file mode 100644 index 0000000000000..abcd4458f06ee --- /dev/null +++ b/launch/control_launch/config/trajectory_follower/longitudinal_controller.param.yaml @@ -0,0 +1,84 @@ +/**: + ros__parameters: + control_rate: 30.0 + delay_compensation_time: 0.17 + + enable_smooth_stop: true + enable_overshoot_emergency: true + enable_slope_compensation: false + + # state transition + drive_state_stop_dist: 0.5 + drive_state_offset_stop_dist: 1.0 + stopping_state_stop_dist: 0.49 + stopped_state_entry_vel: 0.1 + stopped_state_entry_acc: 0.1 + emergency_state_overshoot_stop_dist: 1.5 + emergency_state_traj_trans_dev: 3.0 + emergency_state_traj_rot_dev: 0.7 + + # drive state + kp: 1.0 + ki: 0.1 + kd: 0.0 + max_out: 1.0 + min_out: -1.0 + max_p_effort: 1.0 + min_p_effort: -1.0 + max_i_effort: 0.3 + min_i_effort: -0.3 + max_d_effort: 0.0 + min_d_effort: 0.0 + lpf_vel_error_gain: 0.9 + current_vel_threshold_pid_integration: 0.5 + enable_brake_keeping_before_stop: false + brake_keeping_acc: -0.2 + + # smooth stop state + smooth_stop_max_strong_acc: -0.5 + smooth_stop_min_strong_acc: -1.0 + smooth_stop_weak_acc: -0.3 + smooth_stop_weak_stop_acc: -0.8 + smooth_stop_strong_stop_acc: -3.4 + smooth_stop_max_fast_vel: 0.5 + smooth_stop_min_running_vel: 0.01 + smooth_stop_min_running_acc: 0.01 + smooth_stop_weak_stop_time: 0.8 + smooth_stop_weak_stop_dist: -0.3 + smooth_stop_strong_stop_dist: -0.5 + + # stopped state + stopped_vel: 0.0 + stopped_acc: -3.4 + stopped_jerk: -5.0 + + # emergency state + emergency_vel: 0.0 + emergency_acc: -5.0 + emergency_jerk: -3.0 + + # acceleration limit + max_acc: 3.0 + min_acc: -5.0 + + # jerk limit + max_jerk: 2.0 + min_jerk: -5.0 + + # pitch + use_trajectory_for_pitch_calculation: false + lpf_pitch_gain: 0.95 + max_pitch_rad: 0.1 + min_pitch_rad: -0.1 + + # vehicle parameters + vehicle: + cg_to_front_m: 1.228 + cg_to_rear_m: 1.5618 + mass_kg: 2400.0 + mass_fl: 600.0 + mass_fr: 600.0 + mass_rl: 600.0 + mass_rr: 600.0 + cf: 155494.663 + cr: 155494.663 diff --git a/launch/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/launch/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml new file mode 100644 index 0000000000000..94b682a6b4c5b --- /dev/null +++ b/launch/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + update_rate: 10.0 + system_emergency_heartbeat_timeout: 0.5 + external_emergency_stop_heartbeat_timeout: 0.0 + stop_hold_acceleration: -1.5 + emergency_acceleration: -2.4 + vel_lim: 25.0 + lon_acc_lim: 5.0 + lon_jerk_lim: 5.0 + lat_acc_lim: 5.0 + lat_jerk_lim: 5.0 diff --git a/launch/control_launch/control_launch.drawio.svg b/launch/control_launch/control_launch.drawio.svg new file mode 100644 index 0000000000000..f51b6ba126767 --- /dev/null +++ b/launch/control_launch/control_launch.drawio.svg @@ -0,0 +1,421 @@ + + + + + + + +
+
+
+ container +
+
+
+ + package: rclcpp_components + +
+
+
+
+ + + container... + + + + + + + +
+
+
+ control.launch.py +
+
+
+ + package: control_launch + +
+
+
+
+ + + control.launch.py... + + + + + + + + + + +
+
+
+ mpc_follower +
+
+
+ + + mpc_follower + + + + + + + + +
+
+
+ pure_pursuit +
+
+
+ + + pure_pursuit + + + + + + + +
+
+
+ $(var lateral_controller_mode) +
+
+
+ + + $(var lateral_controller_mode) + + + + + + + +
+
+
+ launch name +
+
+
+ + package: package name + +
+
+
+
+ + + launch name... + + + + + + + +
+
+
+ ex: +
+
+
+ + + ex: + + + + + + + +
+
+
+ node name +
+
+
+ + package: package name + +
+
+
+
+ + + node name... + + + + + + + +
+
+
+ latlon_muxer +
+
+
+ + package: latlon_muxer + +
+
+
+
+ + + latlon_muxer... + + + + + + + +
+
+
+ mpc_follower +
+
+
+ + package: mpc_follower + +
+
+
+
+ + + mpc_follower... + + + + + + + +
+
+
+ pure_pursuit +
+
+
+ + package: pure_pursuit + +
+
+
+
+ + + pure_pursuit... + + + + + + + +
+
+
+ velocity_controller +
+
+
+ + package: velocity_controller + +
+
+
+
+ + + velocity_controller... + + + + + + + +
+
+
+ lane_departure_checker_node +
+
+
+ + package: lane_departure_checker_node + +
+
+
+
+ + + lane_departure_checker_node... + + + + + + + +
+
+
+ shift_decider +
+
+
+ + package: shift_decider + +
+
+
+
+ + + shift_decider... + + + + + + + +
+
+
+ vehicle_cmd_gate +
+
+
+ + package: vehicle_cmd_gate + +
+
+
+
+ + + vehicle_cmd_gate... + + + + + + + +
+
+
+ external_cmd_converter +
+
+
+ + package: external_cmd_converter + +
+
+
+
+ + + external_cmd_converter... + + + + + + + +
+
+
+ external_cmd_selector +
+
+
+ + package: external_cmd_selector + +
+
+
+
+ + + external_cmd_selector... + + + + + + + +
+
+
+ other name +
+
+
+ + package: package name + +
+
+
+
+ + + other name... + + + + + + + + + Viewer does not support full SVG 1.1 + + + + diff --git a/launch/control_launch/launch/control.launch.py b/launch/control_launch/launch/control.launch.py new file mode 100644 index 0000000000000..7f33b89b5f414 --- /dev/null +++ b/launch/control_launch/launch/control.launch.py @@ -0,0 +1,356 @@ +# Copyright 2020 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 launch +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +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 +import yaml + + +def launch_setup(context, *args, **kwargs): + vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) + with open(vehicle_info_param_path, "r") as f: + vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] + lat_controller_param_path = LaunchConfiguration("lat_controller_param_path").perform(context) + with open(lat_controller_param_path, "r") as f: + lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] + 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 + ) + with open(vehicle_cmd_gate_param_path, "r") as f: + vehicle_cmd_gate_param = yaml.safe_load(f)["/**"]["ros__parameters"] + lane_departure_checker_param_path = LaunchConfiguration( + "lane_departure_checker_param_path" + ).perform(context) + with open(lane_departure_checker_param_path, "r") as f: + lane_departure_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + # lateral controller + lat_controller_component = ComposableNode( + package="trajectory_follower_nodes", + plugin="autoware::motion::control::trajectory_follower_nodes::LateralController", + name="lateral_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, + vehicle_info_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/slope_angle", "longitudinal/slope_angle"), + ("~/output/diagnostic", "longitudinal/diagnostic"), + ], + parameters=[ + lon_controller_param, + vehicle_info_param, + { + "control_rate": LaunchConfiguration("control_rate"), + "show_debug_info": LaunchConfiguration("show_debug_info"), + "enable_smooth_stop": LaunchConfiguration("enable_smooth_stop"), + "enable_pub_debug": LaunchConfiguration("enable_pub_debug"), + }, + ], + 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/control_cmd", "control_cmd"), + ], + parameters=[ + latlon_muxer_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # lane departure checker + lane_departure_component = ComposableNode( + package="lane_departure_checker", + plugin="lane_departure_checker::LaneDepartureCheckerNode", + name="lane_departure_checker_node", + namespace="trajectory_follower", + remappings=[ + ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/lanelet_map_bin", "/map/vector_map"), + ("~/input/route", "/planning/mission_planning/route"), + ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ], + parameters=[lane_departure_checker_param, vehicle_info_param], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # shift decider + shift_decider_component = ComposableNode( + package="shift_decider", + plugin="ShiftDecider", + name="shift_decider", + remappings=[ + ("input/control_cmd", "/control/trajectory_follower/control_cmd"), + ("output/gear_cmd", "/control/shift_decider/gear_cmd"), + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # vehicle cmd gate + vehicle_cmd_gate_component = ComposableNode( + package="vehicle_cmd_gate", + plugin="VehicleCmdGate", + name="vehicle_cmd_gate", + remappings=[ + ("input/emergency_state", "/system/emergency/emergency_state"), + ("input/steering", "/vehicle/status/steering_status"), + ("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"), + ("input/auto/gear_cmd", "/control/shift_decider/gear_cmd"), + ("input/external/control_cmd", "/external/selected/control_cmd"), + ("input/external/turn_indicators_cmd", "/external/selected/turn_indicators_cmd"), + ("input/external/hazard_lights_cmd", "/external/selected/hazard_lights_cmd"), + ("input/external/gear_cmd", "/external/selected/gear_cmd"), + ("input/external_emergency_stop_heartbeat", "/external/selected/heartbeat"), + ("input/gate_mode", "/control/gate_mode_cmd"), + ("input/emergency/control_cmd", "/system/emergency/control_cmd"), + ("input/emergency/hazard_lights_cmd", "/system/emergency/hazard_lights_cmd"), + ("input/emergency/gear_cmd", "/system/emergency/gear_cmd"), + ("output/vehicle_cmd_emergency", "/control/command/emergency_cmd"), + ("output/control_cmd", "/control/command/control_cmd"), + ("output/gear_cmd", "/control/command/gear_cmd"), + ("output/turn_indicators_cmd", "/control/command/turn_indicators_cmd"), + ("output/hazard_lights_cmd", "/control/command/hazard_lights_cmd"), + ("output/gate_mode", "/control/current_gate_mode"), + ("output/engage", "/api/autoware/get/engage"), + ("output/external_emergency", "/api/autoware/get/emergency"), + ("~/service/engage", "/api/autoware/set/engage"), + ("~/service/external_emergency", "/api/autoware/set/emergency"), + # TODO(Takagi, Isamu): deprecated + ("input/engage", "/autoware/engage"), + ("~/service/external_emergency_stop", "~/external_emergency_stop"), + ("~/service/clear_external_emergency_stop", "~/clear_external_emergency_stop"), + ], + parameters=[ + vehicle_cmd_gate_param, + vehicle_info_param, + { + "use_emergency_handling": LaunchConfiguration("use_emergency_handling"), + "use_external_emergency_stop": LaunchConfiguration("use_external_emergency_stop"), + "use_start_request": LaunchConfiguration("use_start_request"), + }, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # external cmd selector + external_cmd_selector_loader = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [FindPackageShare("external_cmd_selector"), "/launch/external_cmd_selector.launch.py"] + ), + launch_arguments=[ + ("use_intra_process", LaunchConfiguration("use_intra_process")), + ("target_container", "/control/control_container"), + ("initial_selector_mode", "remote"), + ], + ) + + # external cmd converter + external_cmd_converter_loader = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [FindPackageShare("external_cmd_converter"), "/launch/external_cmd_converter.launch.py"] + ), + launch_arguments=[ + ("use_intra_process", LaunchConfiguration("use_intra_process")), + ("target_container", "/control/control_container"), + ], + ) + + # set container to run all required components in the same process + container = ComposableNodeContainer( + name="control_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + lon_controller_component, + latlon_muxer_component, + lane_departure_component, + shift_decider_component, + vehicle_cmd_gate_component, + ], + ) + + # lateral controller is separated since it may be another controller (e.g. pure pursuit) + lat_controller_loader = LoadComposableNodes( + composable_node_descriptions=[lat_controller_component], + target_container=container, + # condition=LaunchConfigurationEquals("lateral_controller_mode", "mpc"), + ) + + group = GroupAction( + [ + PushRosNamespace("control"), + container, + external_cmd_selector_loader, + external_cmd_converter_loader, + lat_controller_loader, + ] + ) + + return [group] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + # add_launch_arg( + # "lateral_controller_mode", + # "mpc_follower", + # "lateral controller mode: `mpc_follower` or `pure_pursuit`", + # ) + + add_launch_arg( + "vehicle_info_param_file", + [ + FindPackageShare("vehicle_info_util"), + "/config/vehicle_info.param.yaml", + ], + "path to the parameter file of vehicle information", + ) + + add_launch_arg( + "lat_controller_param_path", + [ + FindPackageShare("control_launch"), + "/config/trajectory_follower/lateral_controller.param.yaml", + ], + "path to the parameter file of lateral controller", + ) + add_launch_arg( + "lon_controller_param_path", + [ + FindPackageShare("control_launch"), + "/config/trajectory_follower/longitudinal_controller.param.yaml", + ], + "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", + [ + FindPackageShare("control_launch"), + "/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml", + ], + "path to the parameter file of vehicle_cmd_gate", + ) + add_launch_arg( + "lane_departure_checker_param_path", + [FindPackageShare("lane_departure_checker"), "/config/lane_departure_checker.param.yaml"], + ) + + # velocity controller + add_launch_arg("control_rate", "30.0", "control rate") + add_launch_arg("show_debug_info", "false", "show debug information") + add_launch_arg( + "enable_smooth_stop", "true", "enable smooth stop (in velocity controller state)" + ) + add_launch_arg("enable_pub_debug", "true", "enable to publish debug information") + + # vehicle cmd gate + add_launch_arg("use_emergency_handling", "false", "use emergency handling") + add_launch_arg("use_external_emergency_stop", "true", "use external emergency stop") + add_launch_arg("use_start_request", "false", "use start request service") + + # external cmd selector + add_launch_arg("initial_selector_mode", "remote", "local or remote") + + # component + add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") + add_launch_arg("use_multithread", "false", "use multithread") + 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/launch/control_launch/package.xml b/launch/control_launch/package.xml new file mode 100644 index 0000000000000..ed1180a06e74e --- /dev/null +++ b/launch/control_launch/package.xml @@ -0,0 +1,27 @@ + + + + control_launch + 0.1.0 + The control_launch package + + Takamasa Horibe + + Apache License 2.0 + + ament_cmake_auto + + external_cmd_converter + lane_departure_checker + shift_decider + trajectory_follower + trajectory_follower_nodes + vehicle_cmd_gate + + ament_lint_auto + autoware_lint_common + + + ament_cmake + +