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.
|
#include <trajectory_follower_wrapper_node.hpp>
Public Member Functions | |
TrajectoryFollowerWrapperNode (const rclcpp::NodeOptions &options) | |
Node constructor. More... | |
rcl_interfaces::msg::SetParametersResult | parameter_update_callback (const std::vector< rclcpp::Parameter > ¶meters) |
Callback for dynamic parameter updates. More... | |
void | autoware_info_timer_callback () |
Timer callback to spin at 30 hz and frequently publish autoware kinematic state and trajectory. More... | |
void | ackermann_control_cb (const autoware_auto_msgs::msg::AckermannControlCommand::SharedPtr msg) |
autoware's control subscription callback More... | |
bool | isControlCommandOld (const autoware_auto_msgs::msg::AckermannControlCommand &cmd) const |
Check to see if the received control command recent or old. More... | |
autoware_auto_msgs::msg::VehicleKinematicState | convert_state (const geometry_msgs::msg::PoseStamped &pose, const geometry_msgs::msg::TwistStamped &twist) const |
convert vehicle's pose and twist messages to autoware kinematic state More... | |
autoware_msgs::msg::ControlCommandStamped | convert_cmd (const autoware_auto_msgs::msg::AckermannControlCommand &cmd) const |
convert autoware Ackermann control command to autoware stamped control command More... | |
double | get_wheel_angle_rad_from_twist (const geometry_msgs::msg::TwistStamped &twist) const |
calculate wheel angle in rad from angular velocity in twist message 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... | |
bool | get_availability () override |
Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More... | |
std::string | get_version_id () override |
Returns the version id of this plugin. 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... | |
FRIEND_TEST (TesttrajectoryFollowerWrapper, TestControlCallback) | |
FRIEND_TEST (TesttrajectoryFollowerWrapper, TestThreshold) | |
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 | |
TrajectoryFollowerWrapperConfig | config_ |
Private Attributes | |
carma_ros2_utils::SubPtr< autoware_auto_msgs::msg::AckermannControlCommand > | control_cmd_sub_ |
carma_ros2_utils::PubPtr< autoware_auto_msgs::msg::Trajectory > | autoware_traj_pub_ |
carma_ros2_utils::PubPtr< autoware_auto_msgs::msg::VehicleKinematicState > | autoware_state_pub_ |
rclcpp::TimerBase::SharedPtr | autoware_info_timer_ |
std::optional< autoware_auto_msgs::msg::AckermannControlCommand > | received_ctrl_command_ |
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... | |
Definition at line 37 of file trajectory_follower_wrapper_node.hpp.
|
explicit |
Node constructor.
Definition at line 22 of file trajectory_follower_wrapper_node.cpp.
References config_, trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::incoming_cmd_time_threshold, trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::vehicle_response_lag, and trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::vehicle_wheel_base.
void trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::ackermann_control_cb | ( | const autoware_auto_msgs::msg::AckermannControlCommand::SharedPtr | msg | ) |
autoware's control subscription callback
Definition at line 119 of file trajectory_follower_wrapper_node.cpp.
References received_ctrl_command_.
Referenced by on_configure_plugin().
void trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::autoware_info_timer_callback | ( | ) |
Timer callback to spin at 30 hz and frequently publish autoware kinematic state and trajectory.
Definition at line 88 of file trajectory_follower_wrapper_node.cpp.
References autoware_state_pub_, autoware_traj_pub_, config_, convert_state(), carma_guidance_plugins::ControlPlugin::current_pose_, carma_guidance_plugins::ControlPlugin::current_trajectory_, carma_guidance_plugins::ControlPlugin::current_twist_, basic_autonomy::waypoint_generation::process_trajectory_plan(), and trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::vehicle_response_lag.
Referenced by on_configure_plugin().
autoware_msgs::msg::ControlCommandStamped trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::convert_cmd | ( | const autoware_auto_msgs::msg::AckermannControlCommand & | cmd | ) | const |
convert autoware Ackermann control command to autoware stamped control command
Definition at line 147 of file trajectory_follower_wrapper_node.cpp.
References carma_cooperative_perception::to_string().
Referenced by generate_command().
autoware_auto_msgs::msg::VehicleKinematicState trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::convert_state | ( | const geometry_msgs::msg::PoseStamped & | pose, |
const geometry_msgs::msg::TwistStamped & | twist | ||
) | const |
convert vehicle's pose and twist messages to autoware kinematic state
Definition at line 126 of file trajectory_follower_wrapper_node.cpp.
References get_wheel_angle_rad_from_twist().
Referenced by autoware_info_timer_callback().
trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::FRIEND_TEST | ( | TesttrajectoryFollowerWrapper | , |
TestControlCallback | |||
) |
trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::FRIEND_TEST | ( | TesttrajectoryFollowerWrapper | , |
TestThreshold | |||
) |
|
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
Implements carma_guidance_plugins::ControlPlugin.
Definition at line 165 of file trajectory_follower_wrapper_node.cpp.
References convert_cmd(), carma_guidance_plugins::ControlPlugin::current_pose_, carma_guidance_plugins::ControlPlugin::current_trajectory_, carma_guidance_plugins::ControlPlugin::current_twist_, isControlCommandOld(), and received_ctrl_command_.
|
overridevirtual |
Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes.
Implements carma_guidance_plugins::PluginBaseNode.
Definition at line 200 of file trajectory_follower_wrapper_node.cpp.
|
overridevirtual |
Returns the version id of this plugin.
Implements carma_guidance_plugins::PluginBaseNode.
Definition at line 205 of file trajectory_follower_wrapper_node.cpp.
double trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::get_wheel_angle_rad_from_twist | ( | const geometry_msgs::msg::TwistStamped & | twist | ) | const |
calculate wheel angle in rad from angular velocity in twist message
Definition at line 107 of file trajectory_follower_wrapper_node.cpp.
References config_, EPSILON, and trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::vehicle_wheel_base.
Referenced by convert_state().
bool trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::isControlCommandOld | ( | const autoware_auto_msgs::msg::AckermannControlCommand & | cmd | ) | const |
Check to see if the received control command recent or old.
Definition at line 188 of file trajectory_follower_wrapper_node.cpp.
References config_, and trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::incoming_cmd_time_threshold.
Referenced by generate_command().
|
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 48 of file trajectory_follower_wrapper_node.cpp.
References ackermann_control_cb(), autoware_info_timer_, autoware_info_timer_callback(), autoware_state_pub_, autoware_traj_pub_, config_, control_cmd_sub_, trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::incoming_cmd_time_threshold, parameter_update_callback(), trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::vehicle_response_lag, and trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::vehicle_wheel_base.
rcl_interfaces::msg::SetParametersResult trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::parameter_update_callback | ( | const std::vector< rclcpp::Parameter > & | parameters | ) |
Callback for dynamic parameter updates.
Definition at line 33 of file trajectory_follower_wrapper_node.cpp.
References config_, trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::incoming_cmd_time_threshold, trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::vehicle_response_lag, and trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::vehicle_wheel_base.
Referenced by on_configure_plugin().
|
private |
Definition at line 49 of file trajectory_follower_wrapper_node.hpp.
Referenced by on_configure_plugin().
|
private |
Definition at line 46 of file trajectory_follower_wrapper_node.hpp.
Referenced by autoware_info_timer_callback(), and on_configure_plugin().
|
private |
Definition at line 45 of file trajectory_follower_wrapper_node.hpp.
Referenced by autoware_info_timer_callback(), and on_configure_plugin().
TrajectoryFollowerWrapperConfig trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::config_ |
Definition at line 83 of file trajectory_follower_wrapper_node.hpp.
Referenced by TrajectoryFollowerWrapperNode(), autoware_info_timer_callback(), get_wheel_angle_rad_from_twist(), isControlCommandOld(), on_configure_plugin(), and parameter_update_callback().
|
private |
Definition at line 42 of file trajectory_follower_wrapper_node.hpp.
Referenced by on_configure_plugin().
|
private |
Definition at line 52 of file trajectory_follower_wrapper_node.hpp.
Referenced by ackermann_control_cb(), and generate_command().