Feature/expand footprint launcher (#318)

control_launch/launch/control.launch.py

@@ -46,6 +46,10 @@ def launch_setup(context, *args, **kwargs):
         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']
     # mpc follower
     mpc_follower_component = ComposableNode(
         package='mpc_follower',
@@ -138,12 +142,10 @@ def launch_setup(context, *args, **kwargs):
             ('~/input/route', '/planning/mission_planning/route'),
             ('~/input/reference_trajectory', '/planning/scenario_planning/trajectory'),
             ('~/input/predicted_trajectory', '/control/trajectory_follower/predicted_trajectory'),
+            ('~/input/covariance', '/localization/pose_with_covariance')
         ],
         parameters=[
-            [
-                FindPackageShare('lane_departure_checker'),
-                '/config/lane_departure_checker.param.yaml'
-            ]
+            lane_departure_checker_param
         ],
         extra_arguments=[{
             'use_intra_process_comms': LaunchConfiguration('use_intra_process')
@@ -352,6 +354,13 @@ def add_launch_arg(name: str, default_value=None, description=None):
         ],
         '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')

localization_launch/launch/localization.launch.xml

@@ -31,11 +31,5 @@
     <include file="$(find-pkg-share localization_error_monitor)/launch/localization_error_monitor.launch.xml">
       <arg name="input/pose_with_cov" value="pose_twist_fusion_filter/pose_with_covariance" />
     </include>
-    <!-- final twist output of localization -->
-    <node pkg="topic_tools" exec="relay" name="relay">
-      <param name="input_topic" value="/localization/pose_twist_fusion_filter/twist" />
-      <param name="output_topic" value="/localization/twist" />
-      <param name="type" value="geometry_msgs/msg/TwistStamped" />
-    </node>
   </group>
 </launch>

localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml

@@ -12,10 +12,10 @@
     <arg name="input_twist_name" value="/localization/twist_estimator/twist"/>
     <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
     <arg name="output_pose_name" value="pose"/>
-    <arg name="output_pose_with_covariance_name" value="pose_with_covariance"/>
+    <arg name="output_pose_with_covariance_name" value="/localization/pose_with_covariance"/>
     <arg name="output_pose_without_yawbias_name" value="pose_without_yawbias"/>
     <arg name="output_pose_with_covariance_without_yawbias_name" value="pose_with_covariance_without_yawbias"/>
-    <arg name="output_twist_name" value="twist"/>
+    <arg name="output_twist_name" value="/localization/twist"/>
     <arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
 
     <arg name="twist_stddev_wz" value="0.003 "/>