Carma-platform v4.11.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.
trajectory_follower_wrapper::TrajectoryFollowerWrapperNode Class Reference

#include <trajectory_follower_wrapper_node.hpp>

Inheritance diagram for trajectory_follower_wrapper::TrajectoryFollowerWrapperNode:
Inheritance graph
Collaboration diagram for trajectory_follower_wrapper::TrajectoryFollowerWrapperNode:
Collaboration graph

Public Member Functions

 TrajectoryFollowerWrapperNode (const rclcpp::NodeOptions &options)
 Node constructor. More...
 
rcl_interfaces::msg::SetParametersResult parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
 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::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)
 

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

Detailed Description

Definition at line 37 of file trajectory_follower_wrapper_node.hpp.

Constructor & Destructor Documentation

◆ TrajectoryFollowerWrapperNode()

trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::TrajectoryFollowerWrapperNode ( const rclcpp::NodeOptions &  options)
explicit

Node constructor.

Definition at line 23 of file trajectory_follower_wrapper_node.cpp.

25 {
26 basic_autonomy::set_logger(get_logger().get_child("basic_autonomy"));
27 // Create initial config
28 config_ = TrajectoryFollowerWrapperConfig();
29 config_.vehicle_response_lag = declare_parameter<double>("vehicle_response_lag", config_.vehicle_response_lag);
30 config_.vehicle_wheel_base = declare_parameter<double>("vehicle_wheel_base", config_.vehicle_wheel_base);
31 config_.incoming_cmd_time_threshold = declare_parameter<double>("incoming_cmd_time_threshold", config_.incoming_cmd_time_threshold);
32
33 }
ControlPlugin base class which can be extended by user provided plugins which wish to implement the C...
void set_logger(rclcpp::Logger logger)
Replace the module-level logger used by all basic_autonomy functions.
Definition: log.cpp:33
rclcpp::Logger get_logger()
Return the module-level logger used by all basic_autonomy functions.
Definition: log.cpp:32

References config_, basic_autonomy::get_logger(), trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::incoming_cmd_time_threshold, basic_autonomy::set_logger(), trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::vehicle_response_lag, and trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::vehicle_wheel_base.

Here is the call graph for this function:

Member Function Documentation

◆ ackermann_control_cb()

void trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::ackermann_control_cb ( const autoware_auto_msgs::msg::AckermannControlCommand::SharedPtr  msg)

autoware's control subscription callback

Definition at line 121 of file trajectory_follower_wrapper_node.cpp.

122 {
123 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("trajectory_follower_wrapper"), "In ackermann control callback");
125 }
std::optional< autoware_auto_msgs::msg::AckermannControlCommand > received_ctrl_command_

References basic_autonomy::get_logger(), and received_ctrl_command_.

Referenced by on_configure_plugin().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ autoware_info_timer_callback()

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 90 of file trajectory_follower_wrapper_node.cpp.

91 {
92 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("trajectory_follower_wrapper"), "In autoware info timer callback");
93
95 {
96 // generate and publish autoware kinematic state
97 auto autoware_state = convert_state(current_pose_.get(), current_twist_.get());
98 autoware_state_pub_->publish(autoware_state);
99
100 // generate and publish autoware trajectory
101 current_trajectory_.get().header.frame_id = autoware_state.header.frame_id;
103
104 autoware_traj_pub_->publish(autoware_traj_plan);
105
106 }
107 }
boost::optional< geometry_msgs::msg::TwistStamped > current_twist_
The most recent velocity message received by this node.
boost::optional< carma_planning_msgs::msg::TrajectoryPlan > current_trajectory_
The most recent trajectory received by this plugin.
boost::optional< geometry_msgs::msg::PoseStamped > current_pose_
The most recent pose message received by this node.
carma_ros2_utils::PubPtr< autoware_auto_msgs::msg::Trajectory > autoware_traj_pub_
carma_ros2_utils::PubPtr< autoware_auto_msgs::msg::VehicleKinematicState > autoware_state_pub_
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
autoware_auto_msgs::msg::Trajectory process_trajectory_plan(const carma_planning_msgs::msg::TrajectoryPlan &tp, double vehicle_response_lag)
Given a carma type of trajectory_plan, generate autoware type of trajectory accounting for speed_lag ...

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::get_logger(), basic_autonomy::waypoint_generation::process_trajectory_plan(), and trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::vehicle_response_lag.

Referenced by on_configure_plugin().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ convert_cmd()

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 149 of file trajectory_follower_wrapper_node.cpp.

