Merge branch '1-add-sensing-launch' of github.com:1222-takeshi/autowa…

…re.universe into 1-add-sensing-launch

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
+)

launch/control_launch/README.md

@@ -0,0 +1,24 @@
+# control_launch
+
+## Structure
+
+![control_launch](./control_launch.drawio.svg)
+
+## Package Dependencies
+
+Please see `<exec_depend>` in `package.xml`.
+
+## Usage
+
+You can include as follows in `*.launch.xml` to use `control.launch.py`.
+
+```xml
+<include file="$(find-pkg-share control_launch)/launch/control.launch.py">
+  <!-- options for lateral_controller_mode: mpc_follower, pure_pursuit -->
+  <arg name="lateral_controller_mode" value="mpc_follower" />
+</include>
+```
+
+## 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 )

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

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.

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

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