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.
bsm_generator_node.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2019-2022 LEIDOS.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
5 * use this file except in compliance with the License. You may obtain a copy of
6 * the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
12 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
13 * License for the specific language governing permissions and limitations under
14 * the License.
15 */
17
18namespace bsm_generator
19{
20 namespace std_ph = std::placeholders;
21
22 BSMGenerator::BSMGenerator(const rclcpp::NodeOptions &options)
23 : carma_ros2_utils::CarmaLifecycleNode(options)
24 {
25 // Create initial config
26 config_ = Config();
27
28 // Declare parameters
29 config_.bsm_generation_frequency = declare_parameter<double>("bsm_generation_frequency", config_.bsm_generation_frequency);
30 config_.bsm_id_change_period = declare_parameter<double>("bsm_id_change_period", config_.bsm_id_change_period);
31 config_.bsm_id_rotation_enabled = declare_parameter<bool>("bsm_id_rotation_enabled", config_.bsm_id_rotation_enabled);
32 config_.bsm_message_id = declare_parameter<int>("bsm_message_id", config_.bsm_message_id);
33 config_.vehicle_length = declare_parameter<double>("vehicle_length", config_.vehicle_length);
34 config_.vehicle_width = declare_parameter<double>("vehicle_width", config_.vehicle_width);
35 }
36
37 rcl_interfaces::msg::SetParametersResult BSMGenerator::parameter_update_callback(const std::vector<rclcpp::Parameter> &parameters)
38 {
39 auto error = update_params<bool>({{"bsm_id_rotation_enabled", config_.bsm_id_rotation_enabled}}, parameters);
40 auto error_2 = update_params<int>({{"bsm_message_id", config_.bsm_message_id}}, parameters);
41 auto error_3 = update_params<double>({
42 {"bsm_generation_frequency", config_.bsm_generation_frequency},
43 {"bsm_id_change_period", config_.bsm_id_change_period},
44 {"vehicle_length", config_.vehicle_length},
45 {"vehicle_width", config_.vehicle_width}
46 }, parameters);
47
48 rcl_interfaces::msg::SetParametersResult result;
49
50 result.successful = !error && !error_2 && !error_3;
51
52 return result;
53 }
54
55 carma_ros2_utils::CallbackReturn BSMGenerator::handle_on_configure(const rclcpp_lifecycle::State &)
56 {
57 RCLCPP_INFO_STREAM(get_logger(), "BSMGenerator trying to configure");
58
59 // Reset config
60 config_ = Config();
61
62 // Load parameters
63 get_parameter<double>("bsm_generation_frequency", config_.bsm_generation_frequency);
64 get_parameter<double>("bsm_id_change_period", config_.bsm_id_change_period);
65 get_parameter<bool>("bsm_id_rotation_enabled", config_.bsm_id_rotation_enabled);
66 get_parameter<int>("bsm_message_id", config_.bsm_message_id);
67 get_parameter<double>("vehicle_length", config_.vehicle_length);
68 get_parameter<double>("vehicle_width", config_.vehicle_width);
69
70 RCLCPP_INFO_STREAM(get_logger(), "Loaded params: " << config_);
71
72 // Register runtime parameter update callback
73 add_on_set_parameters_callback(std::bind(&BSMGenerator::parameter_update_callback, this, std_ph::_1));
74
75 // Setup subscribers
76 pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>("pose", 1,
77 std::bind(&BSMGenerator::poseCallback, this, std_ph::_1));
78 accel_sub_ = create_subscription<automotive_platform_msgs::msg::VelocityAccelCov>("velocity_accel_cov", 1,
79 std::bind(&BSMGenerator::accelCallback, this, std_ph::_1));
80 yaw_sub_ = create_subscription<sensor_msgs::msg::Imu>("imu_raw", 1,
81 std::bind(&BSMGenerator::yawCallback, this, std_ph::_1));
82 gear_sub_ = create_subscription<j2735_v2x_msgs::msg::TransmissionState>("transmission_state", 1,
83 std::bind(&BSMGenerator::gearCallback, this, std_ph::_1));
84 speed_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>("ekf_twist", 1,
85 std::bind(&BSMGenerator::speedCallback, this, std_ph::_1));
86 steer_wheel_angle_sub_ = create_subscription<std_msgs::msg::Float64>("steering_wheel_angle", 1,
87 std::bind(&BSMGenerator::steerWheelAngleCallback, this, std_ph::_1));
88 brake_sub_ = create_subscription<std_msgs::msg::Float64>("brake_position", 1,
89 std::bind(&BSMGenerator::brakeCallback, this, std_ph::_1));
90 heading_sub_ = create_subscription<gps_msgs::msg::GPSFix>("gnss_fix_fused", 1,
91 std::bind(&BSMGenerator::headingCallback, this, std_ph::_1));
92 georeference_sub_ = create_subscription<std_msgs::msg::String>("georeference", 1,
93 std::bind(&BSMGenerator::georeferenceCallback, this, std_ph::_1));
94
95 // Setup publishers
96 bsm_pub_ = create_publisher<carma_v2x_msgs::msg::BSM>("bsm_outbound", 5);
97
98 // Initialize the generated BSM message
100
101 worker = std::make_shared<BSMGeneratorWorker>();
102
103 // Return success if everthing initialized successfully
104 return CallbackReturn::SUCCESS;
105 }
106
107 carma_ros2_utils::CallbackReturn BSMGenerator::handle_on_activate(const rclcpp_lifecycle::State &prev_state)
108 {
109 // Timer setup for generating a BSM
110 int bsm_generation_period_ms = (1 / config_.bsm_generation_frequency) * 1000; // Conversion from frequency (Hz) to milliseconds time period
111 timer_ = create_timer(get_clock(),
112 std::chrono::milliseconds(bsm_generation_period_ms),
113 std::bind(&BSMGenerator::generateBSM, this));
114
115 return CallbackReturn::SUCCESS;
116 }
117
119 {
120 bsm_.core_data.presence_vector = 0;
121 bsm_.core_data.size.vehicle_width = config_.vehicle_width;
122 bsm_.core_data.size.vehicle_length = config_.vehicle_length;
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;
125 }
126
127 void BSMGenerator::georeferenceCallback(const std_msgs::msg::String::UniquePtr msg)
128 {
129 // Build projector from proj string
130 if (georeference_ != msg->data)
131 {
132 georeference_ = msg->data;
133 map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(msg->data.c_str());
134 }
135 }
136
137 void BSMGenerator::speedCallback(const geometry_msgs::msg::TwistStamped::UniquePtr msg)
138 {
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;
141 }
142
143 void BSMGenerator::gearCallback(const j2735_v2x_msgs::msg::TransmissionState::UniquePtr msg)
144 {
145 bsm_.core_data.transmission.transmission_state = msg->transmission_state;
146 }
147
148 void BSMGenerator::steerWheelAngleCallback(const std_msgs::msg::Float64::UniquePtr msg)
149 {
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;
152 }
153
154 void BSMGenerator::accelCallback(const automotive_platform_msgs::msg::VelocityAccelCov::UniquePtr msg)
155 {
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;
158 }
159
160 void BSMGenerator::yawCallback(const sensor_msgs::msg::Imu::UniquePtr msg)
161 {
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;
164 }
165
166 void BSMGenerator::brakeCallback(const std_msgs::msg::Float64::UniquePtr msg)
167 {
168 bsm_.core_data.brakes.wheel_brakes.brake_applied_status = worker->getBrakeAppliedStatus(msg->data);
169 }
170
171 void BSMGenerator::poseCallback(const geometry_msgs::msg::PoseStamped::UniquePtr msg)
172 {
173 // Use pose message as an indicator of new location updates
174 if (!map_projector_) {
175 RCLCPP_DEBUG_STREAM(get_logger(), "Ignoring pose message as projection string has not been defined");
176 return;
177 }
178
179 lanelet::GPSPoint coord = map_projector_->reverse( { msg->pose.position.x, msg->pose.position.y, msg->pose.position.z } );
180
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;
187 }
188
189 void BSMGenerator::headingCallback(const gps_msgs::msg::GPSFix::UniquePtr msg)
190 {
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;
193 }
194
196 {
197 bsm_.header.stamp = this->now();
198 bsm_.core_data.msg_count = worker->getNextMsgCount();
199
201 bsm_.core_data.id = worker->getMsgId( this->now(), config_.bsm_id_change_period);
202 else
203 {
204 std::vector<uint8_t> id(4);
205 for(int i = 0; i < id.size(); ++i)
206 {
207 id[i] = config_.bsm_message_id >> (8 * i);
208 }
209 bsm_.core_data.id = id;
210 }
211
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;
214 // currently the accuracy is not available because ndt_matching does not provide accuracy measurement
215 bsm_.core_data.accuracy.presence_vector = 0;
216 bsm_pub_->publish(bsm_);
217 }
218
219} // namespace bsm_generator
220
221#include "rclcpp_components/register_node_macro.hpp"
222
223// Register the component with class_loader
224RCLCPP_COMPONENTS_REGISTER_NODE(bsm_generator::BSMGenerator)
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 > &parameters)
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 &)
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.