port platooning_control to ros2

.sonarqube/sonar-scanner.properties

@@ -73,7 +73,6 @@ sonar.modules= bsm_generator, \
   object_visualizer, \
   points_map_filter, \
   light_controlled_intersection_tactical_plugin, \
-  platoon_control_ihp, \
   carma_guidance_plugins, \
   carma_cloud_client, \
   approaching_emergency_vehicle_plugin, \
@@ -119,7 +118,6 @@ frame_transformer.sonar.projectBaseDir = /opt/carma/src/carma-platform/frame_tra
 object_visualizer.sonar.projectBaseDir = /opt/carma/src/carma-platform/object_visualizer
 points_map_filter.sonar.projectBaseDir = /opt/carma/src/carma-platform/points_map_filter
 light_controlled_intersection_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/light_controlled_intersection_tactical_plugin
-platoon_control_ihp.sonar.projectBaseDir = /opt/carma/src/carma-platform/platooning_control_ihp
 lci_strategic_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/lci_strategic_plugin
 carma_guidance_plugins.sonar.projectBaseDir = /opt/carma/src/carma-platform/carma_guidance_plugins
 carma_cloud_client.sonar.projectBaseDir = /opt/carma/src/carma-platform/carma_cloud_client
@@ -206,8 +204,6 @@ points_map_filter.sonar.sources = src
 points_map_filter.sonar.exclusions  =test/**
 light_controlled_intersection_tactical_plugin.sonar.sources = src
 light_controlled_intersection_tactical_plugin.sonar.exclusions  =test/**
-platoon_control_ihp.sonar.sources = src
-platoon_control_ihp.sonar.exclusions  =test/**
 lci_strategic_plugin.sonar.sources = src
 lci_strategic_plugin.sonar.exclusions  =test/**
 carma_guidance_plugins.sonar.sources = src
@@ -250,7 +246,6 @@ yield_plugin.sonar.tests = test
 basic_autonomy.sonar.tests = test
 mobilitypath_visualizer.sonar.tests = test
 sci_strategic_plugin.sonar.tests = test
-platoon_control_ihp.sonar.tests = test
 
 
 stop_controlled_intersection_tactical_plugin.sonar.tests = test
@@ -260,7 +255,6 @@ frame_transformer.sonar.sources = test
 object_visualizer.sonar.sources = test
 points_map_filter.sonar.sources = test
 light_controlled_intersection_tactical_plugin.sonar.tests = test
-platoon_control_ihp_plugin.sonar.tests = test
 lci_strategic_plugin.sonar.sources = src
 lci_strategic_plugin.sonar.sources = test
 carma_guidance_plugins.sonar.sources = test

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,65 @@
 # 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)
+## Find dependencies using ament auto
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
-###################################
-## catkin specific configuration ##
-###################################
-
-
-catkin_package(
-   INCLUDE_DIRS include
-   CATKIN_DEPENDS carma_utils cav_msgs roscpp std_msgs autoware_msgs
-)
+# Name build targets
+set(node_exec platoon_control_node_exec)
+set(node_lib platoon_control_node)
 
-###########
-## Build ##
-###########
-
-## 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}
 )
 
-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
 )
 
-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/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()
+  target_link_libraries(test_platoon_control ${node_lib})
 
-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()
 
-
+# Install
+ament_auto_package(
+        INSTALL_TO_SHARE config launch
+)

platooning_control/README.md

@@ -1,7 +1,6 @@
 # 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. 
 
 Overview
 https://usdot-carma.atlassian.net/wiki/spaces/CUCS/pages/2392981532/Basic+Travel+and+IHP