Add CAMApplication prototype

- Mandatory parameters filled through the use of 3 ros topics
- Sending ITS packets with CAMs

Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>

CMakeLists.txt

@@ -35,6 +35,7 @@ ament_auto_add_library(autoware_v2x SHARED
   src/v2x_app.cpp
   src/application.cpp
   src/cpm_application.cpp
+  src/cam_application.cpp
   src/ethernet_device.cpp
   src/link_layer.cpp
   src/raw_socket_link.cpp

config/autoware_v2x.param.yaml

@@ -1,4 +1,4 @@
 /**:
   ros__parameters:
-    network_interface: "wlp4s0"
+    network_interface: "enx5414a7c7d3af"
     is_sender: true

include/autoware_v2x/cam_application.hpp

@@ -0,0 +1,148 @@
+#ifndef CAM_APPLICATION_HPP_EUIC2VFR
+#define CAM_APPLICATION_HPP_EUIC2VFR
+
+#include "autoware_v2x/application.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include <boost/asio/io_service.hpp>
+#include <boost/asio/steady_timer.hpp>
+#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp"
+#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp"
+#include "autoware_v2x/positioning.hpp"
+#include <vanetza/asn1/cam.hpp>
+
+namespace v2x
+{
+class V2XNode;
+class CamApplication : public Application
+{
+public:
+  CamApplication(V2XNode *node, vanetza::Runtime &, bool is_sender);
+  PortType port() override;
+  //std::string uuidToHexString(const unique_identifier_msgs::msg::UUID&);
+  void indicate(const DataIndication &, UpPacketPtr) override;
+  void set_interval(vanetza::Clock::duration);
+  //void setAllObjectsOfPersonsAnimalsToSend(const autoware_auto_control_msgs::msg::LongitudinalCommand::ConstSharedPtr);
+  void getVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions);
+  void updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr);
+  void updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr);
+  void updateMGRS(double *, double *);
+  void updateRP(double *, double *, double *);
+  void updateGenerationTime(int *, long *);
+  void updateHeading(double *);
+  //void printObjectsList(int);
+  void send();
+
+  /** Creates the database schema.
+         *  Creates tables for cam_sent, cam_received
+   */
+  //void createTables();
+
+  /** Inserts CAM into the database.
+         *  Constructs and executes queries for inserting the specified CAM data.
+         *  @param cam The CAM to be inserted 
+         *  @param table_name The table to insert the CAM into (cam_sent or cam_received)
+   */
+  //void insertCamToCamTable(vanetza::asn1::Cam, char*);
+
+  struct Object {
+    std::string uuid;
+    int vehicleID;
+    vanetza::Clock::time_point timestamp;
+    rclcpp::Time timestamp_ros;
+    
+    // BasicContainer
+    long stationType;
+    long latitude;                            // Obtained from tfMessage (same value as CPMs)
+    long longitude;                           // Obtained from tfMessage (same value as CPMs)
+    long semiMajorConfidence;                 // Confidence (?)
+    long semiMinorConfidence;                 // Confidence (?)
+    long semiMajorOrientation;                // Confidence (?)
+    long altitude;                            // Obtained from tfMessage (same value as CPMs)
+    long altitudeConfidence;                  // Confidence (?)
+
+    // BasicVehicleContainerHighFrequency
+    long heading;                             // Obtained from tfMessage (same value as CPMs)
+    long headingConfidence;                   // Confidence (?)
+    long speed;                               // Obtained from velocityReport after calculation
+    long speedConfidence;                     // Confidence (?)
+    long driveDirection;                      // Obtained from gearReport
+    long vehicleLength;
+    long vehicleLengthConfidence;             // Confidence (?)
+    long vehicleWidth;
+    long longitudinalAcceleration;            // Obtained from velocityReport after calculation over time
+    long longitudinalAccelerationConfidence;  // Confidence (?)
+    long curvature;                           // Obtained from velocityReport after calculation
+    long curvatureConfidence;                 // Confidence (?)
+    long curvatureCalculationMode;            // Manually set
+    long yawRate;                             // Obtained from velocityReport
+    long yawRateConfidence;                   // Confidence (?)
+
+    vanetza::PositionFix position;
+    int timeOfMeasurement;
+    bool to_send;
+    int to_send_trigger;
+  };
+  
+private:
+  void schedule_timer();
+  void on_timer(vanetza::Clock::time_point);
+
+  V2XNode *node_;
+  vanetza::Runtime &runtime_;
+  vanetza::Clock::duration cam_interval_;
+
+  struct VehicleDimensions {
+    float wheel_radius;
+    float wheel_width;
+    float wheel_base;
+    float wheel_tread;
+    float front_overhang;
+    float rear_overhang;
+    float left_overhang;
+    float right_overhang;
+    float height;
+  };
+  VehicleDimensions vehicleDimensions_;
+
+  struct Ego_station {
+    double mgrs_x;
+    double mgrs_y;
+    double latitude;
+    double longitude;
+    double altitude;
+    double heading;
+  };
+  Ego_station ego_;
+
+  struct VelocityReport {
+    rclcpp::Time stamp;
+    float heading_rate;
+    float lateral_velocity;
+    float longitudinal_velocity;
+    float longitudinal_acceleration;
+  };
+  VelocityReport velocityReport_;
+  
+  uint8_t gearReport_;
+  
+  int generationTime_;
+  long gdt_timestamp_;
+
+  double objectConfidenceThreshold_;
+
+  bool updating_velocity_report_;
+  bool updating_gear_report_;
+  bool sending_;
+  bool is_sender_;
+  bool reflect_packet_;
+  bool include_all_persons_and_animals_;
+
+  int cam_num_;
+  int received_cam_num_;
+
+  bool use_dynamic_generation_rules_;
+};
+}
+
+#endif /* CAM_APPLICATION_HPP_EUIC2VFR */

