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. More...

#include <platooning_control.hpp>

Inheritance diagram for platooning_control::PlatooningControlPlugin:

Collaboration diagram for platooning_control::PlatooningControlPlugin:

Public Member Functions
	PlatooningControlPlugin (const rclcpp::NodeOptions &options)
	PlatooningControlPlugin constructor. More...

autoware_msgs::msg::ControlCommandStamped	generate_control_signals (const carma_planning_msgs::msg::TrajectoryPlanPoint &first_trajectory_point, const geometry_msgs::msg::PoseStamped &current_pose, const geometry_msgs::msg::TwistStamped &current_twist)
	generate control signal by calculating speed and steering commands. More...

geometry_msgs::msg::TwistStamped	compose_twist_cmd (double linear_vel, double angular_vel)
	Compose twist message from linear and angular velocity commands. More...

motion::motion_common::State	convert_state (const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::TwistStamped &twist) const

rcl_interfaces::msg::SetParametersResult	parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
	Callback for dynamic parameter updates. More...

void	current_trajectory_callback (const carma_planning_msgs::msg::TrajectoryPlan::UniquePtr tp)
	callback function for trajectory plan More...

autoware_msgs::msg::ControlCommandStamped	compose_ctrl_cmd (double linear_vel, double steering_angle)
	Compose control message from speed and steering commands. More...

bool	get_availability () override
	Returns availability of plugin. Always true. More...

std::string	get_version_id () override
	Returns version id of plugn. More...

autoware_msgs::msg::ControlCommandStamped	generate_command () override
	Extending class provided method which should generate a command message which will be published to the required topic by the base class. More...

carma_ros2_utils::CallbackReturn	on_configure_plugin () override
	This method should be used to load parameters and will be called on the configure state transition. More...

Public Member Functions inherited from carma_guidance_plugins::ControlPlugin
	ControlPlugin (const rclcpp::NodeOptions &)
	ControlPlugin constructor. More...

virtual	~ControlPlugin ()=default
	Virtual destructor for safe deletion. More...

virtual autoware_msgs::msg::ControlCommandStamped	generate_command ()=0
	Extending class provided method which should generate a command message which will be published to the required topic by the base class. More...

std::string	get_capability () override
	Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...

uint8_t	get_type () override final
	Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &) override final

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_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override final

Public Member Functions inherited from carma_guidance_plugins::PluginBaseNode
	PluginBaseNode (const rclcpp::NodeOptions &)
	PluginBaseNode constructor. More...

virtual	~PluginBaseNode ()=default
	Virtual destructor for safe deletion. More...

virtual std::shared_ptr< carma_wm::WMListener >	get_world_model_listener () final
	Method to return the default world model listener provided as a convience by this base class If this method or get_world_model() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...

virtual carma_wm::WorldModelConstPtr	get_world_model () final
	Method to return the default world model provided as a convience by this base class If this method or get_world_model_listener() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...

virtual bool	get_activation_status () final
	Returns the activation status of this plugin. The plugins API callbacks will only be triggered when this method returns true. More...

virtual uint8_t	get_type ()
	Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...

std::string	get_plugin_name_and_ns () const
	Return the name of this plugin with namespace. NOTE: If only the name of the plugin is required, use get_plugin_name() More...

std::string	get_plugin_name () const
	Return the name of this plugin. More...

virtual bool	get_availability ()=0
	Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...

virtual std::string	get_capability ()=0
	Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...

virtual std::string	get_version_id ()=0
	Returns the version id of this plugin. More...

virtual carma_ros2_utils::CallbackReturn	on_configure_plugin ()=0
	Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. This method should be used to load parameters and is required to be implemented. More...

virtual carma_ros2_utils::CallbackReturn	on_activate_plugin ()
	Method which is triggered when this plugin is moved from the INACTIVE to ACTIVE states. This method should be used to prepare for future callbacks for plugin's capabilites. More...

virtual carma_ros2_utils::CallbackReturn	on_deactivate_plugin ()
	Method which is triggered when this plugin is moved from the ACTIVE to INACTIVE states. This method should be used to disable any functionality which should cease execution when plugin is inactive. More...

virtual carma_ros2_utils::CallbackReturn	on_cleanup_plugin ()
	Method which is triggered when this plugin is moved from the INACTIVE to UNCONFIGURED states. This method should be used to fully reset the plugin such that a future call to on_configure_plugin would leave the plugin in a fresh state as though just launched. More...

virtual carma_ros2_utils::CallbackReturn	on_shutdown_plugin ()
	Method which is triggered when this plugin is moved from any state to FINALIZED This method should be used to generate any shutdown logs or final cleanup. More...

