port platooning_control to ros2

platooning_control/CMakeLists.txt

@@ -31,8 +31,6 @@ set(node_lib platoon_control_node)
 
 # Includes
 include_directories(
-  ${Boost_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
   include
 )
 
@@ -64,14 +62,11 @@ if(BUILD_TESTING)
 
   ament_add_gtest(test_platoon_control
   test/test_main.cpp
-  test/node_test.cpp
   test/test_control.cpp
   test/test_pid.cpp
   test/test_worker.cpp
   )
 
-  ament_target_dependencies(test_platoon_control ${${PROJECT_NAME}_FOUND_TEST_DEPENDS})
-
   target_link_libraries(test_platoon_control ${node_lib})
 
 endif()

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

platooning_control/config/parameters.yaml

@@ -1,19 +1,12 @@
-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)
+stand_still_headway_m: 12.0 # Standstill gap between vehicles (m)
+max_accel_ms2: 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)
+max_delta_speed_per_timestep: 2.0 # Max value to restrict speed adjustment at one time step (limit on delta_v) (m/s)
+min_delta_speed_per_timestep: -10.0 # Min value to restrict speed adjustment at one time step (limit on delta_v) (m/s)
+cmd_timestamp_ms: 100 # Timestep to calculate ctrl commands (ms)
+adjustment_cap_mps: 15.0 # Adjustment cap for speed command (m/s)
 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
+vehicle_response_lag: 0.2

platooning_control/include/platoon_control/pid_controller.hpp