include/autoware_v2x/v2x_app.hpp

@@ -4,9 +4,13 @@
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
+#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp"
+#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 #include <boost/asio/io_service.hpp>
 #include "autoware_v2x/cpm_application.hpp"
+#include "autoware_v2x/cam_application.hpp"
 #include "autoware_v2x/time_trigger.hpp"
 #include "autoware_v2x/link_layer.hpp"
 #include "autoware_v2x/ethernet_device.hpp"
@@ -24,18 +28,29 @@ namespace v2x
     V2XApp(V2XNode *);
     void start();
     void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr);
+    void getVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions);
+    void velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr);
+    void gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr);
     void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr);
 
     CpmApplication *cp;
+    CamApplication *cam;
     // V2XNode *v2x_node;
 
   private:
     friend class CpmApplication;
+    friend class CamApplication;
     friend class Application;
     V2XNode* node_;
     bool tf_received_;
     int tf_interval_;
+    bool velocity_report_received_;
+    int velocity_report_interval_;
+    bool gear_report_received_;
+    int gear_report_interval_;
+    bool vehicle_dimensions_set_;
     bool cp_started_;
+    bool cam_started_;
   };
 }
 

include/autoware_v2x/v2x_node.hpp

@@ -4,10 +4,15 @@
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
+#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp"
+#include "autoware_adapi_v1_msgs/srv/get_vehicle_dimensions.hpp"
+#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 #include <boost/asio/io_service.hpp>
 #include "autoware_v2x/v2x_app.hpp"
 #include "autoware_v2x/cpm_application.hpp"
+#include "autoware_v2x/cam_application.hpp"
 #include "autoware_v2x/time_trigger.hpp"
 #include "autoware_v2x/link_layer.hpp"
 #include "autoware_v2x/ethernet_device.hpp"
@@ -30,12 +35,20 @@ namespace v2x
 
   private:
     void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg);
+    void velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg);
+    void gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg);
+    void getVehicleDimensionsCallback(const autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response::ConstSharedPtr msg);
     void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg);
 
     rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr objects_sub_;
+    rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>::SharedPtr velocity_report_sub_;
+    rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::GearReport>::SharedPtr gear_report_sub_;
+    rclcpp::Subscription<autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response>::SharedPtr get_vehicle_dimensions_;
     rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_sub_;
     rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cpm_objects_pub_;
     rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cpm_sender_pub_;
+    rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cam_objects_pub_;
+    rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cam_sender_pub_;
 
     double pos_lat_;
     double pos_lon_;

launch/v2x.launch.xml

@@ -1,5 +1,5 @@
 <launch>
-  <arg name="network_interface" default="wlp5s0"/>
+  <arg name="network_interface" default="enx5414a7c7d3af"/>
   <arg name="is_sender" default="true"/>
   <node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen">
     <param from="$(find-pkg-share autoware_v2x)/config/autoware_v2x.param.yaml"/>