virtual carma_ros2_utils::CallbackReturn	on_error_plugin (const std::string &exception_string)
	Method which is triggered when an unhandled exception occurs in this plugin This method should be used to cleanup such that the plugin could be moved to UNCONFIGURED state if possible. More...

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_deactivate (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_cleanup (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_shutdown (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override

	FRIEND_TEST (carma_guidance_plugins_test, connections_test)

Public Attributes
double	trajectory_speed_ = 0.0

std::shared_ptr< pure_pursuit::PurePursuit >	pp_

Private Member Functions
void	platoon_info_cb (const carma_planning_msgs::msg::PlatooningInfo::SharedPtr msg)
	callback function for platoon info More...

double	get_trajectory_speed (const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &trajectory_points)
	calculate average speed of a set of trajectory points More...

	FRIEND_TEST (PlatooningControlPluginTest, test_platoon_info_cb)

	FRIEND_TEST (PlatooningControlPluginTest, test_get_trajectory_speed)

	FRIEND_TEST (PlatooningControlPluginTest, test_generate_controls)

	FRIEND_TEST (PlatooningControlPluginTest, test_current_trajectory_callback)

Private Attributes
PlatooningControlPluginConfig	config_

PlatooningControlWorker	pcw_

PlatoonLeaderInfo	platoon_leader_

double	prev_input_time_ms_ = 0

long	consecutive_input_counter_ = 0

carma_ros2_utils::SubPtr< carma_planning_msgs::msg::TrajectoryPlan >	trajectory_plan_sub_

carma_ros2_utils::SubPtr< carma_planning_msgs::msg::PlatooningInfo >	platoon_info_sub_

carma_ros2_utils::PubPtr< carma_planning_msgs::msg::PlatooningInfo >	platoon_info_pub_

Additional Inherited Members
Protected Member Functions inherited from carma_guidance_plugins::ControlPlugin
void	current_pose_callback (geometry_msgs::msg::PoseStamped::UniquePtr msg)

void	current_twist_callback (geometry_msgs::msg::TwistStamped::UniquePtr msg)

virtual void	current_trajectory_callback (carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg)
	Extending class provided method which can optionally handle trajectory plan callbacks. More...

Protected Attributes inherited from carma_guidance_plugins::ControlPlugin
boost::optional< geometry_msgs::msg::PoseStamped >	current_pose_
	The most recent pose message received by this node. More...

boost::optional< geometry_msgs::msg::TwistStamped >	current_twist_
	The most recent velocity message received by this node. More...

boost::optional< carma_planning_msgs::msg::TrajectoryPlan >	current_trajectory_
	The most recent trajectory received by this plugin. More...

Detailed Description

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.

Definition at line 41 of file platooning_control.hpp.

Constructor & Destructor Documentation

◆ PlatooningControlPlugin()

platooning_control::PlatooningControlPlugin::PlatooningControlPlugin ( const rclcpp::NodeOptions & options )

explicit

PlatooningControlPlugin constructor.

Definition at line 22 of file platooning_control.cpp.

      : carma_guidance_plugins::ControlPlugin(options), config_(PlatooningControlPluginConfig()), pcw_(PlatooningControlWorker())
  {
 
    // Declare parameters
    config_.stand_still_headway_m = declare_parameter<double>("stand_still_headway_m", config_.stand_still_headway_m);
    config_.max_accel_mps2 = declare_parameter<double>("max_accel_mps2", config_.max_accel_mps2);
    config_.kp = declare_parameter<double>("kp", config_.kp);
    config_.kd = declare_parameter<double>("kd", config_.kd);
    config_.ki = declare_parameter<double>("ki", config_.ki);
    config_.max_delta_speed_per_timestep = declare_parameter<double>("max_delta_speed_per_timestep", config_.max_delta_speed_per_timestep);
    config_.min_delta_speed_per_timestep = declare_parameter<double>("min_delta_speed_per_timestep", config_.min_delta_speed_per_timestep);
    config_.adjustment_cap_mps = declare_parameter<double>("adjustment_cap_mps", config_.adjustment_cap_mps);
    config_.cmd_timestamp_ms = declare_parameter<int>("cmd_timestamp_ms", config_.cmd_timestamp_ms);
    config_.integrator_max = declare_parameter<double>("integrator_max", config_.integrator_max);
    config_.integrator_min = declare_parameter<double>("integrator_min", config_.integrator_min);
 
    config_.vehicle_response_lag = declare_parameter<double>("vehicle_response_lag", config_.vehicle_response_lag);
    config_.max_lookahead_dist = declare_parameter<double>("maximum_lookahead_distance", config_.max_lookahead_dist);
    config_.min_lookahead_dist = declare_parameter<double>("minimum_lookahead_distance", config_.min_lookahead_dist);
    config_.speed_to_lookahead_ratio = declare_parameter<double>("speed_to_lookahead_ratio", config_.speed_to_lookahead_ratio);
    config_.is_interpolate_lookahead_point = declare_parameter<bool>("is_interpolate_lookahead_point", config_.is_interpolate_lookahead_point);
    config_.is_delay_compensation = declare_parameter<bool>("is_delay_compensation", config_.is_delay_compensation);
    config_.emergency_stop_distance = declare_parameter<double>("emergency_stop_distance", config_.emergency_stop_distance);
    config_.speed_thres_traveling_direction = declare_parameter<double>("speed_thres_traveling_direction", config_.speed_thres_traveling_direction);
    config_.dist_front_rear_wheels = declare_parameter<double>("dist_front_rear_wheels", config_.dist_front_rear_wheels);
 
    config_.dt = declare_parameter<double>("dt", config_.dt);
    config_.integrator_max_pp = declare_parameter<double>("integrator_max_pp", config_.integrator_max_pp);
    config_.integrator_min_pp = declare_parameter<double>("integrator_min_pp", config_.integrator_min_pp);
    config_.ki_pp = declare_parameter<double>("Ki_pp", config_.ki_pp);
    config_.is_integrator_enabled = declare_parameter<bool>("is_integrator_enabled", config_.is_integrator_enabled);
    config_.enable_max_adjustment_filter = declare_parameter<bool>("enable_max_adjustment_filter", config_.enable_max_adjustment_filter);
    config_.enable_max_accel_filter = declare_parameter<bool>("enable_max_accel_filter", config_.enable_max_accel_filter);
 
    //Global params (from vehicle config)
    config_.vehicle_id  = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
    config_.shutdown_timeout = declare_parameter<int>("control_plugin_shutdown_timeout", config_.shutdown_timeout);
    config_.ignore_initial_inputs = declare_parameter<int>("control_plugin_ignore_initial_inputs", config_.ignore_initial_inputs);
 
    pcw_.ctrl_config_ = std::make_shared<PlatooningControlPluginConfig>(config_);
 
  }

Member Function Documentation

◆ compose_ctrl_cmd()

autoware_msgs::msg::ControlCommandStamped platooning_control::PlatooningControlPlugin::compose_ctrl_cmd	(	double	linear_vel,
		double	steering_angle
	)

Compose control message from speed and steering commands.

Parameters

linear_vel	linear velocity in m/s
steering_angle	steering angle in rad

Returns: control command

Definition at line 334 of file platooning_control.cpp.

  {
    autoware_msgs::msg::ControlCommandStamped cmd_ctrl;
    cmd_ctrl.header.stamp = this->now();
    cmd_ctrl.cmd.linear_velocity = linear_vel;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "ctrl command speed " << cmd_ctrl.cmd.linear_velocity);
    cmd_ctrl.cmd.steering_angle = steering_angle;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "ctrl command steering " << cmd_ctrl.cmd.steering_angle);
 
    return cmd_ctrl;
  }

Referenced by generate_control_signals().

Here is the caller graph for this function:

◆ compose_twist_cmd()

geometry_msgs::msg::TwistStamped platooning_control::PlatooningControlPlugin::compose_twist_cmd	(	double	linear_vel,
		double	angular_vel
	)

Compose twist message from linear and angular velocity commands.

Parameters

linear_vel	linear velocity in m/s
angular_vel	angular velocity in rad/s

Returns: twist message

Definition at line 325 of file platooning_control.cpp.

  {
    geometry_msgs::msg::TwistStamped cmd_twist;
    cmd_twist.twist.linear.x = linear_vel;
    cmd_twist.twist.angular.z = angular_vel;
    cmd_twist.header.stamp = this->now();
    return cmd_twist;
  }

◆ convert_state()

motion::motion_common::State platooning_control::PlatooningControlPlugin::convert_state	(	const geometry_msgs::msg::PoseStamped &	pose,
		const geometry_msgs::msg::TwistStamped &	twist
	)		const

Definition at line 296 of file platooning_control.cpp.

  {
    motion::motion_common::State state;
    state.header = pose.header;
    state.state.x = pose.pose.position.x;
    state.state.y = pose.pose.position.y;
    state.state.z = pose.pose.position.z;
    state.state.heading.real = pose.pose.orientation.w;
    state.state.heading.imag = pose.pose.orientation.z;
 
    state.state.longitudinal_velocity_mps = twist.twist.linear.x;
    return state;
  }

Referenced by generate_control_signals().

Here is the caller graph for this function:

◆ current_trajectory_callback()

void platooning_control::PlatooningControlPlugin::current_trajectory_callback ( const carma_planning_msgs::msg::TrajectoryPlan::UniquePtr tp )

virtual

callback function for trajectory plan

Parameters

msg	trajectory plan msg

Reimplemented from carma_guidance_plugins::ControlPlugin.

Definition at line 310 of file platooning_control.cpp.

  {
    if (tp->trajectory_points.size() < 2) {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("platooning_control"), "PlatooningControlPlugin cannot execute trajectory as only 1 point was provided");
            return;
        }
 
        current_trajectory_ = *tp;
        prev_input_time_ms_ = this->now().nanoseconds() / 1000000;
        ++consecutive_input_counter_;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "New trajectory plan #" << consecutive_input_counter_ << " at time " << prev_input_time_ms_);
        rclcpp::Time tp_time(tp->header.stamp);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "tp header time =                " << tp_time.nanoseconds() / 1000000);
  }

