Carma-platform v4.2.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
carma_guidance_plugins::ControlPlugin Class Referenceabstract

ControlPlugin base class which can be extended by user provided plugins which wish to implement the Control Plugin ROS API. More...

#include <control_plugin.hpp>

Inheritance diagram for carma_guidance_plugins::ControlPlugin:
Inheritance graph
Collaboration diagram for carma_guidance_plugins::ControlPlugin:
Collaboration graph

Public Member Functions

 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::WMListenerget_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)
 

Protected Member Functions

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

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

Private Attributes

carma_ros2_utils::SubPtr< geometry_msgs::msg::PoseStamped > current_pose_sub_
 
carma_ros2_utils::SubPtr< geometry_msgs::msg::TwistStamped > current_velocity_sub_
 
carma_ros2_utils::SubPtr< carma_planning_msgs::msg::TrajectoryPlan > trajectory_plan_sub_
 
carma_ros2_utils::PubPtr< autoware_msgs::msg::ControlCommandStamped > vehicle_cmd_pub_
 
rclcpp::TimerBase::SharedPtr command_timer_
 

Detailed Description

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.

Definition at line 38 of file control_plugin.hpp.

Constructor & Destructor Documentation

◆ ControlPlugin()

carma_guidance_plugins::ControlPlugin::ControlPlugin ( const rclcpp::NodeOptions &  options)
explicit

ControlPlugin constructor.

Definition at line 25 of file control_plugin.cpp.

26 : PluginBaseNode(options)
27 {}
PluginBaseNode(const rclcpp::NodeOptions &)
PluginBaseNode constructor.

◆ ~ControlPlugin()

virtual carma_guidance_plugins::ControlPlugin::~ControlPlugin ( )
virtualdefault

Virtual destructor for safe deletion.

Member Function Documentation

◆ current_pose_callback()

void carma_guidance_plugins::ControlPlugin::current_pose_callback ( geometry_msgs::msg::PoseStamped::UniquePtr  msg)
protected

Definition at line 39 of file control_plugin.cpp.

40 {
41 RCLCPP_DEBUG(rclcpp::get_logger("carma_guidance_plugins"), "Received pose message");
42 current_pose_ = *msg;
43 }
boost::optional< geometry_msgs::msg::PoseStamped > current_pose_
The most recent pose message received by this node.

References current_pose_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ current_trajectory_callback()

void carma_guidance_plugins::ControlPlugin::current_trajectory_callback ( carma_planning_msgs::msg::TrajectoryPlan::UniquePtr  msg)
protectedvirtual

Extending class provided method which can optionally handle trajectory plan callbacks.

Reimplemented in platooning_control::PlatooningControlPlugin.

Definition at line 51 of file control_plugin.cpp.

52 {
53 RCLCPP_DEBUG(rclcpp::get_logger("carma_guidance_plugins"), "Received trajectory message");
55 }
boost::optional< carma_planning_msgs::msg::TrajectoryPlan > current_trajectory_
The most recent trajectory received by this plugin.

References current_trajectory_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ current_twist_callback()

void carma_guidance_plugins::ControlPlugin::current_twist_callback ( geometry_msgs::msg::TwistStamped::UniquePtr  msg)
protected

Definition at line 45 of file control_plugin.cpp.

46 {
47 RCLCPP_DEBUG(rclcpp::get_logger("carma_guidance_plugins"), "Received twist message");
48 current_twist_ = *msg;
49 }
boost::optional< geometry_msgs::msg::TwistStamped > current_twist_
The most recent velocity message received by this node.

References current_twist_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ generate_command()

virtual autoware_msgs::msg::ControlCommandStamped carma_guidance_plugins::ControlPlugin::generate_command ( )
pure virtual

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

Implemented in platooning_control::PlatooningControlPlugin, pure_pursuit_wrapper::PurePursuitWrapperNode, SUB::Node, and trajectory_follower_wrapper::TrajectoryFollowerWrapperNode.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ get_capability()

std::string carma_guidance_plugins::ControlPlugin::get_capability ( )
overridevirtual

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.

Returns
Forward slash separated string describing the plugin's capabilities per the plugin capabilites API

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 29 of file control_plugin.cpp.

30 {
31 return "control/trajectory_control";
32 }

◆ get_type()

uint8_t carma_guidance_plugins::ControlPlugin::get_type ( )
finaloverridevirtual

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.

Returns
The extending class type or UNKOWN if the class or no parent class has implement this method.

Reimplemented from carma_guidance_plugins::PluginBaseNode.

Definition at line 34 of file control_plugin.cpp.

35 {
36 return carma_planning_msgs::msg::Plugin::CONTROL;
37 }

◆ handle_on_activate()

carma_ros2_utils::CallbackReturn carma_guidance_plugins::ControlPlugin::handle_on_activate ( const rclcpp_lifecycle::State &  prev_state)
finaloverride

Definition at line 85 of file control_plugin.cpp.

86 {
87 return PluginBaseNode::handle_on_activate(prev_state);
88 }
carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override

References carma_guidance_plugins::PluginBaseNode::handle_on_activate().

Here is the call graph for this function:

◆ handle_on_cleanup()

carma_ros2_utils::CallbackReturn carma_guidance_plugins::ControlPlugin::handle_on_cleanup ( const rclcpp_lifecycle::State &  prev_state)
finaloverride

Definition at line 95 of file control_plugin.cpp.

96 {
97 return PluginBaseNode::handle_on_cleanup(prev_state);
98 }
carma_ros2_utils::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &) override

References carma_guidance_plugins::PluginBaseNode::handle_on_cleanup().

Here is the call graph for this function:

◆ handle_on_configure()

