port platooning_control to ros2 (#2377)

.sonarqube/sonar-scanner.properties

@@ -52,7 +52,7 @@ sonar.modules= bsm_generator, \
  platooning_tactical_plugin, \
  port_drayage_plugin, \
  mobilitypath_publisher, \
- platoon_control_plugin, \
+ platooning_control_plugin, \
  rosbag_mock_drivers, \
  lightbar_manager, \
  inlanecruising_plugin, \
@@ -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, \
@@ -97,7 +96,7 @@ truck_inspection_client.sonar.projectBaseDir = /opt/carma/src/carma-platform/t
 roadway_objects.sonar.projectBaseDir = /opt/carma/src/carma-platform/roadway_objects
 platooning_strategic_IHP.sonar.projectBaseDir = /opt/carma/src/carma-platform/platooning_strategic_IHP
 platooning_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/platooning_tactical_plugin
-platoon_control_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/platooning_control
+platooning_control_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/platooning_control
 mobilitypath_publisher.sonar.projectBaseDir = /opt/carma/src/carma-platform/mobilitypath_publisher
 mock_lightbar_driver.sonar.projectBaseDir = /opt/carma/src/carma-platform/mock_drivers/mock_lightbar_driver
 port_drayage_plugin.sonar.projectBaseDir = /opt/carma/src/carma-platform/port_drayage_plugin
@@ -120,7 +119,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
@@ -162,8 +160,8 @@ platooning_strategic_IHP.sonar.sources = src
 platooning_strategic_IHP.sonar.exclusions =test/**
 platooning_tactical_plugin.sonar.sources = src
 platooning_tactical_plugin.sonar.exclusions =test/**
-platoon_control_plugin.sonar.sources = src
-platoon_control_plugin.sonar.exclusions =test/**
+platooning_control_plugin.sonar.sources = src
+platooning_control_plugin.sonar.exclusions =test/**
 mobilitypath_publisher.sonar.sources = src
 mobilitypath_publisher.sonar.exclusions =test/**
 mock_lightbar_driver.sonar.sources = src
@@ -208,8 +206,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
@@ -239,7 +235,7 @@ truck_inspection_client.sonar.tests = test
 roadway_objects.sonar.tests = test
 platooning_strategic_IHP.sonar.tests = test
 platooning_tactical_plugin.sonar.tests = test
-platoon_control_plugin.sonar.tests = test
+platooning_control_plugin.sonar.tests = test
 mobilitypath_publisher.sonar.tests = test
 mock_lightbar_driver.sonar.tests = test
 port_drayage_plugin.sonar.tests = test
@@ -254,7 +250,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
@@ -264,7 +259,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,25 +87,9 @@
  <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"/> 
-
- <!-- Planning Stack -->
- <group ns="plugins">
+ <remap from="/current_pose" to="$(optenv CARMA_LOCZ_NS)/current_pose"/>
 
- <!-- Remappings for common topics to move out of the plugin namespace -->
- <remap from="route" to="$(optenv CARMA_GUIDE_NS)/route"/>
- <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

@@ -91,7 +91,10 @@ def generate_launch_description():
 
  pure_pursuit_tuning_parameters = [vehicle_calibration_dir, "/pure_pursuit/calibration.yaml"]
 
- lci_plugin_calibration_params = [vehicle_calibration_dir, "/identifiers/UniqueVehicleParams.yaml"]
+ unique_vehicle_calibration_params = [vehicle_calibration_dir, "/identifiers/UniqueVehicleParams.yaml"]
+
+ platooning_control_param_file = os.path.join(
+ get_package_share_directory('platooning_control'), 'config/parameters.yaml')
 
  carma_inlanecruising_plugin_container = ComposableNodeContainer(
  package='carma_ros2_utils',
@@ -292,7 +295,7 @@ def generate_launch_description():
  parameters=[
  lci_strategic_plugin_file_path,
  vehicle_config_param_file,
- lci_plugin_calibration_params
+ unique_vehicle_calibration_params
  ]
  ),
  ]
@@ -559,6 +562,33 @@ def generate_launch_description():
  ]
  )
 
+ platooning_control_plugin_container = ComposableNodeContainer(
+ package='carma_ros2_utils',
+ name='platooning_control_container',
+ executable='carma_component_container_mt',
+ namespace=GetCurrentNamespace(),
+ composable_node_descriptions=[
+ ComposableNode(
+ package='platooning_control',
+ plugin='platooning_control::PlatooningControlPlugin',
+ name='platooning_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" ] ),
+ ("platooning_control/plan_trajectory", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/platooning_control/plan_trajectory" ] ),
+ ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
+ ("vehicle/twist", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
+ ],
+ parameters=[ platooning_control_param_file, vehicle_config_param_file, unique_vehicle_calibration_params ]
+ )
+ ]
+ )
+
  carma_stop_and_dwell_strategic_plugin_container = ComposableNodeContainer(
  package='carma_ros2_utils',
  name='carma_stop_and_dwell_strategic_plugin_container',
@@ -630,6 +660,7 @@ def generate_launch_description():
  carma_trajectory_follower_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

@@ -66,8 +66,11 @@ 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);
 
+ /**
+ * \brief Extending class provided method which can optionally handle trajectory plan callbacks.
+ */
+ virtual void current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg);
 
 
  public:

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,66 @@
 # License for the specific language governing permissions and limitations under
 # the License.
 
-cmake_minimum_required(VERSION 2.8.3)
-project(platoon_control)
+cmake_minimum_required(VERSION 3.5)
+project(platooning_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 platooning_control_node_exec)
+set(node_lib platooning_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}
 )
 
-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/platooning_control.cpp
+ src/platooning_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} "platooning_control::PlatooningControlPlugin")
 
-# 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_platooning_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)
+ test/test_pure.cpp
+ )
 
-if(TARGET ${PROJECT_NAME}-test)
- target_link_libraries(${PROJECT_NAME}-test platoon_control_plugin_lib ${catkin_LIBRARIES})
-endif()
+ target_link_libraries(test_platooning_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
 
 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