References consecutive_input_counter_, carma_guidance_plugins::ControlPlugin::current_trajectory_, and prev_input_time_ms_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ FRIEND_TEST() [1/4]

platooning_control::PlatooningControlPlugin::FRIEND_TEST	(	PlatooningControlPluginTest	,
		test_current_trajectory_callback
	)

private

◆ FRIEND_TEST() [2/4]

platooning_control::PlatooningControlPlugin::FRIEND_TEST	(	PlatooningControlPluginTest	,
		test_generate_controls
	)

private

◆ FRIEND_TEST() [3/4]

platooning_control::PlatooningControlPlugin::FRIEND_TEST	(	PlatooningControlPluginTest	,
		test_get_trajectory_speed
	)

private

◆ FRIEND_TEST() [4/4]

platooning_control::PlatooningControlPlugin::FRIEND_TEST	(	PlatooningControlPluginTest	,
		test_platoon_info_cb
	)

private

◆ generate_command()

autoware_msgs::msg::ControlCommandStamped platooning_control::PlatooningControlPlugin::generate_command ( )

overridevirtual

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

Returns: The command message to publish

Implements carma_guidance_plugins::ControlPlugin.

Definition at line 201 of file platooning_control.cpp.

  {
 
    autoware_msgs::msg::ControlCommandStamped ctrl_msg;
    if (!current_trajectory_ || !current_pose_ || !current_twist_)
      return ctrl_msg;
 
    // If it has been a long time since input data has arrived then reset the input counter and return
    // Note: this quiets the controller after its input stream stops, which is necessary to allow
    // the replacement controller to publish on the same output topic after this one is done.
    double current_time_ms = this->now().nanoseconds() / 1e6;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "current_time_ms = " << current_time_ms << ", prev_input_time_ms_ = " << prev_input_time_ms_ << ", input counter = " << consecutive_input_counter_);
 
    if(current_time_ms - prev_input_time_ms_ > config_.shutdown_timeout)
    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "returning due to timeout.");
        consecutive_input_counter_ = 0;
        return ctrl_msg;
    }
 
    // If there have not been enough consecutive timely inputs then return (waiting for
    // previous control plugin to time out and stop publishing, since it uses same output topic)
    if (consecutive_input_counter_ <= config_.ignore_initial_inputs)
    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "returning due to first data input");
        return ctrl_msg;
    }
 
    carma_planning_msgs::msg::TrajectoryPlanPoint second_trajectory_point = current_trajectory_.get().trajectory_points[1];
 
    trajectory_speed_ = get_trajectory_speed(current_trajectory_.get().trajectory_points);
 
    ctrl_msg = generate_control_signals(second_trajectory_point, current_pose_.get(), current_twist_.get());
 
    return ctrl_msg;
 
  }

