Ros2 v0.8.0 control launch (autowarefoundation#53)

* restore file name for v0.8.0 update Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Add use_emergency_handling arg to vehicle_cmd_gate.launch (autowarefoundation#83) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add parameter for using steer prediction (autowarefoundation#88) * change stop dist param (autowarefoundation#85) * mpc: change param name steer_rate_lim_deg -> steer_rate_lim_dps (autowarefoundation#79) Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * add mpc parameter (autowarefoundation#105) * Add parameters for stop state (autowarefoundation#126) * Add parameters for stop state * Change default value * Add vehicle cmd gate config (autowarefoundation#136) * Add config_file of vehicle_cmd_gate Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add use_emergency_stop Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Rename emergency_stop to external_emergency_stop Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix command_gate diag Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add lane_departure_checker (autowarefoundation#123) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Revert "restore file name for v0.8.0 update" This reverts commit 516b366819f7f9d69b9bc3e2de180d4523794bcd. * fix control launch Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix args Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix param type Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix lane departure checker Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
tkimura4 · Dec 10, 2021 · 20de8b5 · 20de8b5
1 parent c99b6f6
commit 20de8b5
Show file tree

Hide file tree

Showing 5 changed files with 47 additions and 4 deletions.
diff --git a/launch/control_launch/config/mpc_follower/mpc_follower_param.yaml b/launch/control_launch/config/mpc_follower/mpc_follower_param.yaml
@@ -4,6 +4,7 @@
     ctrl_period: 0.03               # control period [s]
     traj_resample_dist: 0.1         # path resampling interval [m]
     enable_yaw_recalculation: false # flag for recalculation of yaw angle after resampling
+    use_steer_prediction: false     # flag for using steer prediction (do not use steer measurement)
     admisible_position_error: 5.0   # stop mpc calculation when error is larger than the following value
     admisible_yaw_error_: 1.57      # stop mpc calculation when error is larger than the following value
 
@@ -25,6 +26,15 @@
     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.00                   # threshold of curvature to use "low vurvature" parameter (0: do not use low curvature parameter)
     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
@@ -34,9 +44,14 @@
     input_delay: 0.24     # steering input delay time for delay compensation
     vehicle_model_steer_tau: 0.3     # steering dynamics time constant (1d approzimation) [s]
     steer_lim_deg: 40.0              # steering angle limit [deg]
-    steer_rate_lim_deg: 600.0        # steering angle rate limit [deg/s]
+    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]
+
+    # stop state
+    stop_state_entry_ego_speed: 0.2
+    stop_state_entry_target_speed: 0.5
+    stop_state_keep_stopping_dist: 0.5
diff --git a/launch/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.yaml b/launch/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.yaml
@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+    update_rate: 10.0
+    system_emergency_heartbeat_timeout: 0.5
+    external_emergency_stop_heartbeat_timeout: 0.0
+
+    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/config/velocity_controller/velocity_controller_param.yaml b/launch/control_launch/config/velocity_controller/velocity_controller_param.yaml
@@ -20,7 +20,7 @@
 
     # smooth stop
     smooth_stop:
-      stop_dist: 3.0
+      stop_dist: 0.5
       exit_ego_speed: 1.0
       entry_ego_speed: 0.8
       exit_target_speed: 1.0

diff --git a/launch/control_launch/launch/control.launch.xml b/launch/control_launch/launch/control.launch.xml
@@ -10,6 +10,7 @@
   <arg name="mpc_follower_param_path" default="$(find-pkg-share control_launch)/config/mpc_follower/mpc_follower_param.yaml"/>
   <arg name="pure_pursuit_param_path" default="$(find-pkg-share control_launch)/config/pure_pursuit/pure_pursuit_param.yaml"/>
   <arg name="velocity_controller_param_path" default="$(find-pkg-share control_launch)/config/velocity_controller/velocity_controller_param.yaml"/>
+  <arg name="vehicle_cmd_gate_param_path" default="$(find-pkg-share control_launch)/config/vehicle_cmd_gate/vehicle_cmd_gate.yaml"/>
 
   <!-- control module -->
   <group>
@@ -21,16 +22,23 @@
 
       <!-- lateral controller -->
       <group if="$(eval &quot;'$(var lateral_controller_mode)'=='mpc_follower'&quot;)">
-        <include file="$(find-pkg-share mpc_follower)/launch/mpc_follower.launch.xml" pass_all_args="true"/>
+        <include file="$(find-pkg-share mpc_follower)/launch/mpc_follower.launch.xml">
+          <arg name="mpc_follower_param_path" value="$(var mpc_follower_param_path)" />
+          <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
+        </include>
       </group>
 
       <group if="$(eval &quot;'$(var lateral_controller_mode)'=='pure_pursuit'&quot;)">
-        <include file="$(find-pkg-share pure_pursuit)/launch/pure_pursuit.launch.xml" pass_all_args="true"/>
+        <include file="$(find-pkg-share pure_pursuit)/launch/pure_pursuit.launch.xml">
+          <arg name="pure_pursuit_param_path" value="$(var pure_pursuit_param_path)" />
+          <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
+        </include>
       </group>
 
       <!-- longitudinal controller -->
       <include file="$(find-pkg-share velocity_controller)/launch/velocity_controller.launch.xml">
         <arg name="velocity_controller_param_path" value="$(var velocity_controller_param_path)"/>
+        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
       </include>
 
       <!-- latlon coupler -->
@@ -39,6 +47,11 @@
         <remap from="input/longitudinal/control_cmd" to="longitudinal/control_cmd"/>
         <remap from="output/control_cmd" to="control_cmd"/>
       </node>
+
+      <!-- lane departure checker -->
+      <include file="$(find-pkg-share lane_departure_checker)/launch/lane_departure_checker.launch.xml">
+        <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
+      </include>
     </group>
 
     <!-- shift decider -->
@@ -48,6 +61,9 @@
     <!-- vehicle cmd gate -->
     <include file="$(find-pkg-share vehicle_cmd_gate)/launch/vehicle_cmd_gate.launch.xml">
       <arg name="vehicle_param_file" value="$(var vehicle_param_file)" />
+      <arg name="config_file" value="$(var vehicle_cmd_gate_param_path)"/>
+      <arg name="use_emergency_handling" value="false"/>
+      <arg name="use_external_emergency_stop" value="true"/>
     </include>
 
     <!-- remote_cmd_converter -->

diff --git a/launch/control_launch/package.xml b/launch/control_launch/package.xml
@@ -18,6 +18,7 @@
   <exec_depend>shift_decider</exec_depend>
   <exec_depend>vehicle_cmd_gate</exec_depend>
   <exec_depend>velocity_controller</exec_depend>
+  <exec_depend>lane_departure_checker</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>