150 {
151 autoware_msgs::msg::ControlCommandStamped return_cmd;
152 return_cmd.header.stamp = cmd.stamp;
153
154 return_cmd.cmd.linear_acceleration = cmd.longitudinal.acceleration;
155 return_cmd.cmd.linear_velocity = cmd.longitudinal.speed;
156 return_cmd.cmd.steering_angle = cmd.lateral.steering_tire_angle;
157
158 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("trajectory_follower_wrapper"), "generated command cmd.stamp: " << std::to_string(rclcpp::Time(cmd.stamp).seconds()));
159 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("trajectory_follower_wrapper"), "generated command cmd.longitudinal.acceleration: " << cmd.longitudinal.acceleration);
160 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("trajectory_follower_wrapper"), "generated command cmd.longitudinal.speed: " << cmd.longitudinal.speed);
161 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("trajectory_follower_wrapper"), "generated command cmd.lateral.steering_tire_angle: " << cmd.lateral.steering_tire_angle);
162 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("trajectory_follower_wrapper"), "generated command cmd.lateral.steering_tire_rotation_rate: " << cmd.lateral.steering_tire_rotation_rate);
163
164 return return_cmd;
165 }
auto to_string(const UtmZone &zone) -> std::string
Definition: utm_zone.cpp:21

References basic_autonomy::get_logger(), and carma_cooperative_perception::to_string().

Referenced by generate_command().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ convert_state()

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 128 of file trajectory_follower_wrapper_node.cpp.

129 {
130 autoware_auto_msgs::msg::VehicleKinematicState state;
131 state.header = pose.header;
132 state.header.frame_id = "map";
133 state.state.x = pose.pose.position.x;
134 state.state.y = pose.pose.position.y;
135 state.state.z = pose.pose.position.z;
136 state.state.heading.real = pose.pose.orientation.w;
137 state.state.heading.imag = pose.pose.orientation.z;
138
139 state.state.front_wheel_angle_rad = get_wheel_angle_rad_from_twist(twist);
140 state.state.longitudinal_velocity_mps = twist.twist.linear.x;
141 state.state.lateral_velocity_mps = twist.twist.linear.y;
142 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("trajectory_follower_wrapper"), "front_wheel_angle_rad: " << state.state.front_wheel_angle_rad);
143
144
145
146 return state;
147 }
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

References basic_autonomy::get_logger(), and get_wheel_angle_rad_from_twist().

Referenced by autoware_info_timer_callback().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ FRIEND_TEST() [1/2]

trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::FRIEND_TEST ( TesttrajectoryFollowerWrapper  ,
TestControlCallback   
)

◆ FRIEND_TEST() [2/2]

trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::FRIEND_TEST ( TesttrajectoryFollowerWrapper  ,
TestThreshold   
)

◆ generate_command()

autoware_msgs::msg::ControlCommandStamped trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::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 167 of file trajectory_follower_wrapper_node.cpp.

168 {
169 // process and save the trajectory
170 autoware_msgs::msg::ControlCommandStamped converted_cmd;
171
173 {
174 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("trajectory_follower_wrapper"), "Insufficient data, empty control command generated");
175 return converted_cmd;
176 }
177
178
180 {
181 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("trajectory_follower_wrapper"), "Control Command is old, empty control command generated");
182 return converted_cmd;
183 }
184
185 converted_cmd = convert_cmd(received_ctrl_command_.value());
186
187 return converted_cmd;
188 }
bool isControlCommandOld(const autoware_auto_msgs::msg::AckermannControlCommand &cmd) const
Check to see if the received control command recent or old.
autoware_msgs::msg::ControlCommandStamped convert_cmd(const autoware_auto_msgs::msg::AckermannControlCommand &cmd) const
convert autoware Ackermann control command to autoware stamped control command

References convert_cmd(), carma_guidance_plugins::ControlPlugin::current_pose_, carma_guidance_plugins::ControlPlugin::current_trajectory_, carma_guidance_plugins::ControlPlugin::current_twist_, basic_autonomy::get_logger(), isControlCommandOld(), and received_ctrl_command_.

Here is the call graph for this function:

◆ get_availability()

bool trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::get_availability ( )
overridevirtual

Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes.

Returns
This method should return true if the plugin's current understanding of the world means it would be capable of planning or executing its capability.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 202 of file trajectory_follower_wrapper_node.cpp.

203 {
204 return true;
205 }

◆ get_version_id()

std::string trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::get_version_id ( )
overridevirtual

Returns the version id of this plugin.

Returns
The version id represented as a string

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 207 of file trajectory_follower_wrapper_node.cpp.

208 {
209 return "1.0";
210 }

◆ get_wheel_angle_rad_from_twist()

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 109 of file trajectory_follower_wrapper_node.cpp.

110 {
111
112 if (std::abs(twist.twist.linear.x) < EPSILON )
113 {
114 return 0.0;
115 }
116
117 double steering_angle = std::atan2(twist.twist.angular.z * config_.vehicle_wheel_base, twist.twist.linear.x);
118 return steering_angle;
119 }
#define EPSILON

References config_, EPSILON, and trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::vehicle_wheel_base.

Referenced by convert_state().

Here is the caller graph for this function:

◆ isControlCommandOld()

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 190 of file trajectory_follower_wrapper_node.cpp.

191 {
192 double difference = std::abs(this->now().seconds() - rclcpp::Time(cmd.stamp).seconds());
193
194 if (difference >= config_.incoming_cmd_time_threshold)
195 {
196 return true;
197 }
198
199 return false;
200 }

