20 namespace std_ph = std::placeholders;
23 : carma_ros2_utils::CarmaLifecycleNode(options)
41 auto error_3 = update_params<double>({
48 rcl_interfaces::msg::SetParametersResult result;
50 result.successful = !error && !error_2 && !error_3;
57 RCLCPP_INFO_STREAM(get_logger(),
"BSMGenerator trying to configure");
70 RCLCPP_INFO_STREAM(get_logger(),
"Loaded params: " <<
config_);
76 pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
"pose", 1,
78 accel_sub_ = create_subscription<automotive_platform_msgs::msg::VelocityAccelCov>(
"velocity_accel_cov", 1,
80 yaw_sub_ = create_subscription<sensor_msgs::msg::Imu>(
"imu_raw", 1,
82 gear_sub_ = create_subscription<j2735_v2x_msgs::msg::TransmissionState>(
"transmission_state", 1,
84 speed_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>(
"ekf_twist", 1,
88 brake_sub_ = create_subscription<std_msgs::msg::Float64>(
"brake_position", 1,
90 heading_sub_ = create_subscription<gps_msgs::msg::GPSFix>(
"gnss_fix_fused", 1,
96 bsm_pub_ = create_publisher<carma_v2x_msgs::msg::BSM>(
"bsm_outbound", 5);
101 worker = std::make_shared<BSMGeneratorWorker>();
104 return CallbackReturn::SUCCESS;
111 timer_ = create_timer(get_clock(),
112 std::chrono::milliseconds(bsm_generation_period_ms),
115 return CallbackReturn::SUCCESS;
120 bsm_.core_data.presence_vector = 0;
123 bsm_.core_data.size.presence_vector =
bsm_.core_data.size.presence_vector |
bsm_.core_data.size.VEHICLE_LENGTH_AVAILABLE;
124 bsm_.core_data.size.presence_vector =
bsm_.core_data.size.presence_vector |
bsm_.core_data.size.VEHICLE_WIDTH_AVAILABLE;
133 map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(msg->data.c_str());
139 bsm_.core_data.speed =
worker->getSpeedInRange(msg->twist.linear.x);
140 bsm_.core_data.presence_vector =
bsm_.core_data.presence_vector |
bsm_.core_data.SPEED_AVAILABLE;
145 bsm_.core_data.transmission.transmission_state = msg->transmission_state;
150 bsm_.core_data.angle =
worker->getSteerWheelAngleInRange(msg->data);
151 bsm_.core_data.presence_vector =
bsm_.core_data.presence_vector |
bsm_.core_data.STEER_WHEEL_ANGLE_AVAILABLE;
156 bsm_.core_data.accel_set.longitudinal =
worker->getLongAccelInRange(msg->accleration);
157 bsm_.core_data.accel_set.presence_vector =
bsm_.core_data.accel_set.presence_vector |
bsm_.core_data.accel_set.ACCELERATION_AVAILABLE;
162 bsm_.core_data.accel_set.yaw_rate =
worker->getYawRateInRange(
static_cast<float>(msg->angular_velocity.z));
163 bsm_.core_data.accel_set.presence_vector =
bsm_.core_data.accel_set.presence_vector |
bsm_.core_data.accel_set.YAWRATE_AVAILABLE;
168 bsm_.core_data.brakes.wheel_brakes.brake_applied_status =
worker->getBrakeAppliedStatus(msg->data);
175 RCLCPP_DEBUG_STREAM(get_logger(),
"Ignoring pose message as projection string has not been defined");
179 lanelet::GPSPoint coord =
map_projector_->reverse( { msg->pose.position.x, msg->pose.position.y, msg->pose.position.z } );
181 bsm_.core_data.longitude = coord.lon;
182 bsm_.core_data.latitude = coord.lat;
183 bsm_.core_data.elev = coord.ele;
184 bsm_.core_data.presence_vector =
bsm_.core_data.presence_vector |
bsm_.core_data.LONGITUDE_AVAILABLE;
185 bsm_.core_data.presence_vector =
bsm_.core_data.presence_vector |
bsm_.core_data.LATITUDE_AVAILABLE;
186 bsm_.core_data.presence_vector =
bsm_.core_data.presence_vector |
bsm_.core_data.ELEVATION_AVAILABLE;
191 bsm_.core_data.heading =
worker->getHeadingInRange(
static_cast<float>(msg->track));
192 bsm_.core_data.presence_vector =
bsm_.core_data.presence_vector |
bsm_.core_data.HEADING_AVAILABLE;
197 bsm_.header.stamp = this->now();
198 bsm_.core_data.msg_count =
worker->getNextMsgCount();
204 std::vector<uint8_t> id(4);
205 for(
int i = 0;
i <
id.size(); ++
i)
209 bsm_.core_data.id = id;
212 bsm_.core_data.sec_mark =
worker->getSecMark( this->now() );
213 bsm_.core_data.presence_vector =
bsm_.core_data.presence_vector |
bsm_.core_data.SEC_MARK_AVAILABLE;
215 bsm_.core_data.accuracy.presence_vector = 0;
221#include "rclcpp_components/register_node_macro.hpp"
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.
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.
double bsm_generation_frequency
bool bsm_id_rotation_enabled
double bsm_id_change_period