port platooning_control to ros2

carma/launch/guidance.launch

@@ -41,7 +41,7 @@
   <remap from="heading" to="$(optenv CARMA_INTR_NS)/gnss/heading_raw"/>
   <remap from="nav_sat_fix" to="$(optenv CARMA_INTR_NS)/gnss/fix_raw"/>
   <remap from="velocity" to="$(optenv CARMA_INTR_NS)/gnss/vel_raw"/>
-  
+
   <remap from="/republish/cmd_lateral" to="$(optenv CARMA_INTR_NS)/controller/cmd_lateral"/>
   <remap from="/republish/cmd_longitudinal_effort" to="$(optenv CARMA_INTR_NS)/controller/cmd_longitudinal_effort"/>
   <remap from="/republish/cmd_speed" to="$(optenv CARMA_INTR_NS)/controller/cmd_speed"/>
@@ -58,15 +58,15 @@
   <remap from="get_transform" to="$(optenv CARMA_TF_NS)/get_transform"/>
 
   <!-- TODO Look into if there is a better way for handling global prameters -->
-  <remap from="~vehicle_id" to="/vehicle_id"/> 
-  <remap from="vehicle_state_machine_type" to="/vehicle_state_machine_type"/> 
-  <remap from="vehicle_length" to="/vehicle_length"/> 
-  <remap from="vehicle_width" to="/vehicle_width"/> 
+  <remap from="~vehicle_id" to="/vehicle_id"/>
+  <remap from="vehicle_state_machine_type" to="/vehicle_state_machine_type"/>
+  <remap from="vehicle_length" to="/vehicle_length"/>
+  <remap from="vehicle_width" to="/vehicle_width"/>
 
   <remap from="system_alert" to="/system_alert"/>
 
-  <remap from="/base/lane_waypoints_raw" to="base/lane_waypoints_raw"/> 
-  <remap from="/based/lane_waypoints_raw" to="base/lane_waypoints_raw"/>      
+  <remap from="/base/lane_waypoints_raw" to="base/lane_waypoints_raw"/>
+  <remap from="/based/lane_waypoints_raw" to="base/lane_waypoints_raw"/>
   <remap from="/lane_waypoints_array" to="lane_waypoints_array"/>
   <remap from="/traffic_waypoints_array" to="traffic_waypoints_array"/>
   <remap from="/red_waypoints_array" to="red_waypoints_array"/>
@@ -87,7 +87,7 @@
   <remap from="incoming_spat" to="$(optenv CARMA_MSG_NS)/incoming_spat"/>
 
   <remap from="/vehicle_status" to="$(optenv CARMA_INTR_NS)/vehicle_status"/>
-  <remap from="/current_pose" to="$(optenv CARMA_LOCZ_NS)/current_pose"/>  
+  <remap from="/current_pose" to="$(optenv CARMA_LOCZ_NS)/current_pose"/>
 
   <!-- Planning Stack -->
   <group ns="plugins">
@@ -97,15 +97,14 @@
     <remap from="plugin_discovery" to="$(optenv CARMA_GUIDE_NS)/plugin_discovery"/>
     <remap from="ctrl_raw" to="$(optenv CARMA_GUIDE_NS)/ctrl_raw"/>
     <remap from="twist_raw" to="$(optenv CARMA_GUIDE_NS)/twist_raw"/>
-      
+
     <!--Platooning Control Plugin-->
-    <include file="$(find platoon_control)/launch/platoon_control.launch" />
 
     <!--Platooning Control IHP Plugin-->
     <include file="$(find platoon_control_ihp)/launch/platoon_control_ihp.launch" />
 
   </group>
-  
+
   <!-- Guidance Plugin Validator -->
   <group>
     <remap from="plugin_discovery" to="$(optenv CARMA_GUIDE_NS)/plugin_discovery"/>

carma/launch/plugins.launch.py

@@ -90,6 +90,9 @@ def generate_launch_description():
 
     lci_plugin_calibration_params = [vehicle_calibration_dir, "/identifiers/UniqueVehicleParams.yaml"]
 