carma_ros2_utils::CallbackReturn carma_guidance_plugins::ControlPlugin::handle_on_configure ( const rclcpp_lifecycle::State &  prev_state)
finaloverride

Definition at line 57 of file control_plugin.cpp.

58 {
59 // Initialize subscribers and publishers
60 current_pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>("current_pose", 1,
61 std::bind(&ControlPlugin::current_pose_callback, this, std_ph::_1));
62
63 current_velocity_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>("vehicle/twist", 1,
64 std::bind(&ControlPlugin::current_twist_callback, this, std_ph::_1));
65
66 trajectory_plan_sub_ = create_subscription<carma_planning_msgs::msg::TrajectoryPlan>(std::string(get_name()) + "/plan_trajectory", 1,
67 std::bind(&ControlPlugin::current_trajectory_callback, this, std_ph::_1));
68
69 vehicle_cmd_pub_ = create_publisher<autoware_msgs::msg::ControlCommandStamped>("ctrl_raw", 1);
70
71 command_timer_ = create_timer(
72 get_clock(),
73 std::chrono::milliseconds(33), // Spin at 30 Hz per plugin API
74 [this]() {
75 if (this->get_activation_status()) // Only trigger when activated
76 {
77 this->vehicle_cmd_pub_->publish(this->generate_command());
78
79 }
80 });
81
82 return PluginBaseNode::handle_on_configure(prev_state);
83 }
rclcpp::TimerBase::SharedPtr command_timer_
void current_pose_callback(geometry_msgs::msg::PoseStamped::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.
carma_ros2_utils::SubPtr< geometry_msgs::msg::PoseStamped > current_pose_sub_
carma_ros2_utils::SubPtr< carma_planning_msgs::msg::TrajectoryPlan > trajectory_plan_sub_
carma_ros2_utils::PubPtr< autoware_msgs::msg::ControlCommandStamped > vehicle_cmd_pub_
virtual autoware_msgs::msg::ControlCommandStamped generate_command()=0
Extending class provided method which should generate a command message which will be published to th...
void current_twist_callback(geometry_msgs::msg::TwistStamped::UniquePtr msg)
carma_ros2_utils::SubPtr< geometry_msgs::msg::TwistStamped > current_velocity_sub_
virtual bool get_activation_status() final
Returns the activation status of this plugin. The plugins API callbacks will only be triggered when t...
carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override

References command_timer_, current_pose_callback(), current_pose_sub_, current_trajectory_callback(), current_twist_callback(), current_velocity_sub_, generate_command(), carma_guidance_plugins::PluginBaseNode::get_activation_status(), carma_guidance_plugins::PluginBaseNode::handle_on_configure(), trajectory_plan_sub_, and vehicle_cmd_pub_.

Here is the call graph for this function:

◆ handle_on_deactivate()

carma_ros2_utils::CallbackReturn carma_guidance_plugins::ControlPlugin::handle_on_deactivate ( const rclcpp_lifecycle::State &  prev_state)
finaloverride

Definition at line 90 of file control_plugin.cpp.

91 {
92 return PluginBaseNode::handle_on_deactivate(prev_state);
93 }
carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override

References carma_guidance_plugins::PluginBaseNode::handle_on_deactivate().

Here is the call graph for this function:

◆ handle_on_error()

carma_ros2_utils::CallbackReturn carma_guidance_plugins::ControlPlugin::handle_on_error ( const rclcpp_lifecycle::State &  prev_state,
const std::string &  exception_string 
)
finaloverride

Definition at line 105 of file control_plugin.cpp.

106 {
107 return PluginBaseNode::handle_on_error(prev_state, exception_string);
108 }
carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &, const std::string &exception_string) override

References carma_guidance_plugins::PluginBaseNode::handle_on_error().

Here is the call graph for this function:

◆ handle_on_shutdown()

carma_ros2_utils::CallbackReturn carma_guidance_plugins::ControlPlugin::handle_on_shutdown ( const rclcpp_lifecycle::State &  prev_state)
finaloverride

Definition at line 100 of file control_plugin.cpp.

101 {
102 return PluginBaseNode::handle_on_shutdown(prev_state);
103 }
carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override

References carma_guidance_plugins::PluginBaseNode::handle_on_shutdown().

Here is the call graph for this function:

Member Data Documentation

◆ command_timer_

rclcpp::TimerBase::SharedPtr carma_guidance_plugins::ControlPlugin::command_timer_
private

Definition at line 51 of file control_plugin.hpp.

Referenced by handle_on_configure().

◆ current_pose_

◆ current_pose_sub_

carma_ros2_utils::SubPtr<geometry_msgs::msg::PoseStamped> carma_guidance_plugins::ControlPlugin::current_pose_sub_
private

Definition at line 43 of file control_plugin.hpp.

Referenced by handle_on_configure().

◆ current_trajectory_

◆ current_twist_

boost::optional<geometry_msgs::msg::TwistStamped> carma_guidance_plugins::ControlPlugin::current_twist_
protected

◆ current_velocity_sub_

carma_ros2_utils::SubPtr<geometry_msgs::msg::TwistStamped> carma_guidance_plugins::ControlPlugin::current_velocity_sub_
private

Definition at line 44 of file control_plugin.hpp.

Referenced by handle_on_configure().

◆ trajectory_plan_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::TrajectoryPlan> carma_guidance_plugins::ControlPlugin::trajectory_plan_sub_
private

Definition at line 45 of file control_plugin.hpp.

Referenced by handle_on_configure().

◆ vehicle_cmd_pub_

carma_ros2_utils::PubPtr<autoware_msgs::msg::ControlCommandStamped> carma_guidance_plugins::ControlPlugin::vehicle_cmd_pub_
private

Definition at line 48 of file control_plugin.hpp.

Referenced by handle_on_configure().


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