References config_, and trajectory_follower_wrapper::TrajectoryFollowerWrapperConfig::incoming_cmd_time_threshold.

Referenced by generate_command().

Here is the caller graph for this function:

◆ on_configure_plugin()

carma_ros2_utils::CallbackReturn trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::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 50 of file trajectory_follower_wrapper_node.cpp.

51 {
52 // Reset config
53 config_ = TrajectoryFollowerWrapperConfig();
54
55 // Load parameters
56 get_parameter<double>("vehicle_response_lag", config_.vehicle_response_lag);
57 get_parameter<double>("vehicle_wheel_base", config_.vehicle_wheel_base);
58 get_parameter<double>("incoming_cmd_time_threshold", config_.incoming_cmd_time_threshold);
59
60 RCLCPP_INFO_STREAM(rclcpp::get_logger("trajectory_follower_wrapper"), "Loaded Params: " << config_);
61 // Register runtime parameter update callback
62 add_on_set_parameters_callback(std::bind(&TrajectoryFollowerWrapperNode::parameter_update_callback, this, std_ph::_1));
63
64 // NOTE: Currently, intra-process comms must be disabled for the following subscriber that is transient_local: https://github.com/ros2/rclcpp/issues/1753
65
66 rclcpp::SubscriptionOptions intra_proc_disabled;
67 intra_proc_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
68
69 auto sub_qos_transient_local = rclcpp::QoS(rclcpp::KeepAll()); // A subscriber with this QoS will store all messages that it has sent on the topic
70 sub_qos_transient_local.transient_local();
71 // Setup subscriber
72 control_cmd_sub_ = create_subscription<autoware_auto_msgs::msg::AckermannControlCommand>("trajectory_follower/control_cmd", sub_qos_transient_local,
73 std::bind(&TrajectoryFollowerWrapperNode::ackermann_control_cb, this, std::placeholders::_1),intra_proc_disabled);
74
75 // Setup publishers
76 autoware_traj_pub_ = create_publisher<autoware_auto_msgs::msg::Trajectory>("trajectory_follower/reference_trajectory", 10);
77 autoware_state_pub_ = create_publisher<autoware_auto_msgs::msg::VehicleKinematicState>("trajectory_follower/current_kinematic_state", 10);
78
79 // Setup timers to publish autoware compatible info (trajectory and state)
80 autoware_info_timer_ = create_timer(
81 get_clock(),
82 std::chrono::milliseconds(33), // Spin at 30 Hz per plugin API
84
85 // Return success if everthing initialized successfully
86 return CallbackReturn::SUCCESS;
87 }
carma_ros2_utils::SubPtr< autoware_auto_msgs::msg::AckermannControlCommand > control_cmd_sub_
void ackermann_control_cb(const autoware_auto_msgs::msg::AckermannControlCommand::SharedPtr msg)
autoware's control subscription callback
void autoware_info_timer_callback()
Timer callback to spin at 30 hz and frequently publish autoware kinematic state and trajectory.
rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector< rclcpp::Parameter > &parameters)
Callback for dynamic parameter updates.

References ackermann_control_cb(), autoware_info_timer_, autoware_info_timer_callback(), autoware_state_pub_, autoware_traj_pub_, config_, control_cmd_sub_, basic_autonomy::get_logger(), 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.

Here is the call graph for this function:

◆ parameter_update_callback()

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 35 of file trajectory_follower_wrapper_node.cpp.

36 {
37 auto error_double = update_params<double>({
38 {"vehicle_response_lag", config_.vehicle_response_lag},
39 {"vehicle_wheel_base", config_.vehicle_wheel_base},
40 {"incoming_cmd_time_threshold", config_.incoming_cmd_time_threshold}
41 }, parameters);
42
43 rcl_interfaces::msg::SetParametersResult result;
44
45 result.successful = !error_double;
46
47 return result;
48 }

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

Here is the caller graph for this function:

Member Data Documentation

◆ autoware_info_timer_

rclcpp::TimerBase::SharedPtr trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::autoware_info_timer_
private

Definition at line 49 of file trajectory_follower_wrapper_node.hpp.

Referenced by on_configure_plugin().

◆ autoware_state_pub_

carma_ros2_utils::PubPtr<autoware_auto_msgs::msg::VehicleKinematicState> trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::autoware_state_pub_
private

◆ autoware_traj_pub_

carma_ros2_utils::PubPtr<autoware_auto_msgs::msg::Trajectory> trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::autoware_traj_pub_
private

◆ config_

◆ control_cmd_sub_

carma_ros2_utils::SubPtr<autoware_auto_msgs::msg::AckermannControlCommand> trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::control_cmd_sub_
private

Definition at line 42 of file trajectory_follower_wrapper_node.hpp.

Referenced by on_configure_plugin().

◆ received_ctrl_command_

std::optional<autoware_auto_msgs::msg::AckermannControlCommand> trajectory_follower_wrapper::TrajectoryFollowerWrapperNode::received_ctrl_command_
private

Definition at line 52 of file trajectory_follower_wrapper_node.hpp.

Referenced by ackermann_control_cb(), and generate_command().


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