add pure_pursuit as lateral controller into launch files

Signed-off-by: Berkay <[email protected]>

control/pure_pursuit/CMakeLists.txt

@@ -56,4 +56,5 @@ endif()
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )

launch/tier4_control_launch/launch/control.launch.py

@@ -31,6 +31,7 @@
 
 
 def launch_setup(context, *args, **kwargs):
+    lateral_controller_mode = LaunchConfiguration("lateral_controller_mode").perform(context)
     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"]
@@ -43,7 +44,9 @@ def launch_setup(context, *args, **kwargs):
     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"]
-
+    pure_pursuit_param_path = LaunchConfiguration("pure_pursuit_param_path").perform(context)
+    with open(pure_pursuit_param_path, "r") as f:
+        pure_pursuit_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     vehicle_cmd_gate_param_path = LaunchConfiguration("vehicle_cmd_gate_param_path").perform(
         context
     )
@@ -76,6 +79,22 @@ def launch_setup(context, *args, **kwargs):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    pure_pursuit_component = ComposableNode(
+        package="pure_pursuit",
+        plugin="pure_pursuit::PurePursuitNode",
+        name="pure_pursuit_node_exe",
+        namespace="trajectory_follower",
+        remappings=[
+            ("input/reference_trajectory", "/planning/scenario_planning/trajectory"),
+            ("input/current_odometry", "/localization/kinematic_state"),
+            ("output/control_raw", "lateral/control_cmd"),
+        ],
+        parameters=[
+            pure_pursuit_param,
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
     # longitudinal controller
     lon_controller_component = ComposableNode(
         package="trajectory_follower_nodes",
@@ -238,11 +257,19 @@ def launch_setup(context, *args, **kwargs):
     )
 
     # 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"),
-    )
+    if lateral_controller_mode == "mpc_follower":
+        lat_controller_loader = LoadComposableNodes(
+            composable_node_descriptions=[lat_controller_component],
+            target_container=container,
+            # condition=LaunchConfigurationEquals("lateral_controller_mode", "mpc"),
+        )
+    elif lateral_controller_mode == "pure_pursuit":
+        lat_controller_loader = LoadComposableNodes(
+            composable_node_descriptions=[pure_pursuit_component],
+            target_container=container,
+            # condition=LaunchConfigurationEquals("lateral_controller_mode", "mpc"),
+        )
+
 
     group = GroupAction(
         [
@@ -271,6 +298,9 @@ def add_launch_arg(name: str, default_value=None, description=None):
     #     "lateral controller mode: `mpc_follower` or `pure_pursuit`",
     # )
 
+    add_launch_arg("lateral_controller_mode", "mpc_follower", "lateral controller mode: `mpc_follower` or `pure_pursuit`")
+
+
     add_launch_arg(
         "vehicle_info_param_file",
         [
@@ -288,6 +318,14 @@ def add_launch_arg(name: str, default_value=None, description=None):
         ],
         "path to the parameter file of lateral controller",
     )
+    add_launch_arg(
+        "pure_pursuit_param_path",
+        [
+            FindPackageShare("pure_pursuit"),
+            "/config/pure_pursuit.param.yaml",
+        ],
+        "path to the parameter file of lateral controller",
+    )
     add_launch_arg(
         "lon_controller_param_path",
         [
@@ -325,6 +363,8 @@ def add_launch_arg(name: str, default_value=None, description=None):
     )
     add_launch_arg("enable_pub_debug", "true", "enable to publish debug information")
 
+    # lateral controller
+
     # 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")