package.xml

@@ -10,6 +10,8 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
   <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>tf2_msgs</depend> 
   <depend>tf2</depend>
   <depend>geometry_msgs</depend>

src/cam_application.cpp

@@ -0,0 +1,299 @@
+#include "autoware_v2x/cam_application.hpp"
+#include "autoware_v2x/positioning.hpp"
+#include "autoware_v2x/security.hpp"
+#include "autoware_v2x/link_layer.hpp"
+#include "autoware_v2x/v2x_node.hpp"
+
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Matrix3x3.h"
+
+#include "rclcpp/rclcpp.hpp"
+#include <vanetza/btp/ports.hpp>
+#include <vanetza/asn1/cam.hpp>
+#include <vanetza/asn1/packet_visitor.hpp>
+#include <chrono>
+#include <functional>
+#include <iostream>
+#include <sstream>
+#include <exception>
+#include <GeographicLib/UTMUPS.hpp>
+#include <GeographicLib/MGRS.hpp>
+#include <string>
+
+#include <boost/units/cmath.hpp>
+#include <boost/units/systems/si/prefixes.hpp>
+
+#include <sqlite3.h>
+
+#define _USE_MATH_DEFINES
+#include <math.h>
+
+using namespace vanetza;
+using namespace std::chrono;
+
+namespace v2x
+{
+  CamApplication::CamApplication(V2XNode * node, Runtime & rt, bool is_sender)
+  : node_(node),
+    runtime_(rt),
+    vehicleDimensions_(),
+    ego_(),
+    velocityReport_(),
+    gearReport_(),
+    generationTime_(0),
+    updating_velocity_report_(false),
+    updating_gear_report_(false),
+    sending_(false),
+    is_sender_(is_sender),
+    reflect_packet_(false),
+    objectConfidenceThreshold_(0.0),
+    include_all_persons_and_animals_(false),
+    cam_num_(0),
+    received_cam_num_(0),
+    use_dynamic_generation_rules_(false)
+  {
+    RCLCPP_INFO(node_->get_logger(), "CamApplication started. is_sender: %d", is_sender_);
+    set_interval(milliseconds(100));
+    //createTables();
+  }
+
+  void CamApplication::set_interval(Clock::duration interval) {
+    cam_interval_ = interval;
+    runtime_.cancel(this);
+    schedule_timer();
+  }
+
+  void CamApplication::schedule_timer() {
+    runtime_.schedule(cam_interval_, std::bind(&CamApplication::on_timer, this, std::placeholders::_1));
+  }
+
+  void CamApplication::on_timer(vanetza::Clock::time_point) {
+    schedule_timer();
+    send();
+  }
+  
+  CamApplication::PortType CamApplication::port() {
+    return btp::ports::CAM;
+  }
+  
+  //std::string CamApplication::uuidToHexString(const unique_identifier_msgs::msg::UUID &id) {
+  //  std::stringstream ss;
+  //  for (int i = 0; i < id.uuid.size(); i++) {
+  //    ss << std::hex << std::setfill('0') << std::setw(2) << (int)id.uuid[i];
+  //  }
+  //  return ss.str();
+  //}
+  
+  void CamApplication::indicate(const Application::DataIndication &, Application::UpPacketPtr)
+  {
+    // TODO: implement
+  }
+
+  void CamApplication::updateMGRS(double *x, double *y) {
+    ego_.mgrs_x = *x;
+    ego_.mgrs_y = *y;
+  }
+  
+  void CamApplication::updateRP(double *lat, double *lon, double *altitude) {
+    ego_.latitude = *lat;
+    ego_.longitude = *lon;
+    ego_.altitude = *altitude;
+  }
+  
+  void CamApplication::updateGenerationTime(int *gdt, long *gdt_timestamp) {
+    generationTime_ = *gdt;
+    gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
+  }
+
+  void CamApplication::updateHeading(double *yaw) {
+    ego_.heading = *yaw;
+  }
+
+  //void CamApplication::setAllObjectsOfPersonsAnimalsToSend(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) {
+  //  if (msg->objects.size() > 0) {
+  //    for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) {
+  //      std::string object_uuid = uuidToHexString(obj.object_id);
+  //      auto found_object = std::find_if(objectsList.begin(), objectsList.end(), [&](auto const &e) {
+  //        return !strcmp(e.uuid.c_str(), object_uuid.c_str());
+  //      });
+  //      
+  //      if (found_object == objectsList.end()) {
+  //        
+  //      } else {
+  //        found_object->to_send = true;
+  //      }
+  //    }
+  //  }
+  //}
+  
+  void CamApplication::getVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions msg) {
+    vehicleDimensions_.wheel_radius = msg.wheel_radius;
+    vehicleDimensions_.wheel_width = msg.wheel_width;
+    vehicleDimensions_.wheel_base = msg.wheel_base;
+    vehicleDimensions_.wheel_tread = msg.wheel_tread;
+    vehicleDimensions_.front_overhang = msg.front_overhang;
+    vehicleDimensions_.rear_overhang = msg.rear_overhang;
+    vehicleDimensions_.left_overhang = msg.left_overhang;
+    vehicleDimensions_.right_overhang = msg.right_overhang;
+    vehicleDimensions_.height = msg.height;
+  }
+  
+  void CamApplication::updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
+    if (updating_velocity_report_) {
+      RCLCPP_WARN(node_->get_logger(), "[CamApplication::updatevelocityReport_] already updating velocity report");
+      return;
+    }
+    
+    updating_velocity_report_ = true;
+    
+    rclcpp::Time msg_stamp(msg->header.stamp.sec, msg->header.stamp.nanosec);
+    float dt = (msg_stamp - velocityReport_.stamp).seconds();
+    if (dt == 0) {
+      RCLCPP_WARN(node_->get_logger(), "[CamApplication::updatevelocityReport_] deltaTime is 0");
+      return;
+    }
+    float longitudinal_acceleration = (msg->longitudinal_velocity - velocityReport_.longitudinal_velocity) / dt;
+    
+    velocityReport_.stamp = msg->header.stamp;
+    velocityReport_.heading_rate = msg->heading_rate;
+    velocityReport_.lateral_velocity = msg->lateral_velocity;
+    velocityReport_.longitudinal_velocity = msg->longitudinal_velocity;
+    velocityReport_.longitudinal_acceleration = longitudinal_acceleration;
+  }
+
+  void CamApplication::updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
+    if (updating_gear_report_) {
+      RCLCPP_WARN(node_->get_logger(), "[CamApplication::updatevelocityReport_] already updating gear report");
+      return;
+    }
+
+    updating_gear_report_ = true;
+
+    gearReport_ = msg->report;
+  }
+  
+  void CamApplication::send() {
+    if (!is_sender_) return;
+    
+    if (sending_) {
+      RCLCPP_WARN(node_->get_logger(), "[CamApplication::send] already sending");
+      return;
+    }
+    
+    sending_ = true;
+
+    //printObjectsList(cam_num_);
+    
+    RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] cam_num: %d", cam_num_);
+    // RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] sending CAM");
+    
+    vanetza::asn1::Cam message;
+    
+    ItsPduHeader_t &header = message->header;
+    header.protocolVersion = 2;
+    header.messageID = ItsPduHeader__messageID_cam;
+    header.stationID = 0;
+    
+    CoopAwareness_t &cam = message->cam;
+
+    //asn_long2INTEGER(&cam.generationDeltaTime, (long) gdt_timestamp_);
+    
+    BasicContainer_t &basic_container = cam.camParameters.basicContainer;
+    basic_container.stationType = StationType_passengerCar;
+    basic_container.referencePosition.latitude = ego_.latitude * 1e7;
+    basic_container.referencePosition.longitude = ego_.longitude * 1e7;
+    basic_container.referencePosition.altitude.altitudeValue = ego_.altitude;
+    
+    // UNAVAILABLE VALUES FOR TESTING
+    basic_container.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable;
+    basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
+    basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
+    basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
+    // ------------------------------
+
+    BasicVehicleContainerHighFrequency_t &bvc = cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
+    cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
+    
+    int heading = std::lround(((-ego_.heading * 180.0 / M_PI) + 90.0) * 10.0);
+    if (heading < 0) heading += 3600;
+    bvc.heading.headingValue = heading;
+    
+    float heading_rate = velocityReport_.heading_rate;
+    float lateral_velocity = velocityReport_.lateral_velocity;
+    float longitudinal_velocity = velocityReport_.longitudinal_velocity;
+    
+    float speed = std::sqrt(std::pow(longitudinal_velocity, 2) + std::pow(lateral_velocity, 2));
+    bvc.speed.speedValue = std::lround(speed * 100);
+
+    if ((gearReport_ >= 2 && gearReport_ <= 19) || gearReport_ == 21 || gearReport_ == 22)
+      bvc.driveDirection = DriveDirection_forward;
+    else if (gearReport_ == 20 || gearReport_ == 21)
+      bvc.driveDirection = DriveDirection_backward;
+    else
+      bvc.driveDirection = DriveDirection_unavailable;
+    
+    float vehicleLength = vehicleDimensions_.front_overhang + vehicleDimensions_.wheel_base + vehicleDimensions_.rear_overhang;
+    if (vehicleLength <= 0 || vehicleLength >= 1023)
+      bvc.vehicleLength.vehicleLengthValue = VehicleLengthValue_unavailable;
+    else
+      bvc.vehicleLength.vehicleLengthValue = vehicleLength;
+    
+    float vehicleWidth = vehicleDimensions_.left_overhang + vehicleDimensions_.wheel_tread + vehicleDimensions_.right_overhang;
+    if (vehicleWidth <= 0 || vehicleWidth >= 62)
+      bvc.vehicleWidth = VehicleWidth_unavailable;
+    else
+      bvc.vehicleWidth = vehicleWidth;
+
+    //if (longitudinal_acceleration > 160 || longitudinal_acceleration < -160) {
+    //  RCLCPP_WARN(node_->get_logger(), "[CamApplication::send] Longitudinal acceleration out of bounds: %d", longitudinal_acceleration);
+    //  bvc.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
+    //} else {
+    //  bvc.longitudinalAcceleration.longitudinalAccelerationValue = longitudinal_acceleration;
+    //}
+    
+    bvc.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
+    
+    if (longitudinal_velocity != 0)
+      bvc.curvature.curvatureValue = std::lround(lateral_velocity / std::pow(longitudinal_velocity, 2) * 100);
+    else
+      bvc.curvature.curvatureValue = std::numeric_limits<long>::infinity();
+    bvc.curvatureCalculationMode = CurvatureCalculationMode_yawRateNotUsed;
+
+    bvc.yawRate.yawRateValue = std::lround(heading_rate * 100);
+
+    // UNAVAILABLE VALUES FOR TESTING
+    bvc.heading.headingConfidence = HeadingConfidence_unavailable;
+    bvc.speed.speedConfidence = SpeedConfidence_unavailable;
+    bvc.vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable;
+    bvc.longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable;
+    bvc.curvature.curvatureConfidence = CurvatureConfidence_unavailable;
+    bvc.yawRate.yawRateConfidence = YawRateConfidence_unavailable;
+    // ------------------------------
+    
+    RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM");
+    //insertCamToCamTable(message, (char *) "cam_sent");
+    std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
+    payload->layer(OsiLayer::Application) = std::move(message);
+    
+    Application::DataRequest request;
+    request.its_aid = aid::CP;
+    request.transport_type = geonet::TransportType::SHB;
+    request.communication_profile = geonet::CommunicationProfile::ITS_G5;
+    
+    Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
+    
+    if (!confirm.accepted()) {
+      throw std::runtime_error("[CamApplication::send] CAM application data request failed");
+    }
+
+    sending_ = false;
+
+    std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds> (
+      std::chrono::system_clock::now().time_since_epoch()
+    );
+    node_->latency_log_file << "T_depart," << cam_num_ << "," << ms.count() << std::endl;
+
+    ++cam_num_;
+  }
+
+}

