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.hpp
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 */
16
17#pragma once
18
19#include <rclcpp/rclcpp.hpp>
20#include <functional>
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>
32#include <vector>
33
34#include <carma_ros2_utils/carma_lifecycle_node.hpp>
37
38namespace bsm_generator
39{
40
45 class BSMGenerator : public carma_ros2_utils::CarmaLifecycleNode
46 {
47
48 private:
49 // Subscribers
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_;
55 carma_ros2_utils::SubPtr<std_msgs::msg::Float64> steer_wheel_angle_sub_;
56 carma_ros2_utils::SubPtr<std_msgs::msg::Float64> brake_sub_;
57 carma_ros2_utils::SubPtr<gps_msgs::msg::GPSFix> heading_sub_;
58 carma_ros2_utils::SubPtr<std_msgs::msg::String> georeference_sub_;
59
60 // Publishers
61 carma_ros2_utils::PubPtr<carma_v2x_msgs::msg::BSM> bsm_pub_;
62
63 // Timer to run the BSM Generation task
64 rclcpp::TimerBase::SharedPtr timer_;
65
66 // Node configuration
68
69 // Worker class
70 std::shared_ptr<BSMGeneratorWorker> worker;
71
72 // The BSM object that all subscribers make updates to
73 carma_v2x_msgs::msg::BSM bsm_;
74
75 std::string georeference_ {""};
76 std::shared_ptr<lanelet::projection::LocalFrameProjector> map_projector_;
77
78 std::vector<uint8_t> bsm_message_id_;
79
83 void initializeBSM();
84
89 void poseCallback(const geometry_msgs::msg::PoseStamped::UniquePtr msg);
90
95 void accelCallback(const automotive_platform_msgs::msg::VelocityAccelCov::UniquePtr msg);
96
101 void yawCallback(const sensor_msgs::msg::Imu::UniquePtr msg);
102
107 void gearCallback(const j2735_v2x_msgs::msg::TransmissionState::UniquePtr msg);
108
113 void speedCallback(const geometry_msgs::msg::TwistStamped::UniquePtr msg);
114
119 void steerWheelAngleCallback(const std_msgs::msg::Float64::UniquePtr msg);
120
125 void brakeCallback(const std_msgs::msg::Float64::UniquePtr msg);
126
131 void headingCallback(const gps_msgs::msg::GPSFix::UniquePtr msg);
132
137 void georeferenceCallback(const std_msgs::msg::String::UniquePtr msg);
138
142 void generateBSM();
143
144 public:
145
149 explicit BSMGenerator(const rclcpp::NodeOptions &);
150
154 rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector<rclcpp::Parameter> &parameters);
155
157 // Overrides
159 carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &);
160 carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &);
161 };
162
163} // namespace bsm_generator
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.
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 &)
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.