bsm_generator_node.hpp

Go to the documentation of this file.

/*
 * Copyright (C) 2019-2022 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */
 
#pragma once
 
#include <rclcpp/rclcpp.hpp>
#include <functional>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/float64.hpp>
#include <carma_v2x_msgs/msg/bsm.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <automotive_platform_msgs/msg/velocity_accel_cov.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <j2735_v2x_msgs/msg/transmission_state.hpp>
#include <std_msgs/msg/float64.hpp>
#include <lanelet2_extension/projection/local_frame_projector.h>
#include <gps_msgs/msg/gps_fix.hpp>
#include <vector>
 
#include <carma_ros2_utils/carma_lifecycle_node.hpp>
#include <tf2/LinearMath/Transform.h>
 
#include "bsm_generator/bsm_generator_worker.hpp"
#include "bsm_generator/bsm_generator_config.hpp"
 
namespace bsm_generator
{
 
  class BSMGenerator : public carma_ros2_utils::CarmaLifecycleNode
  {
 
  private:
    // Subscribers
    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_;
 
    // Publishers
    carma_ros2_utils::PubPtr<carma_v2x_msgs::msg::BSM> bsm_pub_;
 
    // Timer to run the BSM Generation task
    rclcpp::TimerBase::SharedPtr timer_;
 
    // Node configuration
    Config config_;
 
    // Worker class
    std::shared_ptr<BSMGeneratorWorker> worker;
 
    // The BSM object that all subscribers make updates to
    carma_v2x_msgs::msg::BSM bsm_;
 
    std::string georeference_ {""};
    std::shared_ptr<lanelet::projection::LocalFrameProjector> map_projector_;
 
    std::vector<uint8_t> bsm_message_id_;
 
    void initializeBSM();
 
    void poseCallback(const geometry_msgs::msg::PoseStamped::UniquePtr msg);
 
    void accelCallback(const automotive_platform_msgs::msg::VelocityAccelCov::UniquePtr msg);
 
    void yawCallback(const sensor_msgs::msg::Imu::UniquePtr msg);
 
    void gearCallback(const j2735_v2x_msgs::msg::TransmissionState::UniquePtr msg);
 
    void speedCallback(const geometry_msgs::msg::TwistStamped::UniquePtr msg);
 
    void steerWheelAngleCallback(const std_msgs::msg::Float64::UniquePtr msg);
 
    void brakeCallback(const std_msgs::msg::Float64::UniquePtr msg);
 
    void headingCallback(const gps_msgs::msg::GPSFix::UniquePtr msg);
 
    void georeferenceCallback(const std_msgs::msg::String::UniquePtr msg);
 
    void generateBSM();
 
  public:
  
    explicit BSMGenerator(const rclcpp::NodeOptions &);
 
    rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector<rclcpp::Parameter> &parameters);
 
    // Overrides
    carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &);
    carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &);
  };
 
} // namespace bsm_generator