src/v2x_app.cpp

@@ -6,6 +6,7 @@
 #include "autoware_v2x/security.hpp"
 #include "autoware_v2x/link_layer.hpp"
 #include "autoware_v2x/cpm_application.hpp"
+#include "autoware_v2x/cam_application.hpp"
 
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
@@ -14,6 +15,7 @@
 #include "tf2/LinearMath/Matrix3x3.h"
 
 #include <vanetza/asn1/cpm.hpp>
+#include <vanetza/asn1/cam.hpp>
 #include <sstream>
 #include <memory>
 #include <GeographicLib/UTMUPS.hpp>
@@ -32,7 +34,9 @@ namespace v2x
     node_(node),
     tf_received_(false),
     tf_interval_(0),
-    cp_started_(false)
+    vehicle_dimensions_set_(false),
+    cp_started_(false),
+    cam_started_(false)
   {
   }
 
@@ -46,6 +50,30 @@ namespace v2x
     }
   }
 
+  void V2XApp::getVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions msg) {
+    if (vehicle_dimensions_set_) return;
+    cam->getVehicleDimensions(msg);
+    vehicle_dimensions_set_ = true;
+  }
+
+  void V2XApp::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
+    if (!velocity_report_received_) {
+      RCLCPP_WARN(node_->get_logger(), "[V2XApp::velocityReportCallback VelocityReport not received yet");
+    }
+    if (velocity_report_received_ && cam_started_) {
+      cam->updateVelocityReport(msg);
+    }
+  }
+
+  void V2XApp::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
+    if (!gear_report_received_) {
+      RCLCPP_WARN(node_->get_logger(), "[V2XApp::gearReportCallback GearReport not received yet");
+    }
+    if (gear_report_received_ && cam_started_) {
+      cam->updateGearReport(msg);
+    }
+  }
+
   void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
 
     tf_received_ = true;
