This is the worker class for platoon controller. It is responsible for generating and smoothing the speed and steering control commands from trajectory points. More...

#include <platooning_control_worker.hpp>

Collaboration diagram for platooning_control::PlatooningControlWorker:

Public Member Functions
	PlatooningControlWorker ()
	Default constructor for platooning control worker. More...

double	get_last_speed_command () const
	Returns latest speed command. More...

void	generate_speed (const carma_planning_msgs::msg::TrajectoryPlanPoint &point)
	Generates speed commands (in m/s) based on the trajectory point. More...

void	set_leader (const PlatoonLeaderInfo &leader)
	Sets the platoon leader object using info from msg. More...

void	set_current_speed (double speed)
	set current speed More...

Public Attributes
PlatoonLeaderInfo	platoon_leader

std::shared_ptr< PlatooningControlPluginConfig >	ctrl_config_ = std::make_shared<PlatooningControlPluginConfig>()

double	speedCmd = 0

double	currentSpeed = 0

double	lastCmdSpeed = 0.0

double	speedCmd_ = 0

double	steerCmd_ = 0

double	angVelCmd_ = 0

double	desired_gap_ = ctrl_config_->stand_still_headway_m

double	actual_gap_ = 0.0

bool	last_cmd_set_ = false

Private Attributes
PIDController	pid_ctrl_

double	dist_to_front_vehicle

Detailed Description

This is the worker class for platoon controller. It is responsible for generating and smoothing the speed and steering control commands from trajectory points.

Definition at line 59 of file platooning_control_worker.hpp.

Constructor & Destructor Documentation

◆ PlatooningControlWorker()

platooning_control::PlatooningControlWorker::PlatooningControlWorker ( )

Default constructor for platooning control worker.

Definition at line 25 of file platooning_control_worker.cpp.

    {
        pid_ctrl_ = PIDController();
 
    }

References pid_ctrl_.

Member Function Documentation

◆ generate_speed()

void platooning_control::PlatooningControlWorker::generate_speed ( const carma_planning_msgs::msg::TrajectoryPlanPoint & point )

Generates speed commands (in m/s) based on the trajectory point.

Parameters

point trajectory point

Definition at line 35 of file platooning_control_worker.cpp.

    {
        double speed_cmd = 0;
 
        if (!last_cmd_set_)
        {
            // last speed command for smooth speed transition
            lastCmdSpeed = currentSpeed;
            last_cmd_set_ = true;
        }
 
        PlatoonLeaderInfo leader = platoon_leader;
        if(!leader.staticId.empty() && leader.staticId != ctrl_config_->vehicle_id)
        {
            double controllerOutput = 0.0;
 
 
            double leaderCurrentPosition = leader.vehiclePosition;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "The current leader position is " << leaderCurrentPosition);
 
            double desiredHostPosition = leaderCurrentPosition - desired_gap_;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "desiredHostPosition = " << desiredHostPosition);
 
            double hostVehiclePosition = leaderCurrentPosition - actual_gap_;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "hostVehiclePosition = " << hostVehiclePosition);
 
            controllerOutput = pid_ctrl_.calculate(desiredHostPosition, hostVehiclePosition);
 
            double adjSpeedCmd = controllerOutput + leader.commandSpeed;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Adjusted Speed Cmd = " << adjSpeedCmd << "; Controller Output = " << controllerOutput
                << "; Leader CmdSpeed= " << leader.commandSpeed << "; Adjustment Cap " << ctrl_config_->adjustment_cap_mps);
                // After we get a adjSpeedCmd, we apply three filters on it if the filter is enabled
                // First: we do not allow the difference between command speed of the host vehicle and the leader's commandSpeed higher than adjustmentCap
 
            speed_cmd = adjSpeedCmd;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "A speed command is generated from command generator: " << speed_cmd << " m/s");
 
            if(ctrl_config_->enable_max_adjustment_filter)
            {
                if(speed_cmd > leader.commandSpeed + ctrl_config_->adjustment_cap_mps) {
                    speed_cmd = leader.commandSpeed + ctrl_config_->adjustment_cap_mps;
                } else if(speed_cmd < leader.commandSpeed - ctrl_config_->adjustment_cap_mps) {
                    speed_cmd = leader.commandSpeed - ctrl_config_->adjustment_cap_mps;
                }
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "The adjusted cmd speed after max adjustment cap is " << speed_cmd << " m/s");
            }
 
        }
 
        else if (leader.staticId == ctrl_config_->vehicle_id)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Host vehicle is the leader");
            speed_cmd = currentSpeed;
 
            if(ctrl_config_->enable_max_adjustment_filter)
            {
                if(speed_cmd > ctrl_config_->adjustment_cap_mps)
                {
                    speed_cmd = ctrl_config_->adjustment_cap_mps;
                }
 
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "The adjusted leader cmd speed after max adjustment cap is " << speed_cmd << " m/s");
            }
 
            pid_ctrl_.reset();
        }
 
        else
        {
            // If there is no leader available, the vehicle will stop. This means there is a mis-communication between platooning strategic and control plugins.
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "There is no leader available");
            speed_cmd = 0.0;
            pid_ctrl_.reset();
        }
 
 
 
        // Third: we allow do not a large gap between two consecutive speed commands
        if(ctrl_config_->enable_max_accel_filter) {
 
                double max = lastCmdSpeed + (ctrl_config_->max_accel_mps2 * (ctrl_config_->cmd_timestamp_ms / 1000.0));
                double min = lastCmdSpeed - (ctrl_config_->max_accel_mps2 * (ctrl_config_->cmd_timestamp_ms / 1000.0));
                if(speed_cmd > max) {
                    speed_cmd = max;
                } else if (speed_cmd < min) {
                    speed_cmd = min;
                }
                lastCmdSpeed = speed_cmd;
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "The speed command after max accel cap is: " << speed_cmd << " m/s");
        }
 
        speedCmd_ = speed_cmd;
 
        lastCmdSpeed = speedCmd_;
 
    }

