port platooning_control to ros2

.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, \
@@ -96,7 +95,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
@@ -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
@@ -160,8 +158,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
@@ -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
@@ -235,7 +231,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
@@ -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,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

@@ -88,7 +88,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',
@@ -289,7 +292,7 @@ def generate_launch_description():
                 parameters=[
                     lci_strategic_plugin_file_path,
                     vehicle_config_param_file,
-                    lci_plugin_calibration_params
+                    unique_vehicle_calibration_params
                 ]
             ),
         ]
@@ -527,6 +530,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',
@@ -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
   {
@@ -50,11 +50,6 @@ namespace carma_guidance_plugins
     // Timers
     rclcpp::TimerBase::SharedPtr command_timer_;
 
-    // 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:
 
@@ -68,24 +63,33 @@ namespace carma_guidance_plugins
     //! The most recent trajectory received by this plugin
     boost::optional<carma_planning_msgs::msg::TrajectoryPlan> current_trajectory_;
 
+    // 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);
+
+    /**
+     * \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:
     /**
-     * \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;
 
     ////
@@ -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;
   };