+    platoon_control_param_file = os.path.join(
+        get_package_share_directory('platoon_control'), 'config/parameters.yaml')
+
     carma_inlanecruising_plugin_container = ComposableNodeContainer(
         package='carma_ros2_utils',
         name='carma_lainlanecruising_plugin_container',
@@ -527,6 +530,33 @@ def generate_launch_description():
         ]
     )
 
+    platooning_control_plugin_container = ComposableNodeContainer(
+        package='carma_ros2_utils',
+        name='platoon_control_container',
+        executable='carma_component_container_mt',
+        namespace=GetCurrentNamespace(),
+        composable_node_descriptions=[
+            ComposableNode(
+                package='platoon_control',
+                plugin='platoon_control::PlatoonControlPlugin',
+                name='platoon_control',
+                extra_arguments=[
+                    {'use_intra_process_comms': True},
+                    {'--log-level' : GetLogLevel('platooning_control_plugin', env_log_levels) }
+                ],
+                remappings = [
+                    ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ),
+                    ("ctrl_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/ctrl_raw" ] ),
+                    ("twist_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/twist_raw" ] ),
+                    ("platoon_control/plan_trajectory", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/platoon_control/plan_trajectory" ] ),
+                    ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
+                    ("vehicle/twist", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
+                ],
+                parameters=[ platoon_control_param_file, vehicle_config_param_file ]
+            )
+        ]
+    )
+
     carma_stop_and_dwell_strategic_plugin_container = ComposableNodeContainer(
         package='carma_ros2_utils',
         name='carma_stop_and_dwell_strategic_plugin_container',
@@ -597,6 +627,7 @@ def generate_launch_description():
         carma_pure_pursuit_wrapper_container,
         #platooning_strategic_plugin_container,
         platooning_tactical_plugin_container,
+        platooning_control_plugin_container,
         intersection_transit_maneuvering_container
 
     ])

carma_guidance_plugins/include/carma_guidance_plugins/control_plugin.hpp

@@ -28,12 +28,12 @@ namespace carma_guidance_plugins
 {
 
   /**
-   * \brief ControlPlugin base class which can be extended by user provided plugins which wish to implement the Control Plugin ROS API. 
-   * 
-   * A control plugin is responsible for generating high frequency vehicle speed and steering commands to execute the currently planned trajectory. 
-   * This plugin provides default subscribers to track the pose, velocity, and current trajectory in the system. 
-   * Extending classes must implement the generate_command() method to use that data and or additional data to plan commands at a 30Hz frequency. 
-   * 
+   * \brief ControlPlugin base class which can be extended by user provided plugins which wish to implement the Control Plugin ROS API.
+   *
+   * A control plugin is responsible for generating high frequency vehicle speed and steering commands to execute the currently planned trajectory.
+   * This plugin provides default subscribers to track the pose, velocity, and current trajectory in the system.
+   * Extending classes must implement the generate_command() method to use that data and or additional data to plan commands at a 30Hz frequency.
+   *
    */
   class ControlPlugin : public PluginBaseNode
   {
@@ -53,7 +53,6 @@ namespace carma_guidance_plugins
     // These callbacks do direct assignment into their respective member variables
     void current_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg);
     void current_twist_callback(geometry_msgs::msg::TwistStamped::UniquePtr msg);
-    void current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg);
 
 
   protected:
@@ -71,23 +70,28 @@ namespace carma_guidance_plugins
 
   public:
     /**
-     * \brief ControlPlugin constructor 
+     * \brief ControlPlugin constructor
      */
     explicit ControlPlugin(const rclcpp::NodeOptions &);
 
     //! Virtual destructor for safe deletion
     virtual ~ControlPlugin() = default;
 
     /**
-     * \brief Extending class provided method which should generate a command message 
+     * \brief Extending class provided method which should generate a command message
      *        which will be published to the required topic by the base class
-     * 
+     *
      * NOTE:  Implementer can determine if trajectory has changed based on current_trajectory_->trajectory_id
-     * 
+     *
      * \return The command message to publish
-     */ 
+     */
     virtual autoware_msgs::msg::ControlCommandStamped generate_command() = 0;
 
+    /**
+     * \brief Extending class provided method which can optionally handle trajectory plan callbacks.
+     */
+    virtual void current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg);
+
     ////
     // Overrides
     ////