@@ -1,5 +1,5 @@
 /*------------------------------------------------------------------------------
-* Copyright (C) 2020-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

platooning_control/include/platoon_control/platoon_control.hpp

@@ -18,8 +18,6 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <functional>
-#include <std_msgs/msg/string.hpp>
-#include <std_srvs/srv/empty.hpp>
 #include <carma_planning_msgs/msg/trajectory_plan_point.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <autoware_msgs/msg/control_command.hpp>
@@ -36,94 +34,50 @@ namespace platoon_control
 {
 
   /**
-    * \brief This class includes logic for Platoon control. It includes publishers and subscribers and their callback functions
+    * \brief This class includes node-level logic for Platooning Control such as its publishers, subscribers, and their callback functions.
+    * Platooning Control is used for generating control commands to maintain the gap in platoon as well as generating longitudinal and lateral control commands to follow the trajectory.
 	*/
   class PlatoonControlPlugin : public carma_guidance_plugins::ControlPlugin
   {
 
-  private:
-
-    // Node configuration
-    PlatooningControlPluginConfig config_;
-
-    // platoon control worker object
-    PlatoonControlWorker pcw_;
-
-    // Variables
-    PlatoonLeaderInfo platoon_leader_;
-    long prev_input_time_ = 0;				//timestamp of the previous trajectory plan input received
-    long consecutive_input_counter_ = 0;	//num inputs seen without a timeout
-
-    std::shared_ptr<pure_pursuit::PurePursuit> pp_;
-
-    /**
-    * \brief callback function for platoon info
-    * \param msg platoon info msg
-    */
-    void platoonInfo_cb(const carma_planning_msgs::msg::PlatooningInfo::SharedPtr msg);
-
-    /**
-    * \brief calculate average speed of a set of trajectory points
-    * \param trajectory_points set of trajectory points
-    * \return trajectory speed
-    */
-    double getTrajectorySpeed(std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> trajectory_points);
-
-
-    // Subscribers
-    carma_ros2_utils::SubPtr<carma_planning_msgs::msg::TrajectoryPlan> trajectory_plan_sub_;
-    carma_ros2_utils::SubPtr<carma_planning_msgs::msg::PlatooningInfo> platoon_info_sub_;
-
-    // Publishers
-    carma_ros2_utils::PubPtr<geometry_msgs::msg::TwistStamped> twist_pub_;
-    carma_ros2_utils::PubPtr<carma_planning_msgs::msg::PlatooningInfo> platoon_info_pub_;
-
-
   public:
     /**
      * \brief PlatoonControlPlugin constructor
      */
     explicit PlatoonControlPlugin(const rclcpp::NodeOptions& options);
 
-    /**
-     * \brief Example callback for dynamic parameter updates
-     */
-    rcl_interfaces::msg::SetParametersResult
-    parameter_update_callback(const std::vector<rclcpp::Parameter> &parameters);
-
     /**
 			* \brief generate control signal by calculating speed and steering commands.
 			* \param point0 start point of control window
 			* \param point_end end point of control wondow
 			*/
-		autoware_msgs::msg::ControlCommandStamped generateControlSignals(const carma_planning_msgs::msg::TrajectoryPlanPoint& point0, const carma_planning_msgs::msg::TrajectoryPlanPoint& point_end);
+		autoware_msgs::msg::ControlCommandStamped generate_control_signals(const carma_planning_msgs::msg::TrajectoryPlanPoint& point0, const carma_planning_msgs::msg::TrajectoryPlanPoint& point_end);
 
     /**
 			* \brief Compose twist message from linear and angular velocity commands.
 			* \param linear_vel linear velocity in m/s
 			* \param angular_vel angular velocity in rad/s
 			* \return twist message
 			*/
-			geometry_msgs::msg::TwistStamped composeTwistCmd(double linear_vel, double angular_vel);
+		geometry_msgs::msg::TwistStamped compose_twist_cmd(double linear_vel, double angular_vel);
 
-      motion::motion_common::State convert_state(geometry_msgs::msg::PoseStamped pose, geometry_msgs::msg::TwistStamped twist);
+    motion::motion_common::State convert_state(const geometry_msgs::msg::PoseStamped& pose, const geometry_msgs::msg::TwistStamped& twist);
 
       /**
 			* \brief find the point correspoding to the lookahead distance
 			* \param trajectory_plan trajectory plan
 			* \return trajectory point
 			*/
-			carma_planning_msgs::msg::TrajectoryPlanPoint getLookaheadTrajectoryPoint(carma_planning_msgs::msg::TrajectoryPlan trajectory_plan);
+		carma_planning_msgs::msg::TrajectoryPlanPoint get_lookahead_trajectory_point(const carma_planning_msgs::msg::TrajectoryPlan& trajectory_plan);
 
-      double trajectory_speed_ = 0.0;
+    double trajectory_speed_ = 0.0;
 
-      carma_planning_msgs::msg::TrajectoryPlan latest_trajectory_;
+    carma_planning_msgs::msg::TrajectoryPlan latest_trajectory_;
 
-    ////
-    // Overrides
-    ////
-
-    autoware_msgs::msg::ControlCommandStamped generate_command() override;
+    /**
+     * \brief Callback for dynamic parameter updates
+     */
+    rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector<rclcpp::Parameter> &parameters);
 
     /**
     * \brief callback function for trajectory plan
@@ -137,18 +91,67 @@ namespace platoon_control
 			* \param steering_angle steering angle in rad
 			* \return control command
 			*/
-		autoware_msgs::msg::ControlCommandStamped composeCtrlCmd(double linear_vel, double steering_angle);
-
+		autoware_msgs::msg::ControlCommandStamped compose_ctrl_cmd(double linear_vel, double steering_angle);
 
+    /**
+    * \brief Returns availability of plugin. Always true
+    */
     bool get_availability() override;
 
+    /**
+    * \brief Returns version id of plugn.
+    */
     std::string get_version_id() override;
 
+    ////
+    // Overrides
+    ////
+
+    autoware_msgs::msg::ControlCommandStamped generate_command() override;
+
     /**
      * \brief This method should be used to load parameters and will be called on the configure state transition.
      */
     carma_ros2_utils::CallbackReturn on_configure_plugin() override;
 