References config_, consecutive_input_counter_, carma_guidance_plugins::ControlPlugin::current_pose_, carma_guidance_plugins::ControlPlugin::current_trajectory_, carma_guidance_plugins::ControlPlugin::current_twist_, generate_control_signals(), get_trajectory_speed(), platooning_control::PlatooningControlPluginConfig::ignore_initial_inputs, prev_input_time_ms_, platooning_control::PlatooningControlPluginConfig::shutdown_timeout, and trajectory_speed_.

Here is the call graph for this function:

◆ generate_control_signals()

autoware_msgs::msg::ControlCommandStamped platooning_control::PlatooningControlPlugin::generate_control_signals	(	const carma_planning_msgs::msg::TrajectoryPlanPoint &	first_trajectory_point,
		const geometry_msgs::msg::PoseStamped &	current_pose,
		const geometry_msgs::msg::TwistStamped &	current_twist
	)

generate control signal by calculating speed and steering commands.

Parameters

point0	start point of control window
point_end	end point of control wondow

Definition at line 272 of file platooning_control.cpp.

  {
    pcw_.set_current_speed(trajectory_speed_); //TODO why this and not the actual vehicle speed?  Method name suggests different use than this.
    // pcw_.set_current_speed(current_twist_.get());
    pcw_.set_leader(platoon_leader_);
    pcw_.generate_speed(first_trajectory_point);
 
    motion::control::controller_common::State state_tf = convert_state(current_pose, current_twist);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Forced from frame_id: " << state_tf.header.frame_id << ", into: " << current_trajectory_.get().header.frame_id);
 
    current_trajectory_.get().header.frame_id = state_tf.header.frame_id;
 
    auto autoware_traj_plan = basic_autonomy::waypoint_generation::process_trajectory_plan(current_trajectory_.get(), config_.vehicle_response_lag);
 
    pp_->set_trajectory(autoware_traj_plan);
    const auto cmd{pp_->compute_command(state_tf)};
 
    auto steer_cmd = cmd.front_wheel_angle_rad; //autoware sets the front wheel angle as the calculated steer. https://github.com/usdot-fhwa-stol/autoware.auto/blob/3450f94fa694f51b00de272d412722d65a2c2d3e/AutowareAuto/src/control/pure_pursuit/src/pure_pursuit.cpp#L88
 
    autoware_msgs::msg::ControlCommandStamped ctrl_msg = compose_ctrl_cmd(pcw_.speedCmd_, steer_cmd);
 
    return ctrl_msg;
  }