@@ -102,7 +106,7 @@ namespace carma_guidance_plugins
     carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override final;
     carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override final;
     carma_ros2_utils::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &) override final;
-    carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override final; 
+    carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override final;
     carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &, const std::string &exception_string) override final;
   };
 

platooning_control/CMakeLists.txt

@@ -1,4 +1,5 @@
-# Copyright (C) 2018-2021 LEIDOS.
+
+# Copyright (C) 2024 LEIDOS.
 #
 # Licensed under the Apache License, Version 2.0 (the "License"); you may not
 # use this file except in compliance with the License. You may obtain a copy of
@@ -12,112 +13,71 @@
 # License for the specific language governing permissions and limitations under
 # the License.
 
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(platoon_control)
 
+# Declare carma package and check ROS version
 find_package(carma_cmake_common REQUIRED)
-carma_check_ros_version(1)
+carma_check_ros_version(2)
 carma_package()
 
-## Find catkin macros and libraries
-find_package(catkin REQUIRED COMPONENTS
-  carma_utils
-  cav_msgs
-  roscpp
-  std_msgs
-  autoware_msgs
-)
-
-## System dependencies are found with CMake's conventions
-find_package(Boost REQUIRED COMPONENTS system)
-
-###################################
-## catkin specific configuration ##
-###################################
-
-
-catkin_package(
-   INCLUDE_DIRS include
-   CATKIN_DEPENDS carma_utils cav_msgs roscpp std_msgs autoware_msgs
-)
+## Find dependencies using ament auto
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
-###########
-## Build ##
-###########
+# Name build targets
+set(node_exec platoon_control_node_exec)
+set(node_lib platoon_control_node)
 
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
+# Includes
 include_directories(
-  include
-  ${catkin_INCLUDE_DIRS}
   ${Boost_INCLUDE_DIRS}
+  include
 )
 
