Port pure pursuit (autowarefoundation#166)

* Fix control.launch.py

* Apply pre-commit

* Fix control.launch.py

* Change debug topic name in rviz setting

autoware_launch/rviz/autoware.rviz

@@ -1421,7 +1421,7 @@ Visualization Manager:
             Durability Policy: Volatile
             History Policy: Keep Last
             Reliability Policy: Reliable
-            Value: /control/trajectory_follower/pure_pursuit/debug/markers
+            Value: /control/trajectory_follower/pure_pursuit_node_exe/debug/markers
           Value: false
       Enabled: true
       Name: Control

control_launch/launch/control.launch.py

@@ -19,6 +19,7 @@
 from launch.actions import OpaqueFunction
 from launch.actions import SetLaunchConfiguration
 from launch.conditions import IfCondition
+from launch.conditions import LaunchConfigurationEquals
 from launch.conditions import UnlessCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
@@ -31,9 +32,21 @@
 
 
 def launch_setup(context, *args, **kwargs):
-    lat_controller_param_path = LaunchConfiguration("lat_controller_param_path").perform(context)
-    with open(lat_controller_param_path, "r") as f:
-        lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    lateral_controller_mode = LaunchConfiguration("lateral_controller_mode").perform(context)
+    if lateral_controller_mode == "mpc_follower":
+        lat_controller_param_path = (
+            FindPackageShare("control_launch").perform(context)
+            + "/config/trajectory_follower/lateral_controller.param.yaml"
+        )
+        with open(lat_controller_param_path, "r") as f:
+            lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    elif lateral_controller_mode == "pure_pursuit":
+        lat_controller_param_path = (
+            FindPackageShare("control_launch").perform(context)
+            + "/config/pure_pursuit/pure_pursuit.param.yaml"
+        )
+        with open(lat_controller_param_path, "r") as f:
+            lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     lon_controller_param_path = LaunchConfiguration("lon_controller_param_path").perform(context)
     with open(lon_controller_param_path, "r") as f:
         lon_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"]
@@ -53,7 +66,7 @@ def launch_setup(context, *args, **kwargs):
         lane_departure_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
     # lateral controller
-    lat_controller_component = ComposableNode(
+    mpc_follower_component = ComposableNode(
         package="trajectory_follower_nodes",
         plugin="autoware::motion::control::trajectory_follower_nodes::LateralController",
         name="lateral_controller_node_exe",
@@ -71,6 +84,21 @@ 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=[
+            lat_controller_param,
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
 
     # longitudinal controller
     lon_controller_component = ComposableNode(
@@ -217,7 +245,7 @@ def launch_setup(context, *args, **kwargs):
     )
 
     # set container to run all required components in the same process
-    container = ComposableNodeContainer(
+    mpc_follower_container = ComposableNodeContainer(
         name="control_container",
         namespace="",
         package="rclcpp_components",
@@ -229,22 +257,43 @@ def launch_setup(context, *args, **kwargs):
             shift_decider_component,
             vehicle_cmd_gate_component,
         ],
+        condition=LaunchConfigurationEquals("lateral_controller_mode", "mpc_follower"),
+    )
+    pure_pursuit_container = ComposableNodeContainer(
+        name="control_container",
+        namespace="",
+        package="rclcpp_components",
+        executable=LaunchConfiguration("container_executable"),
+        composable_node_descriptions=[
+            lon_controller_component,
+            latlon_muxer_component,
+            shift_decider_component,
+            vehicle_cmd_gate_component,
+        ],
+        condition=LaunchConfigurationEquals("lateral_controller_mode", "pure_pursuit"),
     )
 
     # 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"),
+    mpc_follower_loader = LoadComposableNodes(
+        composable_node_descriptions=[mpc_follower_component],
+        target_container=mpc_follower_container,
+        condition=LaunchConfigurationEquals("lateral_controller_mode", "mpc_follower"),
+    )
+    pure_pursuit_loader = LoadComposableNodes(
+        composable_node_descriptions=[pure_pursuit_component],
+        target_container=pure_pursuit_container,
+        condition=LaunchConfigurationEquals("lateral_controller_mode", "pure_pursuit"),
     )
 
     group = GroupAction(
         [
             PushRosNamespace("control"),
-            container,
+            mpc_follower_container,
+            pure_pursuit_container,
             external_cmd_selector_loader,
             external_cmd_converter_loader,
-            lat_controller_loader,
+            mpc_follower_loader,
+            pure_pursuit_loader,
         ]
     )
 
@@ -259,20 +308,14 @@ def add_launch_arg(name: str, default_value=None, description=None):
             DeclareLaunchArgument(name, default_value=default_value, description=description)
         )
 
-    # add_launch_arg(
-    #     "lateral_controller_mode",
-    #     "mpc_follower",
-    #     "lateral controller mode: `mpc_follower` or `pure_pursuit`",
-    # )
-
+    # lateral controller mode
     add_launch_arg(
-        "lat_controller_param_path",
-        [
-            FindPackageShare("control_launch"),
-            "/config/trajectory_follower/lateral_controller.param.yaml",
-        ],
-        "path to the parameter file of lateral controller",
+        "lateral_controller_mode",
+        "mpc_follower",
+        "lateral controller mode: `mpc_follower` or `pure_pursuit`",
     )
+
+    # parameter file path
     add_launch_arg(
         "lon_controller_param_path",
         [