Merge pull request #120 from tier4/sync-upstream-develop

sync upstream develop

control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml

@@ -3,7 +3,8 @@
     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

control_launch/launch/control.launch.py

@@ -172,18 +172,18 @@ def launch_setup(context, *args, **kwargs):
         name='vehicle_cmd_gate',
         remappings=[
             ('input/system_emergency', '/system/emergency/is_emergency'),
-            ('input/external_emergency_stop', '/remote/emergency_stop'),
             ('input/emergency', '/system/emergency/is_emergency'),
-            ('input/gate_mode', '/remote/gate_mode_cmd'),
             ('input/steering', '/vehicle/status/steering'),
 
             ('input/auto/control_cmd', 'trajectory_follower/control_cmd'),
             ('input/auto/turn_signal_cmd', '/planning/turn_signal_decider/turn_signal_cmd'),
             ('input/auto/shift_cmd', '/control/shift_decider/shift_cmd'),
 
-            ('input/remote/control_cmd', '/external/external_cmd_selector/control_cmd'),
-            ('input/remote/turn_signal_cmd', '/external/external_cmd_selector/turn_signal_cmd'),
-            ('input/remote/shift_cmd', '/external/external_cmd_selector/shift_cmd'),
+            ('input/external/control_cmd', '/external/selected/control_cmd'),
+            ('input/external/turn_signal_cmd', '/external/selected/turn_signal_cmd'),
+            ('input/external/shift_cmd', '/external/selected/shift_cmd'),
+            ('input/external_emergency_stop', '/external/selected/heartbeat'),
+            ('input/gate_mode', '/control/gate_mode_cmd'),
 
             ('input/emergency/control_cmd', '/system/emergency/control_cmd'),
             ('input/emergency/turn_signal_cmd', '/system/emergency/turn_signal_cmd'),
@@ -223,18 +223,20 @@ def launch_setup(context, *args, **kwargs):
         plugin='ExternalCmdSelector',
         name='external_cmd_selector',
         remappings=[
+            ('~/service/select_external_command', '~/select_external_command'),
             ('~/input/local/control_cmd', '/external/local/control_cmd'),
             ('~/input/local/shift_cmd', '/external/local/shift_cmd'),
             ('~/input/local/turn_signal_cmd', '/external/local/turn_signal_cmd'),
+            ('~/input/local/heartbeat', '/external/local/heartbeat'),
             ('~/input/remote/control_cmd', '/external/remote/control_cmd'),
             ('~/input/remote/shift_cmd', '/external/remote/shift_cmd'),
             ('~/input/remote/turn_signal_cmd', '/external/remote/turn_signal_cmd'),
+            ('~/input/remote/heartbeat', '/external/remote/heartbeat'),
+            ('~/output/control_cmd', '/external/selected/external_control_cmd'),
+            ('~/output/shift_cmd', '/external/selected/shift_cmd'),
+            ('~/output/turn_signal_cmd', '/external/selected/turn_signal_cmd'),
+            ('~/output/heartbeat', '/external/selected/heartbeat'),
             ('~/output/current_selector_mode', '~/current_selector_mode'),
-            ('~/output/external_control_cmd',
-             '/external/external_cmd_selector/external_control_cmd'),
-            ('~/output/shift_cmd', '/external/external_cmd_selector/shift_cmd'),
-            ('~/output/turn_signal_cmd', '/external/external_cmd_selector/turn_signal_cmd'),
-            ('~/service/select_external_command', '~/select_external_command'),
         ],
         parameters=[
             {
@@ -247,18 +249,18 @@ def launch_setup(context, *args, **kwargs):
     )
 
     # remote cmd converter
-    remote_cmd_converter_component = ComposableNode(
-        package='remote_cmd_converter',
-        plugin='remote_cmd_converter::RemoteCmdConverterNode',
-        name='remote_cmd_converter',
+    external_cmd_converter_component = ComposableNode(
+        package='external_cmd_converter',
+        plugin='external_cmd_converter::ExternalCmdConverterNode',
+        name='external_cmd_converter',
         remappings=[
-            ('in/external_control_cmd', '/external/external_cmd_selector/external_control_cmd'),
-            ('in/shift_cmd', '/external/external_cmd_selector/shift_cmd'),
+            ('in/external_control_cmd', '/external/selected/external_control_cmd'),
+            ('in/shift_cmd', '/external/selected/shift_cmd'),
             ('in/emergency_stop', '/remote/emergency_stop'),
             ('in/current_gate_mode', '/control/current_gate_mode'),
             ('in/twist', '/localization/twist'),
-            ('out/control_cmd', '/external/external_cmd_selector/control_cmd'),
-            ('out/latest_remote_control_cmd', '/remote/latest_remote_control_cmd'),
+            ('out/control_cmd', '/external/selected/control_cmd'),
+            ('out/latest_external_control_cmd', '/external/selected/latest_external_control_cmd'),
         ],
         parameters=[
             {
@@ -288,7 +290,7 @@ def launch_setup(context, *args, **kwargs):
             lane_departure_component,
             shift_decider_component,
             vehicle_cmd_gate_component,
-            remote_cmd_converter_component,
+            external_cmd_converter_component,
             external_cmd_selector_component,
         ],
     )
@@ -377,7 +379,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg('use_start_request', 'false', 'use start request service')
 
     # external cmd selector
-    add_launch_arg('initial_selector_mode', '1', '0: Local, 1: Remote')
+    add_launch_arg('initial_selector_mode', 'remote', 'local or remote')
 
     # remote cmd converter
     add_launch_arg(

control_launch/launch/control.launch.xml

@@ -58,43 +58,24 @@
       <arg name="use_external_emergency_stop" value="true"/>
     </include>
 
-    <!-- For backward compatibility of external commands -->
-    <group>
-      <node pkg="topic_tools" exec="relay" name="relay_remote_remote_control_cmd">
-        <param name="input_topic" value="/remote/external_control_cmd" />
-        <param name="output_topic" value="/external/remote/external_control_cmd" />
-        <param name="type" value="autoware_vehicle_msgs/msg/RawControlCommandStamped" />
-      </node>
-      <node pkg="topic_tools" exec="relay" name="relay_remote_shift_cmd">
-        <param name="input_topic" value="/remote/shift_cmd" />
-        <param name="output_topic" value="/external/remote/shift_cmd" />
-        <param name="type" value="autoware_vehicle_msgs/msg/ShiftStamped" />
-      </node>
-      <node pkg="topic_tools" exec="relay" name="relay_remote_turn_signal_cmd">
-        <param name="input_topic" value="/remote/turn_signal_cmd" />
-        <param name="output_topic" value="/external/remote/turn_signal_cmd" />
-        <param name="type" value="autoware_vehicle_msgs/msg/TurnSignal" />
-      </node>
-    </group>
-
     <!-- external_cmd_selector -->
     <include file="$(find-pkg-share external_cmd_selector)/launch/external_cmd_selector.launch.xml">
-      <arg name="initial_selector_mode" value="1" />
-
+      <arg name="initial_selector_mode" value="remote" />
       <arg name="input/local/control_cmd" value="/external/local/control_cmd" />
       <arg name="input/local/shift_cmd" value="/external/local/shift_cmd" />
       <arg name="input/local/turn_signal_cmd" value="/external/local/turn_signal_cmd" />
-
+      <arg name="input/local/heartbeat" value="/external/local/heartbeat" />
       <arg name="input/remote/control_cmd" value="/external/remote/control_cmd" />
       <arg name="input/remote/shift_cmd" value="/external/remote/shift_cmd" />
       <arg name="input/remote/turn_signal_cmd" value="/external/remote/turn_signal_cmd" />
+      <arg name="input/remote/heartbeat" value="/external/remote/heartbeat" />
     </include>
 
-    <!-- remote_cmd_converter -->
-    <include file="$(find-pkg-share remote_cmd_converter)/launch/remote_cmd_converter.launch.xml">
-      <arg name="in/external_control_cmd" value="/external/external_cmd_selector/external_control_cmd" />
-      <arg name="in/shift_cmd" value="/external/external_cmd_selector/shift_cmd" />
-      <arg name="out/control_cmd" value="/external/external_cmd_selector/control_cmd" />
+    <!-- external_cmd_converter -->
+    <include file="$(find-pkg-share external_cmd_converter)/launch/external_cmd_converter.launch.xml">
+      <arg name="in/external_control_cmd" value="/external/selected/external_control_cmd" />
+      <arg name="in/shift_cmd" value="/external/selected/shift_cmd" />
+      <arg name="out/control_cmd" value="/external/selected/control_cmd" />
     </include>
   </group>
 </launch>

control_launch/package.xml

@@ -14,7 +14,7 @@
   <exec_depend>latlon_muxer</exec_depend>
   <exec_depend>mpc_follower</exec_depend>
   <exec_depend>pure_pursuit</exec_depend>
-  <exec_depend>remote_cmd_converter</exec_depend>
+  <exec_depend>external_cmd_converter</exec_depend>
   <exec_depend>shift_decider</exec_depend>
   <exec_depend>vehicle_cmd_gate</exec_depend>
   <exec_depend>velocity_controller</exec_depend>

planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml

@@ -0,0 +1,25 @@
+/**:
+  ros__parameters:
+    resample:
+      ds_resample: 0.1
+      num_resample: 1
+      delta_yaw_threshold: 0.785
+
+    latacc:
+      enable_constant_velocity_while_turning: false
+      constant_velocity_dist_threshold: 2.0
+
+    forward:
+      max_acc: 1.0
+      min_acc: -1.0
+      max_jerk: 0.3
+      min_jerk: -0.3
+      kp: 0.3
+
+    backward:
+      start_jerk: -0.1
+      min_jerk_mild_stop: -0.3
+      min_jerk: -1.5
+      min_acc_mild_stop: -1.0
+      min_acc: -2.5
+      span_jerk: -0.01

planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml

@@ -65,7 +65,8 @@
     # trajectory param for mpt
     num_curvature_sampling_points: 5 # number of sampling points when calculating curvature
     delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt[m]
-    forward_fixing_mpt_distance: 10 # number of fixing points around ego vehicle
+    forward_fixing_mpt_min_distance: 10 # number of fixing points around ego vehicle
+    forward_fixing_mpt_time: 1.0 # forward fixing time with current velocity [s]
 
     # optimization param for mpt
     is_hard_fixing_terminal_point: false # hard fixing terminal point
@@ -82,6 +83,7 @@
     terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point
     terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point
     zero_ff_steer_angle: 0.5 # threshold that feed-forward angle becomes zero
+    optimization_center_offset: 1.5
 
     # replanning & trimming trajectory param outside algorithm
     min_delta_dist_for_replan: 10.0 # minimum delta dist thres for replan[m]

planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml

@@ -7,7 +7,8 @@
       consider_obj_velocity: true # consider forward vehicle velocity to ACC or not
 
       # general parameter for ACC
-      obstacle_stop_velocity_thresh: 2.0 # threshold of forward obstacle velocity to insert stop line (to stop acc) [m/s]
+      obstacle_velocity_thresh_to_start_acc: 1.5 # start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s]
+      obstacle_velocity_thresh_to_stop_acc: 1.0 # stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s]
       emergency_stop_acceleration: -4.5 # supposed minimum acceleration (deceleration) in emergency stop [m/ss]
       emergency_stop_idling_time: 0.5 # supposed idling time to start emergency stop [s]
       min_dist_stop: 4.0 # minimum distance of emergency stop [m]

planning_launch/launch/planning.launch.xml

@@ -19,5 +19,12 @@
       <include file="$(find-pkg-share planning_launch)/launch/scenario_planning/scenario_planning.launch.xml">
       </include>
     </group>
+
+    <!-- planning error monitor -->
+    <group>
+      <push-ros-namespace namespace="planning_diagnostics"/>
+      <include file="$(find-pkg-share planning_launch)/launch/planning_diagnostics/planning_error_monitor.launch.xml">
+      </include>
+    </group>    
   </group>
 </launch>

planning_launch/launch/planning_diagnostics/planning_error_monitor.launch.xml

@@ -0,0 +1,8 @@
+<launch>
+
+  <!-- planning_error_monitor -->
+  <group>
+    <include file="$(find-pkg-share planning_error_monitor)/launch/planning_error_monitor.launch.xml">
+    </include>
+  </group>
+</launch>