References actual_gap_, platooning_control::PIDController::calculate(), platooning_control::PlatoonLeaderInfo::commandSpeed, ctrl_config_, currentSpeed, desired_gap_, last_cmd_set_, lastCmdSpeed, pid_ctrl_, platoon_leader, platooning_control::PIDController::reset(), speedCmd_, platooning_control::PlatoonLeaderInfo::staticId, and platooning_control::PlatoonLeaderInfo::vehiclePosition.

Referenced by platooning_control::PlatooningControlPlugin::generate_control_signals().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_last_speed_command()

double platooning_control::PlatooningControlWorker::get_last_speed_command ( ) const

Returns latest speed command.

Returns: lastest speed command in m/s

Definition at line 31 of file platooning_control_worker.cpp.

                                                                 {
        return speedCmd_;
    }

References speedCmd_.

◆ set_current_speed()

void platooning_control::PlatooningControlWorker::set_current_speed ( double speed )

set current speed

Parameters

speed speed value

Definition at line 137 of file platooning_control_worker.cpp.

                                                                {
        currentSpeed = speed;
 
    }

References currentSpeed.

Referenced by platooning_control::PlatooningControlPlugin::generate_control_signals().

Here is the caller graph for this function:

◆ set_leader()

void platooning_control::PlatooningControlWorker::set_leader ( const PlatoonLeaderInfo & leader )

Sets the platoon leader object using info from msg.

Parameters

leader leader information msg received from strategic plugin

Definition at line 133 of file platooning_control_worker.cpp.

                                                                           {
        platoon_leader = leader;
    }

References platoon_leader.

Referenced by platooning_control::PlatooningControlPlugin::generate_control_signals().

Here is the caller graph for this function:

Member Data Documentation

◆ actual_gap_

double platooning_control::PlatooningControlWorker::actual_gap_ = 0.0

Definition at line 108 of file platooning_control_worker.hpp.

Referenced by generate_speed(), and platooning_control::PlatooningControlPlugin::platoon_info_cb().

◆ angVelCmd_

double platooning_control::PlatooningControlWorker::angVelCmd_ = 0

Definition at line 106 of file platooning_control_worker.hpp.

◆ ctrl_config_

std::shared_ptr<PlatooningControlPluginConfig> platooning_control::PlatooningControlWorker::ctrl_config_ = std::make_shared<PlatooningControlPluginConfig>()

Definition at line 99 of file platooning_control_worker.hpp.

Referenced by platooning_control::PlatooningControlPlugin::PlatooningControlPlugin(), and generate_speed().