19#include <rclcpp/rclcpp.hpp>
21#include <std_msgs/msg/string.hpp>
22#include <std_msgs/msg/float64.hpp>
23#include <carma_v2x_msgs/msg/bsm.hpp>
24#include <geometry_msgs/msg/pose_stamped.hpp>
25#include <automotive_platform_msgs/msg/velocity_accel_cov.hpp>
26#include <geometry_msgs/msg/twist_stamped.hpp>
27#include <sensor_msgs/msg/imu.hpp>
28#include <j2735_v2x_msgs/msg/transmission_state.hpp>
29#include <std_msgs/msg/float64.hpp>
30#include <lanelet2_extension/projection/local_frame_projector.h>
31#include <gps_msgs/msg/gps_fix.hpp>
34#include <carma_ros2_utils/carma_lifecycle_node.hpp>
50 carma_ros2_utils::SubPtr<geometry_msgs::msg::PoseStamped>
pose_sub_;
51 carma_ros2_utils::SubPtr<automotive_platform_msgs::msg::VelocityAccelCov>
accel_sub_;
52 carma_ros2_utils::SubPtr<sensor_msgs::msg::Imu>
yaw_sub_;
53 carma_ros2_utils::SubPtr<j2735_v2x_msgs::msg::TransmissionState>
gear_sub_;
54 carma_ros2_utils::SubPtr<geometry_msgs::msg::TwistStamped>
speed_sub_;
56 carma_ros2_utils::SubPtr<std_msgs::msg::Float64>
brake_sub_;
61 carma_ros2_utils::PubPtr<carma_v2x_msgs::msg::BSM>
bsm_pub_;
70 std::shared_ptr<BSMGeneratorWorker>
worker;
73 carma_v2x_msgs::msg::BSM
bsm_;
89 void poseCallback(
const geometry_msgs::msg::PoseStamped::UniquePtr msg);
95 void accelCallback(
const automotive_platform_msgs::msg::VelocityAccelCov::UniquePtr msg);
101 void yawCallback(
const sensor_msgs::msg::Imu::UniquePtr msg);
107 void gearCallback(
const j2735_v2x_msgs::msg::TransmissionState::UniquePtr msg);
113 void speedCallback(
const geometry_msgs::msg::TwistStamped::UniquePtr msg);
125 void brakeCallback(
const std_msgs::msg::Float64::UniquePtr msg);
The class responsible for publishing BSM messages.
carma_ros2_utils::SubPtr< std_msgs::msg::Float64 > brake_sub_
std::shared_ptr< lanelet::projection::LocalFrameProjector > map_projector_
carma_ros2_utils::SubPtr< sensor_msgs::msg::Imu > yaw_sub_
void generateBSM()
Timer callback, which publishes a BSM.
carma_ros2_utils::SubPtr< automotive_platform_msgs::msg::VelocityAccelCov > accel_sub_
rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector< rclcpp::Parameter > ¶meters)
Function callback for dynamic parameter updates.
std::vector< uint8_t > bsm_message_id_
carma_ros2_utils::SubPtr< gps_msgs::msg::GPSFix > heading_sub_
carma_ros2_utils::SubPtr< std_msgs::msg::String > georeference_sub_
carma_ros2_utils::SubPtr< j2735_v2x_msgs::msg::TransmissionState > gear_sub_
void georeferenceCallback(const std_msgs::msg::String::UniquePtr msg)
Callback for map projection string to define lat/lon -> map conversion.
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::BSM > bsm_pub_
void speedCallback(const geometry_msgs::msg::TwistStamped::UniquePtr msg)
Callback to populate BSM message with vehicle speed data.
void headingCallback(const gps_msgs::msg::GPSFix::UniquePtr msg)
Callback to populate BSM message with vehicle heading data.
std::shared_ptr< BSMGeneratorWorker > worker
void brakeCallback(const std_msgs::msg::Float64::UniquePtr msg)
Callback to populate BSM message with vehicle applied brake status.
carma_ros2_utils::SubPtr< std_msgs::msg::Float64 > steer_wheel_angle_sub_
carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &)
std::string georeference_
void poseCallback(const geometry_msgs::msg::PoseStamped::UniquePtr msg)
Callback to populate BSM message with longitude, latitude, and elevation data.
carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &)
BSMGenerator(const rclcpp::NodeOptions &)
BSMGenerator constructor.
carma_ros2_utils::SubPtr< geometry_msgs::msg::TwistStamped > speed_sub_
carma_v2x_msgs::msg::BSM bsm_
carma_ros2_utils::SubPtr< geometry_msgs::msg::PoseStamped > pose_sub_
void gearCallback(const j2735_v2x_msgs::msg::TransmissionState::UniquePtr msg)
Callback to populate BSM message with transmission state data.
void initializeBSM()
Function to fill the BSM message with initial default data.
rclcpp::TimerBase::SharedPtr timer_
void yawCallback(const sensor_msgs::msg::Imu::UniquePtr msg)
Callback to populate BSM message with yaw rate data.
void steerWheelAngleCallback(const std_msgs::msg::Float64::UniquePtr msg)
Callback to populate BSM message with vehicle steering wheel angle data.
void accelCallback(const automotive_platform_msgs::msg::VelocityAccelCov::UniquePtr msg)
Callback to populate BSM message with longitudinal acceleration data.
Stuct containing the algorithm configuration values for bsm_generator.