References compose_ctrl_cmd(), config_, convert_state(), carma_guidance_plugins::ControlPlugin::current_trajectory_, platooning_control::PlatooningControlWorker::generate_speed(), pcw_, platoon_leader_, pp_, basic_autonomy::waypoint_generation::process_trajectory_plan(), platooning_control::PlatooningControlWorker::set_current_speed(), platooning_control::PlatooningControlWorker::set_leader(), platooning_control::PlatooningControlWorker::speedCmd_, trajectory_speed_, and platooning_control::PlatooningControlPluginConfig::vehicle_response_lag.

Referenced by generate_command().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_availability()

bool platooning_control::PlatooningControlPlugin::get_availability ( )

overridevirtual

Returns availability of plugin. Always true.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 346 of file platooning_control.cpp.

                                                 {
    return true; // TODO for user implement actual check on availability if applicable to plugin
  }

◆ get_trajectory_speed()

double platooning_control::PlatooningControlPlugin::get_trajectory_speed ( const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > & trajectory_points )

private

calculate average speed of a set of trajectory points

Parameters

trajectory_points set of trajectory points

Returns: trajectory speed

Definition at line 355 of file platooning_control.cpp.

  {
    double trajectory_speed = 0;
 
    double dx1 = trajectory_points[trajectory_points.size()-1].x - trajectory_points[0].x;
    double dy1 = trajectory_points[trajectory_points.size()-1].y - trajectory_points[0].y;
    double d1 = sqrt(dx1*dx1 + dy1*dy1);
    double t1 = (rclcpp::Time((trajectory_points[trajectory_points.size()-1].target_time)).nanoseconds() - rclcpp::Time(trajectory_points[0].target_time).nanoseconds())/1e9;
 
    double avg_speed = d1/t1;
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "trajectory_points size = " << trajectory_points.size() << ", d1 = " << d1 << ", t1 = " << t1 << ", avg_speed = " << avg_speed);
 
    for(size_t i = 0; i < trajectory_points.size() - 2; i++ )
    {
        double dx = trajectory_points[i + 1].x - trajectory_points[i].x;
        double dy = trajectory_points[i + 1].y - trajectory_points[i].y;
        double d = sqrt(dx*dx + dy*dy);
        double t = rclcpp::Time((trajectory_points[i + 1].target_time)).seconds() - rclcpp::Time(trajectory_points[i].target_time).seconds();
        double v = d/t;
        if(v > trajectory_speed)
        {
            trajectory_speed = v;
        }
    }
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "trajectory speed: " << trajectory_speed);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "avg trajectory speed: " << avg_speed);
 
    return avg_speed; //TODO: why are 2 speeds being calculated? Which should be returned?
 
  }