@@ -94,6 +122,13 @@ namespace v2x
       cp->updateHeading(&yaw);
       cp->updateGenerationTime(&gdt, &timestamp_msec);
     }
+
+    if (cam && cp_started_) {
+      cam->updateMGRS(&x, &y);
+      cam->updateRP(&lat, &lon, &z);
+      cam->updateHeading(&yaw);
+      cam->updateGenerationTime(&gdt, &timestamp_msec);
+    }
   }
 
   void V2XApp::start() {
@@ -132,10 +167,13 @@ namespace v2x
     bool is_sender;
     node_->get_parameter("is_sender", is_sender);
     cp = new CpmApplication(node_, trigger.runtime(), is_sender);
+    cam = new CamApplication(node_, trigger.runtime(), is_sender);
 
     context.enable(cp);
+    context.enable(cam);
 
     cp_started_ = true;
+    cam_started_ = true;
 
     io_service.run();
   }

src/v2x_node.cpp

@@ -6,11 +6,13 @@
 #include "autoware_v2x/security.hpp"
 #include "autoware_v2x/link_layer.hpp"
 #include "autoware_v2x/cpm_application.hpp"
+#include "autoware_v2x/cam_application.hpp"
 
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 
 #include <vanetza/asn1/cpm.hpp>
+#include <vanetza/asn1/cam.hpp>
 #include <sstream>
 #include <memory>
 #include <boost/thread.hpp>
