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.
|
The class responsible for publishing BSM messages. More...
#include <bsm_generator_node.hpp>
Public Member Functions | |
BSMGenerator (const rclcpp::NodeOptions &) | |
BSMGenerator constructor. More... | |
rcl_interfaces::msg::SetParametersResult | parameter_update_callback (const std::vector< rclcpp::Parameter > ¶meters) |
Function callback for dynamic parameter updates. More... | |
carma_ros2_utils::CallbackReturn | handle_on_configure (const rclcpp_lifecycle::State &) |
carma_ros2_utils::CallbackReturn | handle_on_activate (const rclcpp_lifecycle::State &) |
Private Member Functions | |
void | initializeBSM () |
Function to fill the BSM message with initial default data. More... | |
void | poseCallback (const geometry_msgs::msg::PoseStamped::UniquePtr msg) |
Callback to populate BSM message with longitude, latitude, and elevation data. More... | |
void | accelCallback (const automotive_platform_msgs::msg::VelocityAccelCov::UniquePtr msg) |
Callback to populate BSM message with longitudinal acceleration data. More... | |
void | yawCallback (const sensor_msgs::msg::Imu::UniquePtr msg) |
Callback to populate BSM message with yaw rate data. More... | |
void | gearCallback (const j2735_v2x_msgs::msg::TransmissionState::UniquePtr msg) |
Callback to populate BSM message with transmission state data. More... | |
void | speedCallback (const geometry_msgs::msg::TwistStamped::UniquePtr msg) |
Callback to populate BSM message with vehicle speed data. More... | |
void | steerWheelAngleCallback (const std_msgs::msg::Float64::UniquePtr msg) |
Callback to populate BSM message with vehicle steering wheel angle data. More... | |
void | brakeCallback (const std_msgs::msg::Float64::UniquePtr msg) |
Callback to populate BSM message with vehicle applied brake status. More... | |
void | headingCallback (const gps_msgs::msg::GPSFix::UniquePtr msg) |
Callback to populate BSM message with vehicle heading data. More... | |
void | georeferenceCallback (const std_msgs::msg::String::UniquePtr msg) |
Callback for map projection string to define lat/lon -> map conversion. More... | |
void | generateBSM () |
Timer callback, which publishes a BSM. More... | |
Private Attributes | |
carma_ros2_utils::SubPtr< geometry_msgs::msg::PoseStamped > | pose_sub_ |
carma_ros2_utils::SubPtr< automotive_platform_msgs::msg::VelocityAccelCov > | accel_sub_ |
carma_ros2_utils::SubPtr< sensor_msgs::msg::Imu > | yaw_sub_ |
carma_ros2_utils::SubPtr< j2735_v2x_msgs::msg::TransmissionState > | gear_sub_ |
carma_ros2_utils::SubPtr< geometry_msgs::msg::TwistStamped > | speed_sub_ |
carma_ros2_utils::SubPtr< std_msgs::msg::Float64 > | steer_wheel_angle_sub_ |
carma_ros2_utils::SubPtr< std_msgs::msg::Float64 > | brake_sub_ |
carma_ros2_utils::SubPtr< gps_msgs::msg::GPSFix > | heading_sub_ |
carma_ros2_utils::SubPtr< std_msgs::msg::String > | georeference_sub_ |
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::BSM > | bsm_pub_ |
rclcpp::TimerBase::SharedPtr | timer_ |
Config | config_ |
std::shared_ptr< BSMGeneratorWorker > | worker |
carma_v2x_msgs::msg::BSM | bsm_ |
std::string | georeference_ {""} |
std::shared_ptr< lanelet::projection::LocalFrameProjector > | map_projector_ |
std::vector< uint8_t > | bsm_message_id_ |
The class responsible for publishing BSM messages.
Definition at line 45 of file bsm_generator_node.hpp.
|
explicit |
BSMGenerator constructor.
Definition at line 22 of file bsm_generator_node.cpp.
References bsm_generator::Config::bsm_generation_frequency, bsm_generator::Config::bsm_id_change_period, bsm_generator::Config::bsm_id_rotation_enabled, bsm_generator::Config::bsm_message_id, config_, bsm_generator::Config::vehicle_length, and bsm_generator::Config::vehicle_width.
|
private |
Callback to populate BSM message with longitudinal acceleration data.
msg | Latest acceleration message |
Definition at line 154 of file bsm_generator_node.cpp.
Referenced by handle_on_configure().
|
private |
Callback to populate BSM message with vehicle applied brake status.
msg | Latest brake status message |
Definition at line 166 of file bsm_generator_node.cpp.
Referenced by handle_on_configure().
|
private |
Callback to populate BSM message with transmission state data.
msg | Latest transmissio state message |
Definition at line 143 of file bsm_generator_node.cpp.
References bsm_.
Referenced by handle_on_configure().
|
private |
Timer callback, which publishes a BSM.
Definition at line 195 of file bsm_generator_node.cpp.
References bsm_, bsm_generator::Config::bsm_id_change_period, bsm_generator::Config::bsm_id_rotation_enabled, bsm_generator::Config::bsm_message_id, bsm_pub_, config_, process_bag::i, and worker.
Referenced by handle_on_activate().
|
private |
Callback for map projection string to define lat/lon -> map conversion.
msg | The proj string defining the projection. |
Definition at line 127 of file bsm_generator_node.cpp.
References georeference_, and map_projector_.
Referenced by handle_on_configure().
carma_ros2_utils::CallbackReturn bsm_generator::BSMGenerator::handle_on_activate | ( | const rclcpp_lifecycle::State & | prev_state | ) |
Definition at line 107 of file bsm_generator_node.cpp.
References bsm_generator::Config::bsm_generation_frequency, config_, generateBSM(), and timer_.
carma_ros2_utils::CallbackReturn bsm_generator::BSMGenerator::handle_on_configure | ( | const rclcpp_lifecycle::State & | ) |
Definition at line 55 of file bsm_generator_node.cpp.
References accel_sub_, accelCallback(), brake_sub_, brakeCallback(), bsm_generator::Config::bsm_generation_frequency, bsm_generator::Config::bsm_id_change_period, bsm_generator::Config::bsm_id_rotation_enabled, bsm_generator::Config::bsm_message_id, bsm_pub_, config_, gear_sub_, gearCallback(), georeference_sub_, georeferenceCallback(), heading_sub_, headingCallback(), initializeBSM(), parameter_update_callback(), pose_sub_, poseCallback(), speed_sub_, speedCallback(), steer_wheel_angle_sub_, steerWheelAngleCallback(), bsm_generator::Config::vehicle_length, bsm_generator::Config::vehicle_width, worker, yaw_sub_, and yawCallback().
|
private |
Callback to populate BSM message with vehicle heading data.
msg | Latest GNSS message |
Definition at line 189 of file bsm_generator_node.cpp.
Referenced by handle_on_configure().
|
private |
Function to fill the BSM message with initial default data.
Definition at line 118 of file bsm_generator_node.cpp.
References bsm_, config_, bsm_generator::Config::vehicle_length, and bsm_generator::Config::vehicle_width.
Referenced by handle_on_configure().
rcl_interfaces::msg::SetParametersResult bsm_generator::BSMGenerator::parameter_update_callback | ( | const std::vector< rclcpp::Parameter > & | parameters | ) |
Function callback for dynamic parameter updates.
Definition at line 37 of file bsm_generator_node.cpp.
References bsm_generator::Config::bsm_generation_frequency, bsm_generator::Config::bsm_id_change_period, bsm_generator::Config::bsm_id_rotation_enabled, bsm_generator::Config::bsm_message_id, config_, bsm_generator::Config::vehicle_length, and bsm_generator::Config::vehicle_width.
Referenced by handle_on_configure().
|
private |
Callback to populate BSM message with longitude, latitude, and elevation data.
msg | Latest pose message |
Definition at line 171 of file bsm_generator_node.cpp.
References bsm_, and map_projector_.
Referenced by handle_on_configure().
|
private |
Callback to populate BSM message with vehicle speed data.
msg | Latest speed message |
Definition at line 137 of file bsm_generator_node.cpp.
Referenced by handle_on_configure().
|
private |
Callback to populate BSM message with vehicle steering wheel angle data.
msg | Latest steering wheel angle message |
Definition at line 148 of file bsm_generator_node.cpp.
Referenced by handle_on_configure().
|
private |
Callback to populate BSM message with yaw rate data.
msg | Latest IMU message |
Definition at line 160 of file bsm_generator_node.cpp.
Referenced by handle_on_configure().
|
private |
Definition at line 51 of file bsm_generator_node.hpp.
Referenced by handle_on_configure().
|
private |
Definition at line 56 of file bsm_generator_node.hpp.
Referenced by handle_on_configure().
|
private |
Definition at line 73 of file bsm_generator_node.hpp.
Referenced by accelCallback(), brakeCallback(), gearCallback(), generateBSM(), headingCallback(), initializeBSM(), poseCallback(), speedCallback(), steerWheelAngleCallback(), and yawCallback().
|
private |
Definition at line 78 of file bsm_generator_node.hpp.
|
private |
Definition at line 61 of file bsm_generator_node.hpp.
Referenced by generateBSM(), and handle_on_configure().
|
private |
Definition at line 67 of file bsm_generator_node.hpp.
Referenced by BSMGenerator(), generateBSM(), handle_on_activate(), handle_on_configure(), initializeBSM(), and parameter_update_callback().
|
private |
Definition at line 53 of file bsm_generator_node.hpp.
Referenced by handle_on_configure().
|
private |
Definition at line 75 of file bsm_generator_node.hpp.
Referenced by georeferenceCallback().
|
private |
Definition at line 58 of file bsm_generator_node.hpp.
Referenced by handle_on_configure().
|
private |
Definition at line 57 of file bsm_generator_node.hpp.
Referenced by handle_on_configure().
|
private |
Definition at line 76 of file bsm_generator_node.hpp.
Referenced by georeferenceCallback(), and poseCallback().
|
private |
Definition at line 50 of file bsm_generator_node.hpp.
Referenced by handle_on_configure().
|
private |
Definition at line 54 of file bsm_generator_node.hpp.
Referenced by handle_on_configure().
|
private |
Definition at line 55 of file bsm_generator_node.hpp.
Referenced by handle_on_configure().
|
private |
Definition at line 64 of file bsm_generator_node.hpp.
Referenced by handle_on_activate().
|
private |
Definition at line 70 of file bsm_generator_node.hpp.
Referenced by accelCallback(), brakeCallback(), generateBSM(), handle_on_configure(), headingCallback(), speedCallback(), steerWheelAngleCallback(), and yawCallback().
|
private |
Definition at line 52 of file bsm_generator_node.hpp.
Referenced by handle_on_configure().