References process_bag::i.

Referenced by generate_command().

Here is the caller graph for this function:

◆ get_version_id()

std::string platooning_control::PlatooningControlPlugin::get_version_id ( )

overridevirtual

Returns version id of plugn.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 350 of file platooning_control.cpp.

                                                    {
    return "v1.0";
  }

◆ on_configure_plugin()

carma_ros2_utils::CallbackReturn platooning_control::PlatooningControlPlugin::on_configure_plugin ( )

overridevirtual

This method should be used to load parameters and will be called on the configure state transition.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 114 of file platooning_control.cpp.

  {
    // Reset config
    config_ = PlatooningControlPluginConfig();
 
    // Load parameters
    get_parameter<double>("stand_still_headway_m", config_.stand_still_headway_m);
    get_parameter<double>("max_accel_mps2", config_.max_accel_mps2);
    get_parameter<double>("kp", config_.kp);
    get_parameter<double>("kd", config_.kd);
    get_parameter<double>("ki", config_.ki);
    get_parameter<double>("max_delta_speed_per_timestep", config_.max_delta_speed_per_timestep);
    get_parameter<double>("min_delta_speed_per_timestep", config_.min_delta_speed_per_timestep);
    get_parameter<double>("adjustment_cap_mps", config_.adjustment_cap_mps);
    get_parameter<int>("cmd_timestamp_ms", config_.cmd_timestamp_ms);
    get_parameter<double>("integrator_max", config_.integrator_max);
    get_parameter<double>("integrator_min", config_.integrator_min);
 
    get_parameter<std::string>("vehicle_id", config_.vehicle_id);
    get_parameter<int>("control_plugin_shutdown_timeout", config_.shutdown_timeout);
    get_parameter<int>("control_plugin_ignore_initial_inputs", config_.ignore_initial_inputs);
    get_parameter<bool>("enable_max_adjustment_filter", config_.enable_max_adjustment_filter);
    get_parameter<bool>("enable_max_accel_filter", config_.enable_max_accel_filter);
 
   //Pure Pursuit params
    get_parameter<double>("vehicle_response_lag", config_.vehicle_response_lag);
    get_parameter<double>("maximum_lookahead_distance", config_.max_lookahead_dist);
    get_parameter<double>("minimum_lookahead_distance", config_.min_lookahead_dist);
    get_parameter<double>("speed_to_lookahead_ratio", config_.speed_to_lookahead_ratio);
    get_parameter<bool>("is_interpolate_lookahead_point", config_.is_interpolate_lookahead_point);
    get_parameter<bool>("is_delay_compensation", config_.is_delay_compensation);
    get_parameter<double>("emergency_stop_distance", config_.emergency_stop_distance);
    get_parameter<double>("speed_thres_traveling_direction", config_.speed_thres_traveling_direction);
    get_parameter<double>("dist_front_rear_wheels", config_.dist_front_rear_wheels);
 
    get_parameter<double>("dt", config_.dt);
    get_parameter<double>("integrator_max_pp", config_.integrator_max_pp);
    get_parameter<double>("integrator_min_pp", config_.integrator_min_pp);
    get_parameter<double>("Ki_pp", config_.ki_pp);
    get_parameter<bool>("is_integrator_enabled", config_.is_integrator_enabled);
 
 
    RCLCPP_INFO_STREAM(rclcpp::get_logger("platooning_control"), "Loaded Params: " << config_);
 
    // create config for pure_pursuit worker
    pure_pursuit::Config cfg{
      config_.min_lookahead_dist,
      config_.max_lookahead_dist,
      config_.speed_to_lookahead_ratio,
      config_.is_interpolate_lookahead_point,
      config_.is_delay_compensation,
      config_.emergency_stop_distance,
      config_.speed_thres_traveling_direction,
      config_.dist_front_rear_wheels,
    };
 
    pure_pursuit::IntegratorConfig i_cfg;
    i_cfg.dt = config_.dt;
    i_cfg.integrator_max_pp = config_.integrator_max_pp;
    i_cfg.integrator_min_pp = config_.integrator_min_pp;
    i_cfg.Ki_pp = config_.ki_pp;
    i_cfg.integral = 0.0; // accumulator of integral starts from 0
    i_cfg.is_integrator_enabled = config_.is_integrator_enabled;
 
    pp_ = std::make_shared<pure_pursuit::PurePursuit>(cfg, i_cfg);
 
    // Register runtime parameter update callback
    add_on_set_parameters_callback(std::bind(&PlatooningControlPlugin::parameter_update_callback, this, std_ph::_1));
 
 
    // Trajectory Plan Subscriber
    trajectory_plan_sub_ = create_subscription<carma_planning_msgs::msg::TrajectoryPlan>("platooning_control/plan_trajectory", 1,
                                                                                            std::bind(&PlatooningControlPlugin::current_trajectory_callback, this, std_ph::_1));
 
    // Platoon Info Subscriber
    platoon_info_sub_ = create_subscription<carma_planning_msgs::msg::PlatooningInfo>("platoon_info", 1, std::bind(&PlatooningControlPlugin::platoon_info_cb, this, std_ph::_1));
 
 
    //Control Publishers
    platoon_info_pub_ = create_publisher<carma_planning_msgs::msg::PlatooningInfo>("platooning_info", 1);
 
 
    // Return success if everthing initialized successfully
    return CallbackReturn::SUCCESS;
  }