-file(GLOB_RECURSE headers */*.hpp */*.h)
-
-add_executable( ${PROJECT_NAME}
-  ${headers}
-  src/platoon_control.cpp
-  src/main.cpp)
-
-
-add_library(platoon_control_plugin_lib
- src/platoon_control.cpp
- src/platoon_control_worker.cpp
- src/pid_controller.cpp
- src/pure_pursuit.cpp
+# Build
+ament_auto_add_library(${node_lib} SHARED
+        src/platoon_control.cpp
+        src/platoon_control_worker.cpp
+        src/pid_controller.cpp
+        src/pure_pursuit.cpp
 )
 
-add_dependencies(platoon_control_plugin_lib ${catkin_EXPORTED_TARGETS})
-
-target_link_libraries(platoon_control_plugin_lib ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-target_link_libraries(${PROJECT_NAME} platoon_control_plugin_lib)
-
-
-#############
-## Install ##
-#############
-
-
-install(DIRECTORY include
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-  FILES_MATCHING PATTERN "*.h"
-  PATTERN ".svn" EXCLUDE
+ament_auto_add_executable(${node_exec}
+        src/main.cpp
 )
 
-## Install C++
-install(TARGETS ${PROJECT_NAME} platoon_control_plugin_lib
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
+# Register component
+rclcpp_components_register_nodes(${node_lib} "platoon_control::PlatoonControlPlugin")
 
-# Mark other files for installation (e.g. launch and bag files, etc.)
-install(DIRECTORY
-  launch config
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# All locally created targets will need to be manually linked
+# ament auto will handle linking of external dependencies
+target_link_libraries(${node_exec}
+        ${node_lib}
 )
 
+# Testing
+if(BUILD_TESTING)
 
-#############
-## Testing ##
-#############
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable
 
-catkin_add_gmock(${PROJECT_NAME}-test
+  ament_add_gtest(test_platoon_control
+  test/test_main.cpp
+  test/node_test.cpp
+  test/test_control.cpp
   test/test_pid.cpp
   test/test_pure.cpp
   test/test_worker.cpp
-  test/test_control.cpp
-  test/test_main.cpp)
+  )
 
-if(TARGET ${PROJECT_NAME}-test)
-  target_link_libraries(${PROJECT_NAME}-test platoon_control_plugin_lib ${catkin_LIBRARIES})
-endif()
+  ament_target_dependencies(test_platoon_control ${${PROJECT_NAME}_FOUND_TEST_DEPENDS})
 
-if(CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
-  add_rostest_gtest(test_platoon_control test/mynode.test test/test_mynode.cpp)
-  target_link_libraries(test_platoon_control ${catkin_LIBRARIES})
-endif()
+  target_link_libraries(test_platoon_control ${node_lib})
 
+endif()
 
+# Install
+ament_auto_package(
+        INSTALL_TO_SHARE config launch
+)

platooning_control/README.md

@@ -1,7 +1,7 @@
 # platoon_control
 
 Platooning Control plugin which allows platooning to maintain the gap; moreover, generates longitudinal and lateral control commands to follow the trajectory. The structure of this plugin is very similar to the control plugin for IHP2, so the same design document is included here.
-The difference between platoon_control, and platoon_control_ihp is that the IHP plugin includes logic to open or close the gap between platoon members, to allow for a new member to join or an existing memeber to exit the platoon. 
+The difference between platoon_control, and platoon_control_ihp is that the IHP plugin includes logic to open or close the gap between platoon members, to allow for a new member to join or an existing memeber to exit the platoon.
 
 Overview
 https://usdot-carma.atlassian.net/wiki/spaces/CUCS/pages/2392981532/Basic+Travel+and+IHP

platooning_control/config/parameters.yaml

@@ -1,19 +1,19 @@
-standStillHeadway: 12.0 # Standstill gap between vehicles (m)
-maxAccel: 1.5 # Maximum acceleration absolute value used in controller filters (m/s^2)
-Kp: 0.4 # Proportional weight for PID controller
-Kd: 0.0  # Derivative Weight for PID controller  
-Ki: 0.03 # Integral weight for PID controller
-maxValue: 2.0 # Max value to restrict speed adjustment at one time step (limit on delta_v) (m/s)
-minValue: -10.0 # Min value to restrict speed adjustment at one time step (limit on delta_v) (m/s)
-cmdTmestamp: 100 # Timestep to calculate ctrl commands (ms)
-adjustmentCap: 15.0 # Adjustment cap for speed command (m/s)
+stand_still_headway: 12.0 # Standstill gap between vehicles (m)
+max_accel: 1.5 # Maximum acceleration absolute value used in controller filters (m/s^2)
+kp: 0.4 # Proportional weight for PID controller
+kd: 0.0  # Derivative Weight for PID controller
+ki: 0.03 # Integral weight for PID controller
+max_value: 2.0 # Max value to restrict speed adjustment at one time step (limit on delta_v) (m/s)
+min_value: -10.0 # Min value to restrict speed adjustment at one time step (limit on delta_v) (m/s)
+cmd_tmestamp: 100 # Timestep to calculate ctrl commands (ms)
+adjustment_cap: 15.0 # Adjustment cap for speed command (m/s)
 dt: 0.1 # Timestep to calculate ctrl commands (s)
-integratorMax: 30 # Max limit for integrator term
-integratorMin: -30  # Min limit for integrator term
-lowpassGain: 0.5 ##Lowpass filter gain
-lookaheadRatio: 2.0 # Ratio to calculate lookahead distance
-minLookaheadDist: 15.0 # Min lookahead distance (m)
-correctionAngle: 0.0 # Correction angle to improve steering accuracy
-integratorMax_pp: 0.1 # Max integrator val for pure pursuit integral controller
-integratorMin_pp: -0.1  # Min integrator val for pure pursuit integral controller
-Ki_pp: 0.012 # Integral weight for pure pursuit integral controller
+integrator_max: 30.0 # Max limit for integrator term
+integrator_min: -30.0  # Min limit for integrator term
+lowpass_gain: 0.5 ##Lowpass filter gain
+lookahead_ratio: 2.0 # Ratio to calculate lookahead distance
+min_lookahead_dist: 15.0 # Min lookahead distance (m)
+correction_angle: 0.0 # Correction angle to improve steering accuracy
+integrator_max_pp: 0.1 # Max integrator val for pure pursuit integral controller
+integrator_min_pp: -0.1  # Min integrator val for pure pursuit integral controller
+ki_pp: 0.012 # Integral weight for pure pursuit integral controller