+
+  private:
+
+    // Node configuration
+    PlatooningControlPluginConfig config_;
+
+    // platoon control worker object
+    PlatoonControlWorker pcw_;
+
+    // Variables
+    PlatoonLeaderInfo platoon_leader_;
+    long prev_input_time_ = 0;				//timestamp of the previous trajectory plan input received
+    long consecutive_input_counter_ = 0;	//num inputs seen without a timeout
+
+    std::shared_ptr<pure_pursuit::PurePursuit> pp_;
+
+    /**
+    * \brief callback function for platoon info
+    * \param msg platoon info msg
+    */
+    void platoon_info_cb(const carma_planning_msgs::msg::PlatooningInfo::SharedPtr msg);
+
+    /**
+    * \brief calculate average speed of a set of trajectory points
+    * \param trajectory_points set of trajectory points
+    * \return trajectory speed
+    */
+    double get_trajectory_speed(const std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>& trajectory_points);
+
+
+    // Subscribers
+    carma_ros2_utils::SubPtr<carma_planning_msgs::msg::TrajectoryPlan> trajectory_plan_sub_;
+    carma_ros2_utils::SubPtr<carma_planning_msgs::msg::PlatooningInfo> platoon_info_sub_;
+
+    // Publishers
+    carma_ros2_utils::PubPtr<geometry_msgs::msg::TwistStamped> twist_pub_;
+    carma_ros2_utils::PubPtr<carma_planning_msgs::msg::PlatooningInfo> platoon_info_pub_;
+
   };
 
 } // platoon_control

platooning_control/include/platoon_control/platoon_control_config.hpp