Here is the call graph for this function:

◆ parameter_update_callback()

rcl_interfaces::msg::SetParametersResult platooning_control::PlatooningControlPlugin::parameter_update_callback ( const std::vector< rclcpp::Parameter > & parameters )

Callback for dynamic parameter updates.

Definition at line 66 of file platooning_control.cpp.

  {
    auto error_double = update_params<double>({
      {"stand_still_headway_m", config_.stand_still_headway_m},
      {"max_accel_mps2", config_.max_accel_mps2},
      {"kp", config_.kp},
      {"kd", config_.kd},
      {"ki", config_.ki},
      {"max_delta_speed_per_timestep", config_.max_delta_speed_per_timestep},
      {"min_delta_speed_per_timestep", config_.min_delta_speed_per_timestep},
      {"adjustment_cap_mps", config_.adjustment_cap_mps},
      {"integrator_max", config_.integrator_max},
      {"integrator_min", config_.integrator_min},
 
      {"vehicle_response_lag", config_.vehicle_response_lag},
      {"max_lookahead_dist", config_.max_lookahead_dist},
      {"min_lookahead_dist", config_.min_lookahead_dist},
      {"speed_to_lookahead_ratio", config_.speed_to_lookahead_ratio},
      {"emergency_stop_distance",config_.emergency_stop_distance},
      {"speed_thres_traveling_direction", config_.speed_thres_traveling_direction},
      {"dist_front_rear_wheels", config_.dist_front_rear_wheels},
      {"dt", config_.dt},
      {"integrator_max_pp", config_.integrator_max_pp},
      {"integrator_min_pp", config_.integrator_min_pp},
      {"Ki_pp", config_.ki_pp},
    }, parameters);
 
    auto error_int = update_params<int>({
      {"cmd_timestamp_ms", config_.cmd_timestamp_ms},
    }, parameters);
 
    auto error_bool = update_params<bool>({
      {"is_interpolate_lookahead_point", config_.is_interpolate_lookahead_point},
      {"is_delay_compensation",config_.is_delay_compensation},
      {"is_integrator_enabled", config_.is_integrator_enabled},
      {"enable_max_adjustment_filter", config_.enable_max_adjustment_filter},
      {"enable_max_accel_filter", config_.enable_max_accel_filter},
  }, parameters);
 
    // vehicle_id, control_plugin_shutdown_timeout and control_plugin_ignore_initial_inputs are not updated as they are global params
    rcl_interfaces::msg::SetParametersResult result;
 
    result.successful = !error_double && !error_int && !error_bool;
 
    return result;
 
  }

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ platoon_info_cb()

void platooning_control::PlatooningControlPlugin::platoon_info_cb ( const carma_planning_msgs::msg::PlatooningInfo::SharedPtr msg )

private

callback function for platoon info

Parameters

msg	platoon info msg