@@ -34,11 +36,16 @@ namespace v2x
     objects_sub_ = this->create_subscription<autoware_auto_perception_msgs::msg::PredictedObjects>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
     tf_sub_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));
     
+    // Topic subscriptions for CAMApplication
+    velocity_report_sub_ = this->create_subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>("/vehicle/status/velocity_status", 10, std::bind(&V2XNode::velocityReportCallback, this, _1));
+    gear_report_sub_ = this->create_subscription<autoware_auto_vehicle_msgs::msg::GearReport>("/vehicle/status/gear_status", 10, std::bind(&V2XNode::gearReportCallback, this, _1));
+    get_vehicle_dimensions_ = this->create_subscription<autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response>("/api/vehicle/dimensions", 10, std::bind(&V2XNode::getVehicleDimensionsCallback, this, _1));
+
     cpm_objects_pub_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/objects", rclcpp::QoS{10});
     // cpm_sender_pub_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/sender", rclcpp::QoS{10});
 
     // Declare Parameters
-    this->declare_parameter<std::string>("network_interface", "vmnet1");
+    this->declare_parameter<std::string>("network_interface", "enx5414a7c7d3af");
     this->declare_parameter<bool>("is_sender", true);
 
     // Launch V2XApp in a new thread
@@ -164,6 +171,18 @@ namespace v2x
   void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
     app->tfCallback(msg);
   }
+  
+  void V2XNode::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
+    app->velocityReportCallback(msg);
+  }
+  
+  void V2XNode::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
+    app->gearReportCallback(msg);
+  }
+  
+  void V2XNode::getVehicleDimensionsCallback(const autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response::ConstSharedPtr msg) {
+    app->getVehicleDimensions(msg->dimensions);
+  }
 }
 
 #include "rclcpp_components/register_node_macro.hpp"