@@ -27,16 +27,16 @@ namespace platoon_control
  */
   struct PlatooningControlPluginConfig
   {
-    double stand_still_headway = 12.0; // Standstill gap between vehicles (m)
-    double max_accel = 2.5;  // Maximum acceleration absolute value used in controller filters (m/s^2)
+    double stand_still_headway_m = 12.0; // Standstill gap between vehicles (m)
+    double max_accel_mps2 = 2.5;  // Maximum acceleration absolute value used in controller filters (m/s^2)
     double kp = 0.5;  // Proportional weight for PID controller
     double kd = -0.5;  // Derivative Weight for PID controller
     double ki = 0.0;  // Integral weight for PID controller
-    double max_value = 2;  // Max value to restrict speed adjustment at one time step (limit on delta_v) (m/s)
-    double min_value = -10; // Min value to restrict speed adjustment at one time step (limit on delta_v) (m/s)
+    double max_delta_speed_per_timestep = 2;  // Max value to restrict speed adjustment at one time step (limit on delta_v) (m/s)
+    double min_delta_speed_per_timestep = -10; // Min value to restrict speed adjustment at one time step (limit on delta_v) (m/s)
 
-    double adjustment_cap = 10;  // Adjustment cap for speed command (m/s)
-    int cmd_timestamp = 100;  // Timestamp to calculate ctrl commands (ms)
+    double adjustment_cap_mps = 10;  // Adjustment cap for speed command (m/s)
+    int cmd_timestamp_ms = 100;  // Timestamp to calculate ctrl commands (ms)
     double integrator_max = 100; // Max limit for integrator term
     double integrator_min = -100;  // Max limit for integrator term
     std::string vehicle_id = "DEFAULT_VEHICLE_ID";         // Vehicle id is the license plate of the vehicle
@@ -65,20 +65,20 @@ namespace platoon_control
   friend std::ostream& operator<<(std::ostream& output, const PlatooningControlPluginConfig& c)
   {
     output << "PlatooningControlPluginConfig { " << std::endl
-           << "stand_still_headway_: " << c.stand_still_headway << std::endl
-           << "max_accel_: " << c.max_accel << std::endl
-           << "kp_: " << c.kp << std::endl
-           << "kd_: " << c.kd << std::endl
-           << "ki_: " << c.ki << std::endl
-           << "max_value_: " << c.max_value << std::endl
-           << "min_value_: " << c.min_value << std::endl
-           << "adjustment_cap_: " << c.adjustment_cap << std::endl
-           << "cmd_timestamp_: " << c.cmd_timestamp << std::endl
-           << "integrator_max_: " << c.integrator_max << std::endl
-           << "integrator_min_: " << c.integrator_min << std::endl
-           << "vehicle_id_: " << c.vehicle_id << std::endl
-           << "shutdown_timeout_: " << c.shutdown_timeout << std::endl
-           << "ignore_initial_inputs_: " << c.ignore_initial_inputs << std::endl
+           << "stand_still_headway_m: " << c.stand_still_headway_m << std::endl
+           << "max_accel_mps2: " << c.max_accel_mps2 << std::endl
+           << "kp: " << c.kp << std::endl
+           << "kd: " << c.kd << std::endl
+           << "ki: " << c.ki << std::endl
+           << "max_delta_speed_per_timestep: " << c.max_delta_speed_per_timestep << std::endl
+           << "min_delta_speed_per_timestep_: " << c.min_delta_speed_per_timestep << std::endl
+           << "adjustment_cap_mps: " << c.adjustment_cap_mps << std::endl
+           << "cmd_timestamp_ms: " << c.cmd_timestamp_ms << std::endl
+           << "integrator_max: " << c.integrator_max << std::endl
+           << "integrator_min: " << c.integrator_min << std::endl
+           << "vehicle_id: " << c.vehicle_id << std::endl
+           << "shutdown_timeout: " << c.shutdown_timeout << std::endl
+           << "ignore_initial_inputs: " << c.ignore_initial_inputs << std::endl
             //Pure Pursuit configs
            << "vehicle_response_lag" << c.vehicle_response_lag << std::endl
            << "max_lookahead_dist: " << c.max_lookahead_dist << std::endl

platooning_control/include/platoon_control/platoon_control_worker.hpp

@@ -24,7 +24,6 @@
 #include <carma_v2x_msgs/msg/plan_type.hpp>
 #include <carma_planning_msgs/msg/trajectory_plan.hpp>
 #include "platoon_control/pid_controller.hpp"
-#include "platoon_control/pure_pursuit.hpp"
 #include "platoon_control/platoon_control_config.hpp"
 #include <boost/optional.hpp>
 
@@ -59,52 +58,43 @@ namespace platoon_control
     */
     class PlatoonControlWorker
     {
-    public:
+        public:
 
         /**
         * \brief Default constructor for platooning control worker
         */
         PlatoonControlWorker();
 
-        /**
-        * \brief Update configurations
-        * \param new_config new configuration
-        */
-        void updateConfigParams(PlatooningControlPluginConfig new_config);
-
         /**
         * \brief Returns latest speed command
         * \return lastest speed command in m/s
         */
-        double getLastSpeedCommand() const;
+        double get_last_speed_command() const;
 
         /**
         * \brief Generates speed commands (in m/s) based on the trajectory point
         * \param point trajectory point
         */
-        void generateSpeed(const carma_planning_msgs::msg::TrajectoryPlanPoint& point);
+        void generate_speed(const carma_planning_msgs::msg::TrajectoryPlanPoint& point);
 
         /**
         * \brief Sets the platoon leader object using info from msg
         * \param leader leader information msg received from strategic plugin
         */
-        void setLeader(const PlatoonLeaderInfo& leader);
+        void set_leader(const PlatoonLeaderInfo& leader);
 
 
         /**
         * \brief set current speed
         * \param speed speed value
         */
-        void setCurrentSpeed(double speed);
+        void set_current_speed(double speed);
 
         // Member Variables
 
         // Platoon Leader
         PlatoonLeaderInfo platoon_leader;
 
-		// geometry pose
-        std::shared_ptr<geometry_msgs::msg::Pose> current_pose_ = std::make_shared<geometry_msgs::msg::Pose>();
-
         // config parameters
         std::shared_ptr<PlatooningControlPluginConfig> ctrl_config_ = std::make_shared<PlatooningControlPluginConfig>();
 
@@ -114,7 +104,7 @@ namespace platoon_control
         double speedCmd_ = 0;
         double steerCmd_ = 0;
         double angVelCmd_ = 0;
-        double desired_gap_ = ctrl_config_->stand_still_headway;
+        double desired_gap_ = ctrl_config_->stand_still_headway_m;
         double actual_gap_ = 0.0;
         bool last_cmd_set_ = false;