Definition at line 239 of file platooning_control.cpp.

  {
 
    platoon_leader_.staticId = msg->leader_id;
    platoon_leader_.vehiclePosition = msg->leader_downtrack_distance;
    platoon_leader_.commandSpeed = msg->leader_cmd_speed;
    // TODO: index is 0 temp to test the leader state
    platoon_leader_.NumberOfVehicleInFront = msg->host_platoon_position;
    platoon_leader_.leaderIndex = 0;
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Platoon leader leader id:  " << platoon_leader_.staticId);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Platoon leader leader pose:  " << platoon_leader_.vehiclePosition);
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Platoon leader leader cmd speed:  " << platoon_leader_.commandSpeed);
 
    carma_planning_msgs::msg::PlatooningInfo platooning_info_msg = *msg;
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "platooning_info_msg.actual_gap:  " << platooning_info_msg.actual_gap);
 
    if (platooning_info_msg.actual_gap > 5.0)
    {
        platooning_info_msg.actual_gap -= 5.0; // TODO: temporary: should be vehicle length
    }
 
    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "platooning_info_msg.actual_gap:  " << platooning_info_msg.actual_gap);
    // platooing_info_msg.desired_gap = pcw_.desired_gap_;
    // platooing_info_msg.actual_gap = pcw_.actual_gap_;
    pcw_.actual_gap_ = platooning_info_msg.actual_gap;
    pcw_.desired_gap_ = platooning_info_msg.desired_gap;
 
    platooning_info_msg.host_cmd_speed = pcw_.speedCmd_;
    platoon_info_pub_->publish(platooning_info_msg);
  }

Referenced by on_configure_plugin().

Here is the caller graph for this function:

Member Data Documentation

◆ config_

PlatooningControlPluginConfig platooning_control::PlatooningControlPlugin::config_

private

Definition at line 115 of file platooning_control.hpp.

Referenced by PlatooningControlPlugin(), generate_command(), generate_control_signals(), on_configure_plugin(), and parameter_update_callback().

◆ consecutive_input_counter_

long platooning_control::PlatooningControlPlugin::consecutive_input_counter_ = 0

private

Definition at line 123 of file platooning_control.hpp.

Referenced by current_trajectory_callback(), and generate_command().

◆ pcw_

PlatooningControlWorker platooning_control::PlatooningControlPlugin::pcw_

private

Definition at line 118 of file platooning_control.hpp.

Referenced by PlatooningControlPlugin(), generate_control_signals(), and platoon_info_cb().

◆ platoon_info_pub_

carma_ros2_utils::PubPtr<carma_planning_msgs::msg::PlatooningInfo> platooning_control::PlatooningControlPlugin::platoon_info_pub_

private

Definition at line 144 of file platooning_control.hpp.

Referenced by on_configure_plugin(), and platoon_info_cb().

◆ platoon_info_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::PlatooningInfo> platooning_control::PlatooningControlPlugin::platoon_info_sub_

private

Definition at line 141 of file platooning_control.hpp.

Referenced by on_configure_plugin().

◆ platoon_leader_

PlatoonLeaderInfo platooning_control::PlatooningControlPlugin::platoon_leader_

private

Definition at line 121 of file platooning_control.hpp.

Referenced by generate_control_signals(), and platoon_info_cb().

◆ pp_

std::shared_ptr<pure_pursuit::PurePursuit> platooning_control::PlatooningControlPlugin::pp_

Definition at line 109 of file platooning_control.hpp.

Referenced by generate_control_signals(), and on_configure_plugin().

◆ prev_input_time_ms_

double platooning_control::PlatooningControlPlugin::prev_input_time_ms_ = 0

private

Definition at line 122 of file platooning_control.hpp.

Referenced by current_trajectory_callback(), and generate_command().

◆ trajectory_plan_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::TrajectoryPlan> platooning_control::PlatooningControlPlugin::trajectory_plan_sub_

private

Definition at line 140 of file platooning_control.hpp.

Referenced by on_configure_plugin().

◆ trajectory_speed_

double platooning_control::PlatooningControlPlugin::trajectory_speed_ = 0.0

Definition at line 67 of file platooning_control.hpp.

Referenced by generate_command(), and generate_control_signals().

The documentation for this class was generated from the following files:

platooning_control/include/platooning_control/platooning_control.hpp
platooning_control/src/platooning_control.cpp

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ PlatooningControlPlugin()

Member Function Documentation

◆ compose_ctrl_cmd()

◆ compose_twist_cmd()

◆ convert_state()

◆ current_trajectory_callback()

◆ FRIEND_TEST() [1/4]

◆ FRIEND_TEST() [2/4]

◆ FRIEND_TEST() [3/4]

◆ FRIEND_TEST() [4/4]

◆ generate_command()

◆ generate_control_signals()

◆ get_availability()

◆ get_trajectory_speed()

◆ get_version_id()

◆ on_configure_plugin()

◆ parameter_update_callback()

◆ platoon_info_cb()

Member Data Documentation

◆ config_

◆ consecutive_input_counter_

◆ pcw_

◆ platoon_info_pub_

◆ platoon_info_sub_

◆ platoon_leader_

◆ pp_

◆ prev_input_time_ms_

◆ trajectory_plan_sub_

◆ trajectory_speed_