[FORGE] test

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

CMakeLists.txt

@@ -22,12 +22,17 @@ set(VANETZA_INSTALL ON)
 option(BUILD_SHARED_LIBS "Build shared libraries" ON)
 find_package(Vanetza REQUIRED)
 find_package(GeographicLib 1.37 REQUIRED)
-find_package(Boost COMPONENTS thread REQUIRED)
+find_package(Boost COMPONENTS thread program_options REQUIRED)
+find_package(etsi_its_cam_ts_coding REQUIRED)
+find_package(etsi_its_cam_ts_conversion REQUIRED)
+find_package(etsi_its_msgs_utils REQUIRED)
 ament_auto_find_build_dependencies()
 find_package(std_msgs REQUIRED)
+find_package(Protobuf REQUIRED)
 
 include_directories(
   include
+  ${CMAKE_CURRENT_BINARY_DIR}/src
 )
 
 ament_auto_add_library(autoware_v2x SHARED
@@ -36,6 +41,7 @@ ament_auto_add_library(autoware_v2x SHARED
   src/application.cpp
   src/cpm_application.cpp
   src/cam_application.cpp
+  src/cube_evk_link.cpp
   src/ethernet_device.cpp
   src/link_layer.cpp
   src/raw_socket_link.cpp
@@ -46,7 +52,9 @@ ament_auto_add_library(autoware_v2x SHARED
   src/security.cpp
 )
 
-target_link_libraries(autoware_v2x Vanetza::vanetza ${GeographicLib_LIBRARIES} Boost::thread sqlite3)
+protobuf_generate(TARGET autoware_v2x PROTOS src/cube_evk_radio.proto)
+
+target_link_libraries(autoware_v2x Vanetza::vanetza ${GeographicLib_LIBRARIES} Boost::thread Boost::program_options sqlite3 etsi_its_cam_ts_coding::etsi_its_cam_ts_coding etsi_its_cam_ts_conversion::etsi_its_cam_ts_conversion protobuf::libprotobuf)
 
 rclcpp_components_register_node(autoware_v2x
   PLUGIN "v2x::V2XNode"

config/autoware_v2x.param.yaml

@@ -1,4 +1,11 @@
 /**:
   ros__parameters:
-    network_interface: "wlp4s0"
+    link_layer: "cube-evk"
+    network_interface: "v2x_testing"
+    cube_ip: "192.168.94.84"
     is_sender: true
+    security: "none"
+    certificate: ""
+    certificate-key: ""
+    certificate-chain:
+      - ""

docs/design/index.md

@@ -17,33 +17,37 @@ The V2XNode launches a ROS2 Node for AutowareV2X. Its main purpose is to act as
 
 ### Input
 
-| Name                 | Type                            | Description      |
-| -------------------- | ------------------------------- | ---------------- |
-| `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Perceived Objects by Autoware |
-| `/tf` | `tf2_msgs::msg::TFMessage` | Pose of Ego Vehicle |
-| `/vehicle/status/velocity_report` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | Velocity of Ego Vehicle |
-| `/api/vehicle/status` | `autoware_adapi_v1_msgs::msg::VehicleStatus` | Vehicle Status (gear, steering angle, etc.) |
-| `/api/vehicle/dimensions` | `autoware_adapi_v1_msgs::srv::GetVehicleDimensions` | Service to get Vehicle Dimensions |
+| Name                                     | Type                                                   | Description                                 |
+|------------------------------------------|--------------------------------------------------------|---------------------------------------------|
+| `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Perceived Objects by Autoware               |
+| `/tf`                                    | `tf2_msgs::msg::TFMessage`                             | Pose of Ego Vehicle                         |
+| `/vehicle/status/velocity_report`        | `autoware_auto_vehicle_msgs::msg::VelocityReport`      | Velocity of Ego Vehicle                     |
+| `/api/vehicle/status`                    | `autoware_adapi_v1_msgs::msg::VehicleStatus`           | Vehicle Status (gear, steering angle, etc.) |
+| `/api/vehicle/dimensions`                | `autoware_adapi_v1_msgs::srv::GetVehicleDimensions`    | Service to get Vehicle Dimensions           |
 
 ### Output
 
-| Name                 | Type                            | Description      |
-| -------------------- | ------------------------------- | ---------------- |
-| `/v2x/cpm/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Objects received by CPMs |
+| Name                   | Type                                                   | Description              |
+|------------------------|--------------------------------------------------------|--------------------------|
+| `/v2x/cpm/objects`     | `autoware_auto_perception_msgs::msg::PredictedObjects` | Objects received by CPMs |
+| `/v2x/cam_ts/received` | `etsi_its_cam_ts_msgs::msg::CAM`                       | Received CAMs            |
 
 ### Functions
 
-| Name                 | Description      |
-| -------------------- | ---------------- |
-| `objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg)` | Call `V2XApp::objectsCallback` |
-| `tfCallback` | Call `V2XApp::tfCallback` |
-| `velocityReportCallback` | Call `V2XApp::velocityReportCallback` |
-| `vehicleStatusCallback` | Call `V2XApp::vehicleStatusCallback` |
-| `getVehicleDimensions` | Sends a request to the service used for vehicle dimensions |
-| `publishObjects(std::vector<CpmApplication::Object> *objectsStack, int cpm_num)` |  |
-| `publishCpmSenderObject` | Not used now |
+| Name                                                                                              | Description                                                |
+|---------------------------------------------------------------------------------------------------|------------------------------------------------------------|
+| `objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg)` | Call `V2XApp::objectsCallback`                             |
+| `tfCallback`                                                                                      | Call `V2XApp::tfCallback`                                  |
+| `velocityReportCallback`                                                                          | Call `V2XApp::velocityReportCallback`                      |
+| `vehicleStatusCallback`                                                                           | Call `V2XApp::vehicleStatusCallback`                       |
+| `getVehicleDimensions`                                                                            | Sends a request to the service used for vehicle dimensions |
+| `publishObjects(std::vector<CpmApplication::Object> *objectsStack, int cpm_num)`                  |                                                            |
+| `publishCpmSenderObject`                                                                          | Not used now                                               |
+| `publishReceivedCam(etsi_its_cam_ts_msgs::msg::CAM &msg)`                                         | Publishes a received CAM into the ROS environment          |
 
 
 ## V2XApp
 
 ## CPM Facility
+
+## CAM Facility

include/autoware_v2x/cam_application.hpp

@@ -2,14 +2,20 @@
 #define CAM_APPLICATION_HPP_EUIC2VFR
 
 #include "autoware_v2x/application.hpp"
+#include "autoware_v2x/positioning.hpp"
 #include "rclcpp/rclcpp.hpp"
+
+#include <vanetza/asn1/cam.hpp>
+
+#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp"
+#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
+
 #include <boost/asio/io_service.hpp>
 #include <boost/asio/steady_timer.hpp>
-#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
-#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp"
-#include "autoware_adapi_v1_msgs/msg/vehicle_status.hpp"
-#include "autoware_v2x/positioning.hpp"
-#include <vanetza/asn1/cam.hpp>
+
+#include <etsi_its_cam_ts_coding/cam_ts_CAM.h>
 
 namespace v2x
 {
@@ -22,32 +28,34 @@ public:
   void indicate(const DataIndication &, UpPacketPtr) override;
   void set_interval(vanetza::Clock::duration);
   void updateMGRS(double *, double *);
-  void updateRP(double *, double *, double *);
-  void updateGenerationDeltaTime(int *, long *);
-  void updateHeading(double *);
+  void updateRP(const double *, const double *, const double *);
+  void updateHeading(const double *);
   void setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &);
   void updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr);
-  void updateVehicleStatus(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr);
+  void updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr);
+  void updateSteeringReport(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr);
   void send();
 
 private:
+  void calc_interval();
   void schedule_timer();
   void on_timer(vanetza::Clock::time_point);
+  static void build_etsi_its_cam_ts_from_vanetza(vanetza::asn1::Cam &, cam_ts_CAM_t &);
 
   V2XNode *node_;
   vanetza::Runtime &runtime_;
-  vanetza::Clock::duration cam_interval_;
+  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;
+    float wheel_radius = 0.0;
+    float wheel_width = 0.0;
+    float wheel_base = 0.0;
+    float wheel_tread = 0.0;
+    float front_overhang = 0.0;
+    float rear_overhang = 0.0;
+    float left_overhang = 0.0;
+    float right_overhang = 0.0;
+    float height = 0.0;
   };
   VehicleDimensions vehicleDimensions_;
 
@@ -77,17 +85,17 @@ private:
       return deque.size();
     }
 
-    double getMean() {
+    [[nodiscard]] double getMean() const {
       return this->mean;
     }
 
     using iterator = std::deque<double>::const_iterator;
 
-    iterator begin() const {
+    [[nodiscard]] iterator begin() const {
       return deque.begin();
     }
 
-    iterator end() const {
+    [[nodiscard]] iterator end() const {
       return deque.end();
     }
 
@@ -109,9 +117,9 @@ private:
     PositionsDeque x;
     PositionsDeque y;
 
-    double semiMajorConfidence;
-    double semiMinorConfidence;
-    double semiMajorOrientation;
+    double semiMajorConfidence{};
+    double semiMinorConfidence{};
+    double semiMajorOrientation{};
   };
   PositionConfidenceEllipse positionConfidenceEllipse_;
 
@@ -120,6 +128,7 @@ private:
     float heading_rate;
     float lateral_velocity;
     float longitudinal_velocity;
+    float speed;
     float longitudinal_acceleration;
   };
   VelocityReport velocityReport_;
@@ -130,20 +139,10 @@ private:
   };
   VehicleStatus vehicleStatus_;
 
-  int generationDeltaTime_;
-  long gdt_timestamp_;
-
-  double objectConfidenceThreshold_;
-
-  bool updating_velocity_report_;
-  bool updating_vehicle_status_;
   bool sending_;
   bool is_sender_;
-  bool reflect_packet_;
 
   unsigned long stationId_;
-  int cam_num_;
-  int received_cam_num_;
 
   bool use_dynamic_generation_rules_;
 };

include/autoware_v2x/cube_evk_link.hpp

@@ -0,0 +1,43 @@
+#ifndef CUBE_EVK_LINK_HPP_
+#define CUBE_EVK_LINK_HPP_
+
+#include "autoware_v2x/link_layer.hpp"
+#include <boost/asio/io_service.hpp>
+#include <boost/asio/ip/udp.hpp>
+#include <vanetza/net/chunk_packet.hpp>
+
+#include <array>
+#include <cstdint>
+#include <memory>
+
+class LinkLayerReception;
+class LinkLayerTransmission;
+
+class CubeEvkLink : public LinkLayer
+{
+public:
+  CubeEvkLink(boost::asio::io_service&, boost::asio::ip::address_v4);
+
+  void handle_packet_received(const boost::system::error_code&, size_t);
+  void request(const vanetza::access::DataRequest&, std::unique_ptr<vanetza::ChunkPacket>) override;
+
+  void indicate(IndicationCallback callback) override;
+
+private:
+  boost::asio::io_service& io_;
+  IndicationCallback indicate_to_router_;
+
+  boost::asio::ip::udp::socket tx_socket_;
+  boost::asio::ip::udp::socket rx_socket_;
+  boost::asio::ip::udp::endpoint host_endpoint_;
+
+  std::array<uint8_t, 4096> received_data_;
+
+  static constexpr unsigned int cube_evk_radio_port_tx = 33210;
+  static constexpr unsigned int cube_evk_radio_port_rx = 33211;
+
+  void pass_message_to_router(std::unique_ptr<LinkLayerReception>);
+  std::unique_ptr<LinkLayerTransmission> create_link_layer_tx(const vanetza::access::DataRequest&, std::unique_ptr<vanetza::ChunkPacket>);
+};
+
+#endif /* CUBE_EVK_LINK_HPP_ */

include/autoware_v2x/link_layer.hpp

@@ -23,7 +23,6 @@ class LinkLayer : public vanetza::access::Interface, public LinkLayerIndication
 };
 
 std::unique_ptr<LinkLayer>
-create_link_layer(boost::asio::io_service&, const EthernetDevice&, const std::string& name);
+create_link_layer(boost::asio::io_service&, const EthernetDevice&, const std::string&, const std::string&);
 
 #endif /* LINK_LAYER_HPP_FGEK0QTH */
-

include/autoware_v2x/security.hpp

@@ -9,7 +9,6 @@
 #include <memory>
 
 std::unique_ptr<vanetza::security::SecurityEntity>
-create_security_entity(const vanetza::Runtime&, vanetza::PositionProvider&);
+create_security_entity(const boost::program_options::variables_map&, const vanetza::Runtime&, vanetza::PositionProvider&);
 
 #endif /* SECURITY_HPP_FV13ZIYA */
-

include/autoware_v2x/v2x_app.hpp

@@ -5,9 +5,8 @@
 #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_adapi_v1_msgs/srv/get_vehicle_dimensions.hpp"
-#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp"
-#include "autoware_adapi_v1_msgs/msg/vehicle_status.hpp"
+#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 #include <boost/asio/io_service.hpp>
 #include "autoware_v2x/cpm_application.hpp"
@@ -29,9 +28,9 @@ namespace v2x
     V2XApp(V2XNode *);
     void start();
     void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr);
-    void setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &);
     void velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr);
-    void vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr);
+    void gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr);
+    void steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr);
     void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr);
 
     CpmApplication *cp;
@@ -45,10 +44,6 @@ namespace v2x
     V2XNode* node_;
     bool tf_received_;
     int tf_interval_;
-    bool velocity_report_received_;
-    int velocity_report_interval_;
-    bool vehicle_status_received_;
-    int vehicle_status_interval_;
     bool vehicle_dimensions_set_;
     bool cp_started_;
     bool cam_started_;

include/autoware_v2x/v2x_node.hpp

@@ -5,7 +5,10 @@
 #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_adapi_v1_msgs/msg/vehicle_status.hpp"
+#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/steering_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"
@@ -19,6 +22,8 @@
 #include "autoware_v2x/router_context.hpp"
 #include <fstream>
 
+#include <etsi_its_cam_ts_msgs/msg/cam.hpp>
+
 namespace v2x
 {
   class V2XNode : public rclcpp::Node
@@ -28,26 +33,32 @@ namespace v2x
     V2XApp *app;
     void publishObjects(std::vector<CpmApplication::Object> *, int cpm_num);
     void publishCpmSenderObject(double, double, double);
+    void publishReceivedCam(etsi_its_cam_ts_msgs::msg::CAM &);
+    void getVehicleDimensions();
 
     std::ofstream latency_log_file;
 
   private:
     void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg);
     void velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg);
-    void vehicleStatucCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg);
-    void getVehicleDimensions();
+    void gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg);
+    void steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::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_adapi_v1_msgs::msg::VehicleStatus>::SharedPtr vehicle_status_sub_;
+    rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::GearReport>::SharedPtr gear_report_sub_;
+    rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>::SharedPtr steering_report_sub_;
     rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_sub_;
     rclcpp::Client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>::SharedPtr get_vehicle_dimensions_;
     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<etsi_its_cam_ts_msgs::msg::CAM>::SharedPtr cam_rec_pub_;
 
     double pos_lat_;
     double pos_lon_;
+
+    bool vehicle_dimensions_available_;
   };
 }
 

launch/v2x.launch.xml

@@ -1,6 +1,9 @@
 <launch>
-  <arg name="network_interface" default="wlp5s0"/>
+  <arg name="link_layer" default="ethernet"/>
+  <arg name="network_interface" default="v2x_testing"/>
+  <arg name="cube_ip" default="127.0.0.1"/>
   <arg name="is_sender" default="true"/>
+  <arg name="security" default="none"/>
   <node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen">
     <param from="$(find-pkg-share autoware_v2x)/config/autoware_v2x.param.yaml"/>
   </node>

package.xml

@@ -19,6 +19,7 @@
   <depend>std_msgs</depend>
   <depend>rclcpp_components</depend>
   <depend>Vanetza</depend>
+  <depend>etsi_its_messages</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/cam_application.cpp

@@ -11,6 +11,7 @@
 #include <vanetza/btp/ports.hpp>
 #include <vanetza/asn1/cam.hpp>
 #include <vanetza/asn1/packet_visitor.hpp>
+#include <vanetza/facilities/cam_functions.hpp>
 #include <chrono>
 #include <functional>
 #include <iostream>
@@ -21,14 +22,14 @@
 #include <GeographicLib/MGRS.hpp>
 #include <string>
 
+#include <etsi_its_cam_ts_msgs/msg/cam.hpp>
+#include <etsi_its_cam_ts_conversion/convertCAM.h>
+
 #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;
 
@@ -37,24 +38,18 @@ namespace v2x
   CamApplication::CamApplication(V2XNode * node, Runtime & rt, bool is_sender)
   : node_(node),
     runtime_(rt),
+    cam_interval_(milliseconds(1000)),
     vehicleDimensions_(),
     ego_(),
     positionConfidenceEllipse_(),
     velocityReport_(),
     vehicleStatus_(),
-    generationDeltaTime_(0),
-    updating_velocity_report_(false),
-    updating_vehicle_status_(false),
     sending_(false),
     is_sender_(is_sender),
-    reflect_packet_(false),
-    objectConfidenceThreshold_(0.0),
-    cam_num_(0),
-    received_cam_num_(0),
-    use_dynamic_generation_rules_(false)
+    use_dynamic_generation_rules_(true)
   {
     RCLCPP_INFO(node_->get_logger(), "CamApplication started. is_sender: %d", is_sender_);
-    set_interval(milliseconds(100));
+    set_interval(cam_interval_);
     //createTables();
 
     // Generate ID for this station
@@ -62,6 +57,8 @@ namespace v2x
     std::mt19937 gen(rd());
     std::uniform_int_distribution<unsigned long> dis(0, 4294967295);
     stationId_ = dis(gen);
+
+    node_->getVehicleDimensions();
   }
 
   void CamApplication::set_interval(Clock::duration interval) {
@@ -70,11 +67,72 @@ namespace v2x
     schedule_timer();
   }
 
+  static double calc_haversine(double lat1, double lon1, double lat2, double lon2) {
+    const double R = 6371e3;
+    double dLat = (lat2 - lat1) * M_PI / 180;
+    double dLon = (lon2 - lon1) * M_PI / 180;
+
+    lat1 = lat1 * (M_PI / 180);
+    lat2 = lat2 * (M_PI / 180);
+
+    double a = std::sin(dLat / 2) * std::sin(dLat / 2) +
+               std::sin(dLon / 2) * std::sin(dLon / 2) * std::cos(lat1) * std::cos(lat2);
+    double c = 2 * std::atan2(std::sqrt(a), std::sqrt(1 - a));
+
+    return R * c;
+  }
+
+  void CamApplication::calc_interval() {
+    if (use_dynamic_generation_rules_) {
+      static struct {
+        double speed;
+        double heading;
+        double latitude;
+        double longitude;
+      } lastCam = {0.0, 0.0, 0.0, 0.0};
+
+      double latitude = ego_.latitude;
+      double longitude = ego_.longitude;
+      double heading = ego_.heading;
+      double speed = velocityReport_.speed;
+
+      double deltaSpeed = std::fabs(speed - lastCam.speed);
+      double deltaHeading = std::fabs(heading - lastCam.heading);
+      if (deltaHeading > 180.0) deltaHeading = 360.0 - deltaHeading;
+
+      double distance = calc_haversine(lastCam.latitude, lastCam.longitude, latitude, longitude);
+
+      const double distance_threshold = 4.0;
+      const double speed_threshold = 0.5;
+      const double heading_threshold = 4.0;
+
+      if (distance > distance_threshold || deltaSpeed > speed_threshold || deltaHeading > heading_threshold) {
+        int interval = static_cast<int>((distance / speed) * 1000);
+
+        if (interval < 100) {
+          cam_interval_ = milliseconds(100);
+        } else if (interval > 1000) {
+          cam_interval_ = milliseconds(1000);
+        } else {
+          cam_interval_ = milliseconds(interval);
+        }
+
+        lastCam.speed = speed;
+        lastCam.heading = heading;
+        lastCam.latitude = latitude;
+        lastCam.longitude = longitude;
+      } else {
+        cam_interval_ = milliseconds(1000);
+      }
+    }
+  }
+
   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) {
+    calc_interval();
     schedule_timer();
     send();
   }
@@ -83,9 +141,31 @@ namespace v2x
     return btp::ports::CAM;
   }
 
-  void CamApplication::indicate(const Application::DataIndication &, Application::UpPacketPtr)
+  void CamApplication::indicate(const Application::DataIndication &indication, Application::UpPacketPtr packet)
   {
-    // TODO: implement
+    asn1::PacketVisitor<asn1::Cam> visitor;
+    std::shared_ptr<const asn1::Cam> rec_cam_ptr = boost::apply_visitor(visitor, *packet);
+
+    if (!rec_cam_ptr) {
+      RCLCPP_INFO(node_->get_logger(), "[CamApplication::indicate] Received invalid CAM");
+      return;
+    }
+
+    asn1::Cam rec_cam = *rec_cam_ptr;
+    std::chrono::milliseconds now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
+    RCLCPP_INFO(node_->get_logger(), "[CamApplication::indicate] Received CAM from station with ID #%ld at %ld epoch time", rec_cam->header.stationID, now_ms.count());
+    vanetza::facilities::print_indented(std::cout, rec_cam, "  ", 0);
+
+    cam_ts_CAM_t ts_cam;
+    std::memset(&ts_cam, 0, sizeof(ts_cam));
+
+    CamApplication::build_etsi_its_cam_ts_from_vanetza(rec_cam, ts_cam);
+
+    etsi_its_cam_ts_msgs::msg::CAM ros_msg;
+    std::memset(&ros_msg, 0, sizeof(ros_msg));
+    etsi_its_cam_ts_conversion::toRos_CAM(ts_cam, ros_msg);
+
+    node_->publishReceivedCam(ros_msg);
   }
 
   void CamApplication::updateMGRS(double *x, double *y) {
@@ -93,7 +173,7 @@ namespace v2x
     ego_.mgrs_y = *y;
   }
 
-  void CamApplication::updateRP(double *lat, double *lon, double *altitude) {
+  void CamApplication::updateRP(const double *lat, const double *lon, const double *altitude) {
     ego_.latitude = *lat;
     ego_.longitude = *lon;
     ego_.altitude = *altitude;
@@ -102,16 +182,16 @@ namespace v2x
     positionConfidenceEllipse_.y.insert(*lon);
   }
 
-  void CamApplication::updateGenerationDeltaTime(int *gdt, long *gdt_timestamp) {
-    generationDeltaTime_ = *gdt;
-    gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
-  }
-
-  void CamApplication::updateHeading(double *yaw) {
+  void CamApplication::updateHeading(const double *yaw) {
     ego_.heading = *yaw;
   }
 
   void CamApplication::setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &msg) {
+    if (msg == autoware_adapi_v1_msgs::msg::VehicleDimensions{}) {
+      RCLCPP_WARN(node_->get_logger(), "[CamApplication::getVehicleDimensions] Vehicle dimensions not available");
+      return;
+    }
+
     vehicleDimensions_.wheel_radius = msg.wheel_radius;
     vehicleDimensions_.wheel_width = msg.wheel_width;
     vehicleDimensions_.wheel_base = msg.wheel_base;
@@ -124,40 +204,30 @@ namespace v2x
   }
 
   void CamApplication::updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
-    if (updating_velocity_report_) {
-      return;
-    }
-
-    updating_velocity_report_ = true;
-
     rclcpp::Time msg_stamp(msg->header.stamp.sec, msg->header.stamp.nanosec);
-    float dt = msg_stamp.seconds() - velocityReport_.stamp.seconds();
+    double dt = msg_stamp.seconds() - 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;
+    float speed = std::sqrt(std::pow(msg->longitudinal_velocity, 2) + std::pow(msg->lateral_velocity, 2));
 
     velocityReport_.stamp = msg->header.stamp;
     velocityReport_.heading_rate = msg->heading_rate;
     velocityReport_.lateral_velocity = msg->lateral_velocity;
     velocityReport_.longitudinal_velocity = msg->longitudinal_velocity;
+    velocityReport_.speed = speed;
     velocityReport_.longitudinal_acceleration = longitudinal_acceleration;
-
-    updating_velocity_report_ = false;
   }
 
-  void CamApplication::updateVehicleStatus(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
-    if (updating_vehicle_status_) {
-      return;
-    }
+  void CamApplication::updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
+    rclcpp::Time msg_stamp(msg->stamp.sec, msg->stamp.nanosec);
+    vehicleStatus_.gear = msg->report;
+  }
 
-    updating_vehicle_status_ = true;
-
-    vehicleStatus_.gear = msg->gear.status;
+  void CamApplication::updateSteeringReport(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
     vehicleStatus_.steering_tire_angle = msg->steering_tire_angle;
-
-    updating_vehicle_status_ = false;
   }
 
   void CamApplication::send() {
@@ -170,7 +240,7 @@ namespace v2x
 
     sending_ = true;
 
-    RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] cam_num: %d", cam_num_);
+    std::chrono::milliseconds now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
 
     vanetza::asn1::Cam message;
 
@@ -181,8 +251,8 @@ namespace v2x
 
     CoopAwareness_t &cam = message->cam;
 
-    RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] %ld", gdt_timestamp_);
-    cam.generationDeltaTime = generationDeltaTime_;
+    // Convert Unix timestamp to ETSI epoch (2004-01-01 00:00:00)
+    cam.generationDeltaTime = (now_ms.count() - 1072915200000) % 65536;
 
     BasicContainer_t &basic_container = cam.camParameters.basicContainer;
     basic_container.stationType = StationType_passengerCar;
@@ -257,13 +327,13 @@ namespace v2x
     uint8_t gearStatus = vehicleStatus_.gear;
     float steering_tire_angle = vehicleStatus_.steering_tire_angle;
 
-    long speed = std::lround(std::sqrt(std::pow(longitudinal_velocity, 2) + std::pow(lateral_velocity, 2)) * 100);
+    long speed = std::lround(velocityReport_.speed * 100);
     if (0 <= speed && speed <= 16382) bvc.speed.speedValue = speed;
     else bvc.speed.speedValue = SpeedValue_unavailable;
 
-    if (gearStatus == 2 || gearStatus == 5)
+    if ((gearStatus >= 2 && gearStatus <= 19) || (gearStatus == 23 || gearStatus == 24))
       bvc.driveDirection = DriveDirection_forward;
-    else if (gearStatus == 3)
+    else if (gearStatus == 20 || gearStatus == 21)
       bvc.driveDirection = DriveDirection_backward;
     else
       bvc.driveDirection = DriveDirection_unavailable;
@@ -300,7 +370,8 @@ namespace v2x
     bvc.yawRate.yawRateConfidence = YawRateConfidence_unavailable;
     // ------------------------------
 
-    RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM from station with ID %ld", stationId_);
+    RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM from station with ID %ld with generationDeltaTime %ld, latitude %f, longitude %f, altitude %f",
+            stationId_, cam.generationDeltaTime, ego_.latitude, ego_.longitude, ego_.altitude);
     std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
     payload->layer(OsiLayer::Application) = std::move(message);
 
@@ -311,18 +382,64 @@ namespace v2x
 
     Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
 
+    cam_ts_CAM_t ts_cam;
+    std::memset(&ts_cam, 0, sizeof(ts_cam));
+    CamApplication::build_etsi_its_cam_ts_from_vanetza(message, ts_cam);
+    etsi_its_cam_ts_msgs::msg::CAM ros_msg;
+    std::memset(&ros_msg, 0, sizeof(ros_msg));
+    etsi_its_cam_ts_conversion::toRos_CAM(ts_cam, ros_msg);
+    node_->publishReceivedCam(ros_msg);
+
     if (!confirm.accepted()) {
       throw std::runtime_error("[CamApplication::send] CAM application data request failed");
     }
 
+    RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Successfully sent");
+
     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_;
   }
 
+  void CamApplication::build_etsi_its_cam_ts_from_vanetza(vanetza::asn1::Cam &in, cam_ts_CAM_t &out) {
+    cam_ts_ItsPduHeader_t &header = out.header;
+    header.protocolVersion = in->header.protocolVersion;
+    header.messageId = in->header.messageID;
+    header.stationId = in->header.stationID;
+
+    cam_ts_CamPayload_t &coopAwareness = out.cam;
+    coopAwareness.generationDeltaTime = in->cam.generationDeltaTime;
+
+    cam_ts_BasicContainer_t &basic_container = coopAwareness.camParameters.basicContainer;
+    BasicContainer_t &rec_basic_container = in->cam.camParameters.basicContainer;
+    basic_container.stationType = rec_basic_container.stationType;
+    basic_container.referencePosition.latitude = rec_basic_container.referencePosition.latitude;
+    basic_container.referencePosition.longitude = rec_basic_container.referencePosition.longitude;
+    basic_container.referencePosition.altitude.altitudeValue = rec_basic_container.referencePosition.altitude.altitudeValue;
+    basic_container.referencePosition.altitude.altitudeConfidence = rec_basic_container.referencePosition.altitude.altitudeConfidence;
+    basic_container.referencePosition.positionConfidenceEllipse.semiMajorAxisLength = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence;
+    basic_container.referencePosition.positionConfidenceEllipse.semiMinorAxisLength = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence;
+    basic_container.referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation;
+
+    cam_ts_BasicVehicleContainerHighFrequency_t &bvc = coopAwareness.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
+    coopAwareness.camParameters.highFrequencyContainer.present = cam_ts_HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
+    BasicVehicleContainerHighFrequency_t &rec_bvc = in->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
+    bvc.heading.headingValue = rec_bvc.heading.headingValue;
+    bvc.heading.headingConfidence = rec_bvc.heading.headingConfidence;
+    bvc.speed.speedValue = rec_bvc.speed.speedValue;
+    bvc.speed.speedConfidence = rec_bvc.speed.speedConfidence;
+    bvc.driveDirection = rec_bvc.driveDirection;
+    bvc.vehicleLength.vehicleLengthValue = rec_bvc.vehicleLength.vehicleLengthValue;
+    bvc.vehicleLength.vehicleLengthConfidenceIndication = rec_bvc.vehicleLength.vehicleLengthConfidenceIndication;
+    bvc.vehicleWidth = rec_bvc.vehicleWidth;
+    bvc.longitudinalAcceleration.value = rec_bvc.longitudinalAcceleration.longitudinalAccelerationValue;
+    bvc.longitudinalAcceleration.confidence = rec_bvc.longitudinalAcceleration.longitudinalAccelerationConfidence;
+    bvc.curvature.curvatureValue = rec_bvc.curvature.curvatureValue;
+    bvc.curvature.curvatureConfidence = rec_bvc.curvature.curvatureConfidence;
+    bvc.curvatureCalculationMode = rec_bvc.curvatureCalculationMode;
+    bvc.yawRate.yawRateValue = rec_bvc.yawRate.yawRateValue;
+    bvc.yawRate.yawRateConfidence = rec_bvc.yawRate.yawRateConfidence;
+  }
 }

src/cpm_application.cpp

@@ -101,7 +101,8 @@ namespace v2x {
 
 
       // Calculate GDT and get GDT from CPM and calculate the "Age of CPM"
-      TimestampIts_t gt_cpm = message->cpm.generationTime;
+      TimestampIts_t gt_cpm;
+      asn_long2INTEGER(&gt_cpm, (long) message->cpm.generationDeltaTime);
       // const auto time_now = duration_cast<milliseconds> (runtime_.now().time_since_epoch());
       // uint16_t gdt = time_now.count();
       // int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536;
@@ -439,7 +440,6 @@ namespace v2x {
   }
 
   void CpmApplication::send() {
-
     if (is_sender_) {
 
       sending_ = true;
@@ -448,6 +448,8 @@ namespace v2x {
 
       // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM...");
 
+      std::chrono::milliseconds now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
+
       vanetza::asn1::Cpm message;
 
       // ITS PDU Header
@@ -460,7 +462,7 @@ namespace v2x {
 
       // Set GenerationTime
       RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] %ld", gdt_timestamp_);
-      asn_long2INTEGER(&cpm.generationTime, (long) gdt_timestamp_);
+      cpm.generationDeltaTime = (now_ms.count() - 1072915200000) % 65536;
 
       CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer;
       management.stationType = StationType_passengerCar;

src/cube_evk_link.cpp

@@ -0,0 +1,148 @@
+#include "autoware_v2x/cube_evk_link.hpp"
+#include "cube_evk_radio.pb.h"
+
+#include <iostream>
+#include <vanetza/net/osi_layer.hpp>
+#include <vanetza/net/packet_variant.hpp>
+#include <vanetza/net/cohesive_packet.hpp>
+#include <vanetza/access/data_request.hpp>
+#include <vanetza/common/byte_view.hpp>
+#include <vanetza/access/ethertype.hpp>
+
+#include <boost/asio/placeholders.hpp>
+#include <boost/bind/bind.hpp>
+
+CubeEvkLink::CubeEvkLink(boost::asio::io_service& io, boost::asio::ip::address_v4 radio_ip)
+: io_(io), tx_socket_(io), rx_socket_(io)
+{
+  const boost::asio::ip::udp::endpoint radio_endpoint_tx(radio_ip, cube_evk_radio_port_tx);
+  tx_socket_.connect(radio_endpoint_tx);
+
+  boost::asio::ip::udp::endpoint radio_endpoint_rx(boost::asio::ip::udp::v4(), cube_evk_radio_port_rx);
+  rx_socket_.open(radio_endpoint_rx.protocol());
+  rx_socket_.bind(radio_endpoint_rx);
+
+  rx_socket_.async_receive_from(
+    boost::asio::buffer(received_data_), host_endpoint_,
+    boost::bind(&CubeEvkLink::handle_packet_received, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
+
+}
+
+void CubeEvkLink::handle_packet_received(const boost::system::error_code& ec, size_t bytes)
+{
+  if (!ec)
+  {
+    vanetza::ByteBuffer buf(received_data_.begin(), received_data_.begin() + bytes);
+
+    GossipMessage gossipMessage;
+    gossipMessage.ParseFromArray(buf.data(), buf.size());
+
+    switch (gossipMessage.kind_case())
+    {
+      case GossipMessage::KindCase::kCbr:
+      {
+        // got CBR; use this for your DCC
+        // const ChannelBusyRatio& cbr = gossipMessage.cbr();
+        // vanetza::dcc::ChannelLoad(cbr.busy(), cbr.total())
+        break;
+      }
+      case GossipMessage::KindCase::kLinklayerRx:
+      {
+        pass_message_to_router(std::unique_ptr<LinkLayerReception>{gossipMessage.release_linklayer_rx()});
+        break;
+      }
+      default:
+      {
+        std::cerr << "Received GossipMessage of unknown kind " << gossipMessage.kind_case() << std::endl;
+      }
+    }
+
+    rx_socket_.async_receive_from(
+      boost::asio::buffer(received_data_), host_endpoint_,
+      boost::bind(&CubeEvkLink::handle_packet_received, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
+
+  }
+  else
+  {
+    std::cerr << "CubeEvkLink::handle_packet_received went wrong: " << ec << std::endl;
+  }
+
+}
+
+void CubeEvkLink::request(const vanetza::access::DataRequest& request, std::unique_ptr<vanetza::ChunkPacket> packet)
+{
+  CommandRequest command;
+  command.set_allocated_linklayer_tx(create_link_layer_tx(request, std::move(packet)).release());
+
+  std::string serializedTransmission;
+  command.SerializeToString(&serializedTransmission);
+  tx_socket_.send(boost::asio::buffer(serializedTransmission));
+}
+
+void CubeEvkLink::indicate(IndicationCallback callback)
+{
+  indicate_to_router_ = callback;
+}
+
+void CubeEvkLink::pass_message_to_router(std::unique_ptr<LinkLayerReception> packet)
+{
+  if (packet->source().size() != vanetza::MacAddress::length_bytes)
+  {
+    std::cerr << "received packet's source MAC address is invalid" << std::endl;
+  }
+  else if (packet->destination().size() != vanetza::MacAddress::length_bytes)
+  {
+    std::cerr << "received packet's destination MAC address is invalid"  << std::endl;
+  }
+  else
+  {
+    vanetza::EthernetHeader ethernet_header;
+    std::copy_n(packet->source().begin(), vanetza::MacAddress::length_bytes, ethernet_header.source.octets.begin());
+    std::copy_n(packet->destination().begin(), vanetza::MacAddress::length_bytes, ethernet_header.destination.octets.begin());
+    ethernet_header.type = vanetza::access::ethertype::GeoNetworking;
+
+    vanetza::ByteBuffer buffer(packet->payload().begin(), packet->payload().end());
+    vanetza::CohesivePacket packet(std::move(buffer), vanetza::OsiLayer::Network);
+
+    indicate_to_router_(std::move(packet), ethernet_header);
+  }
+}
+
+std::unique_ptr<LinkLayerTransmission> CubeEvkLink::create_link_layer_tx(const vanetza::access::DataRequest& req, std::unique_ptr<vanetza::ChunkPacket> packet)
+{
+  using namespace vanetza;
+
+  std::unique_ptr<LinkLayerTransmission> transmission{new LinkLayerTransmission()};
+  transmission->set_source(req.source_addr.octets.data(), req.source_addr.octets.size());
+  transmission->set_destination(req.destination_addr.octets.data(), req.destination_addr.octets.size());
+
+  LinkLayerPriority prio = LinkLayerPriority::BEST_EFFORT;
+  switch (req.access_category)
+  {
+    case access::AccessCategory::VO:
+      prio = LinkLayerPriority::VOICE;
+      break;
+    case access::AccessCategory::VI:
+      prio = LinkLayerPriority::VIDEO;
+      break;
+    case access::AccessCategory::BE:
+      prio = LinkLayerPriority::BEST_EFFORT;
+      break;
+    case access::AccessCategory::BK:
+      prio = LinkLayerPriority::BACKGROUND;
+      break;
+    default:
+      std::cerr << "Unknown access category requested, falling back to best effort!" << std::endl;
+      break;
+  }
+  transmission->set_priority(prio);
+
+  std::string* payload = transmission->mutable_payload();
+  for (auto& layer : osi_layer_range<OsiLayer::Network, OsiLayer::Application>())
+  {
+    auto byte_view = create_byte_view(packet->layer(layer));
+    payload->append(byte_view.begin(), byte_view.end());
+  }
+
+  return transmission;
+}

src/cube_evk_radio.proto

@@ -0,0 +1,78 @@
+syntax = "proto2";
+
+message CommandRequest {
+  oneof kind {
+    LifecycleAction lifecycle = 1;
+    LinkLayerTransmission linklayer_tx = 2;
+    RadioConfiguration radio_cfg = 3;
+  }
+}
+
+message CommandResponse {
+  enum Status {
+    SUCCESS = 0;
+    FAILURE = 1;
+    UNKNOWN = 2;
+    NOT_IMPLEMENTED = 3;
+  }
+
+  required Status status = 1;
+  optional string message = 2;
+  optional CommandResponseData data = 3;
+}
+
+message CommandResponseData {
+  oneof kind {
+    RadioConfiguration radio_cfg = 1;
+  }
+}
+
+enum LifecycleAction {
+  SOFT_RESET = 0;
+  HARD_RESET = 1;
+}
+
+enum LinkLayerPriority {
+  BACKGROUND = 0;
+  BEST_EFFORT = 1;
+  VIDEO = 2;
+  VOICE = 3;
+}
+
+message RadioConfiguration {
+  optional bytes address = 1;
+  optional uint32 channel_frequency_mhz = 2;
+  optional bool filter_unicast_destination = 3;
+  optional sint32 default_tx_power_cbm = 4;
+  optional uint32 default_tx_datarate_500kbps = 5;
+}
+
+message LinkLayerTransmission {
+  optional bytes source = 1;
+  required bytes destination = 2;
+  required LinkLayerPriority priority = 3;
+  optional uint32 channel = 4;
+  optional uint32 datarate_500kbps = 5;
+  optional sint32 power_cbm = 6; // centi Bel mW = 10 * dBm
+  required bytes payload = 10;
+}
+
+message LinkLayerReception {
+  required bytes source = 1;
+  required bytes destination = 2;
+  optional uint32 channel = 4;
+  optional sint32 power_cbm = 6;
+  required bytes payload = 10;
+}
+
+message ChannelBusyRatio {
+  required uint32 busy = 1;
+  required uint32 total = 2;
+}
+
+message GossipMessage {
+  oneof kind {
+    ChannelBusyRatio cbr = 1;
+    LinkLayerReception linklayer_rx = 2;
+  }
+}

src/link_layer.cpp

@@ -1,10 +1,11 @@
 #include "autoware_v2x/link_layer.hpp"
+#include "autoware_v2x/cube_evk_link.hpp"
 #include <boost/asio/generic/raw_protocol.hpp>
 #include <vanetza/access/ethertype.hpp>
 #include "autoware_v2x/raw_socket_link.hpp"
 
 std::unique_ptr<LinkLayer> create_link_layer(
-  boost::asio::io_service & io_service, const EthernetDevice & device, const std::string & name)
+  boost::asio::io_service &io_service, const EthernetDevice &device, const std::string &name, const std::string &cube_ip)
 {
   std::unique_ptr<LinkLayer> link_layer;
 
@@ -14,6 +15,8 @@ std::unique_ptr<LinkLayer> create_link_layer(
     boost::asio::generic::raw_protocol::socket raw_socket(io_service, raw_protocol);
     raw_socket.bind(device.endpoint(AF_PACKET));
     link_layer.reset(new RawSocketLink{std::move(raw_socket)});
+  } else if (name == "cube-evk") {
+    link_layer.reset(new CubeEvkLink{ io_service, boost::asio::ip::address_v4::from_string(cube_ip) });
   }
 
   // Removed Cohda and UDP Support

src/security.cpp

@@ -1,13 +1,27 @@
+/**
+ * From Vanetza (https://github.com/riebl/Vanetza)
+ * Using Security V3
+ *
+ * https://github.com/riebl/vanetza/tree/master/vanetza/security
+ * https://github.com/riebl/vanetza/tree/master/tools/socktap/security.cpp
+ */
+
 #include "autoware_v2x/security.hpp"
-#include <vanetza/security/certificate_cache.hpp>
-#include <vanetza/security/default_certificate_validator.hpp>
+
+#include <memory>
 #include <vanetza/security/delegating_security_entity.hpp>
-#include <vanetza/security/naive_certificate_provider.hpp>
-#include <vanetza/security/null_certificate_validator.hpp>
-#include <vanetza/security/persistence.hpp>
-#include <vanetza/security/sign_header_policy.hpp>
-#include <vanetza/security/static_certificate_provider.hpp>
-#include <vanetza/security/trust_store.hpp>
+#include <vanetza/security/straight_verify_service.hpp>
+#include "vanetza/security/v2/certificate_provider.hpp"
+#include <vanetza/security/v2/certificate_cache.hpp>
+#include <vanetza/security/v2/default_certificate_validator.hpp>
+#include <vanetza/security/v2/sign_service.hpp>
+#include <vanetza/security/v3/certificate_cache.hpp>
+#include <vanetza/security/v3/naive_certificate_provider.hpp>
+#include <vanetza/security/v3/persistence.hpp>
+#include <vanetza/security/v3/sign_header_policy.hpp>
+#include <vanetza/security/v3/sign_service.hpp>
+#include <vanetza/security/v3/static_certificate_provider.hpp>
+
 #include <stdexcept>
 
 using namespace vanetza;
@@ -16,18 +30,18 @@ namespace po = boost::program_options;
 class SecurityContext : public security::SecurityEntity
 {
 public:
-  SecurityContext(const Runtime &runtime, PositionProvider &positioning) : runtime(runtime), positioning(positioning),
-                                                                           backend(security::create_backend("default")),
-                                                                           sign_header_policy(runtime, positioning),
-                                                                           cert_cache(runtime),
-                                                                           cert_validator(*backend, cert_cache, trust_store)
+  SecurityContext(const Runtime &runtime, PositionProvider &positioning)
+  : runtime(runtime),
+    positioning(positioning),
+    backend(security::create_backend("default")),
+    sign_header_policy(runtime, positioning),
+    cert_cache()
   {
   }
 
   security::EncapConfirm encapsulate_packet(security::EncapRequest &&request) override
   {
-    if (!entity)
-    {
+    if (!entity) {
       throw std::runtime_error("security entity is not ready");
     }
     return entity->encapsulate_packet(std::move(request));
@@ -35,8 +49,7 @@ public:
 
   security::DecapConfirm decapsulate_packet(security::DecapRequest &&request) override
   {
-    if (!entity)
-    {
+    if (!entity) {
       throw std::runtime_error("security entity is not ready");
     }
     return entity->decapsulate_packet(std::move(request));
@@ -44,31 +57,78 @@ public:
 
   void build_entity()
   {
-    if (!cert_provider)
-    {
+    if (!cert_provider) {
       throw std::runtime_error("certificate provider is missing");
     }
-    security::SignService sign_service = straight_sign_service(*cert_provider, *backend, sign_header_policy);
-    security::VerifyService verify_service = straight_verify_service(runtime, *cert_provider, cert_validator,
-                                                                     *backend, cert_cache, sign_header_policy, positioning);
-    entity.reset(new security::DelegatingSecurityEntity{sign_service, verify_service});
+    std::unique_ptr<security::SignService> sign_service{new security::v3::StraightSignService(*cert_provider, *backend, sign_header_policy)};
+    std::unique_ptr<security::StraightVerifyService> verify_service{new security::StraightVerifyService(runtime, *backend, positioning)};
+    verify_service->use_certificate_cache(&cert_cache);
+    entity = std::make_unique<security::DelegatingSecurityEntity>(std::move(sign_service), std::move(verify_service));
   }
 
-  const Runtime &runtime;
-  PositionProvider &positioning;
+  const Runtime & runtime;
+  PositionProvider & positioning;
   std::unique_ptr<security::Backend> backend;
   std::unique_ptr<security::SecurityEntity> entity;
-  std::unique_ptr<security::CertificateProvider> cert_provider;
-  security::DefaultSignHeaderPolicy sign_header_policy;
-  security::TrustStore trust_store;
-  security::CertificateCache cert_cache;
-  security::DefaultCertificateValidator cert_validator;
+  std::unique_ptr<security::v3::CertificateProvider> cert_provider;
+  security::v3::DefaultSignHeaderPolicy sign_header_policy;
+  security::v3::CertificateCache cert_cache;
 };
 
-std::unique_ptr<security::SecurityEntity>
-create_security_entity(const Runtime &runtime, PositionProvider &positioning)
+std::unique_ptr<security::v3::CertificateProvider> load_certificates(
+  const std::string &cert_path, const std::string &cert_key_path,
+  const std::vector<std::string> &cert_chain_path, security::v3::CertificateCache &cert_cache)
+{
+  auto authorization_ticket = security::v3::load_certificate_from_file(cert_path);
+  auto authorization_ticket_key = security::v3::load_private_key_from_file(cert_key_path);
+
+  std::list<security::v3::Certificate> chain;
+  for (auto & chain_path : cert_chain_path) {
+    auto chain_certificate = security::v3::load_certificate_from_file(chain_path);
+    chain.push_back(chain_certificate);
+    cert_cache.store(chain_certificate);
+  }
+
+  return std::make_unique<security::v3::StaticCertificateProvider>(authorization_ticket, authorization_ticket_key.private_key, chain);
+}
+
+std::unique_ptr<security::SecurityEntity> create_security_entity(const po::variables_map &options, const Runtime &runtime, PositionProvider &positioning)
 {
   std::unique_ptr<security::SecurityEntity> security;
+  const std::string name = options["security"].as<std::string>();
+
+  if (name.empty() || name == "none") {
+    // no operation
+  } else if (name == "certs") {
+    if (options.count("certificate") ^ options.count("certificate-key")) {
+      throw std::runtime_error("Either certificate and certificate-key must be present or none.");
+    }
+
+    if (options.count("certificate") && options.count("certificate-key")) {
+      const auto &cert_path = options["certificate"].as<std::string>();
+      const auto &cert_key_path = options["certificate-key"].as<std::string>();
+      std::vector<std::string> chain_paths;
+      if (options.count("certificate-chain")) {
+        chain_paths = options["certificate-chain"].as<std::vector<std::string>>();
+      }
+
+      auto context = std::make_unique<SecurityContext>(runtime, positioning);
+      context->cert_provider = load_certificates(cert_path, cert_key_path, chain_paths, context->cert_cache);
+      context->build_entity();
+      security = std::move(context);
+    } else {
+      auto context = std::make_unique<SecurityContext>(runtime, positioning);
+      context->cert_provider = std::make_unique<security::v3::NaiveCertificateProvider>(runtime);
+      context->build_entity();
+      security = std::move(context);
+    }
+
+    if (!security) {
+      throw std::runtime_error("internal failure setting up security entity");
+    }
+  } else {
+    throw std::runtime_error("Unknown security entity requested");
+  }
 
   return security;
 }

src/v2x_app.cpp

@@ -24,6 +24,7 @@
 #include <boost/date_time/posix_time/posix_time.hpp>
 
 namespace gn = vanetza::geonet;
+namespace po = boost::program_options;
 
 using namespace vanetza;
 using namespace std::chrono;
@@ -50,31 +51,22 @@ namespace v2x
     }
   }
 
-  void V2XApp::setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &msg) {
-    if (cam_started_)
-      cam->setVehicleDimensions(msg);
-    else
-      this->setVehicleDimensions(msg);
-  }
-
   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_) {
+    if (cam_started_)
       cam->updateVelocityReport(msg);
-    }
   }
 
-  void V2XApp::vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
-    if (!vehicle_status_received_) {
-      RCLCPP_WARN(node_->get_logger(), "[V2XApp::vehicleStatusCallback] VehicleStatus not received yet");
-    }
-    if (vehicle_status_received_ && cam_started_) {
-      cam->updateVehicleStatus(msg);
-    }
+  void V2XApp::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
+    if (cam_started_)
+      cam->updateGearReport(msg);
   }
 
+  void V2XApp::steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
+    if (cam_started_)
+      cam->updateSteeringReport(msg);
+  }
+
+
   void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
 
     tf_received_ = true;
@@ -128,7 +120,6 @@ namespace v2x
       cam->updateMGRS(&x, &y);
       cam->updateRP(&lat, &lon, &z);
       cam->updateHeading(&yaw);
-      cam->updateGenerationDeltaTime(&gdt, &timestamp_msec);
     }
   }
 
@@ -138,6 +129,10 @@ namespace v2x
     boost::asio::io_service io_service;
     TimeTrigger trigger(io_service);
 
+    std::string link_layer_name;
+    node_->get_parameter("link_layer", link_layer_name);
+    RCLCPP_INFO(node_->get_logger(), "Link Layer: %s", link_layer_name.c_str());
+
     std::string network_interface;
     node_->get_parameter("network_interface", network_interface);
     RCLCPP_INFO(node_->get_logger(), "Network Interface: %s", network_interface.c_str());
@@ -157,10 +152,38 @@ namespace v2x
     mib.itsGnSecurity = false;
     mib.itsGnProtocolVersion = 1;
 
+    std::string cube_ip;
+    node_->get_parameter("cube_ip", cube_ip);
+    RCLCPP_INFO(node_->get_logger(), "CubeEVK IP: %s", cube_ip.c_str());
+
     // Create raw socket on device and LinkLayer object
-    auto link_layer =  create_link_layer(io_service, device, "ethernet");
+    auto link_layer =  create_link_layer(io_service, device, link_layer_name, cube_ip);
     auto positioning = create_position_provider(io_service, trigger.runtime());
-    auto security = create_security_entity(trigger.runtime(), *positioning);
+
+    po::variables_map security_options;
+    std::string entity;
+    std::string certificate;
+    std::string certificate_key;
+    std::vector<std::string> certificate_chain;
+    // Grab the values from the node parameters
+    node_->get_parameter("security", entity);
+    security_options.insert(std::make_pair("security", po::variable_value(entity, false)));
+    if (node_->has_parameter("certificate")) {
+      node_->get_parameter("certificate", certificate);
+      security_options.insert(std::make_pair("certificate", po::variable_value(certificate, false)));
+    }
+    if (node_->has_parameter("certificate-key")) {
+      node_->get_parameter("certificate-key", certificate_key);
+      security_options.insert(std::make_pair("certificate-key", po::variable_value(certificate_key, false)));
+    }
+    if (node_->has_parameter("certificate-chain")) {
+      node_->get_parameter("certificate-chain", certificate_chain);
+      security_options.insert(std::make_pair("certificate-chain", po::variable_value(certificate_chain, false)));
+    }
+    auto security = create_security_entity(security_options, trigger.runtime(), *positioning);
+
+    RCLCPP_INFO(node_->get_logger(), "Security layer: %s", entity == "certs" ? "Certificates" : "None");
+
     RouterContext context(mib, trigger, *positioning, security.get());
 
     context.set_link_layer(link_layer.get());

src/v2x_node.cpp

@@ -36,26 +36,35 @@ namespace v2x
   V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) {
     using std::placeholders::_1;
 
+    int timeout = 10;
+    get_vehicle_dimensions_ = this->create_client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>("/api/vehicle/dimensions");
+    if (get_vehicle_dimensions_->wait_for_service(std::chrono::seconds(timeout))) {
+      RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] Service /api/vehicle/dimensions is now available.");
+      vehicle_dimensions_available_ = true;
+    } else {
+      RCLCPP_ERROR(get_logger(), "[V2XNode::V2XNode] Service /api/vehicle/dimensions is not available after waiting (timeout=%ds).", timeout);
+      vehicle_dimensions_available_ = false;
+    }
+
     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));
-    vehicle_status_sub_ = this->create_subscription<autoware_adapi_v1_msgs::msg::VehicleStatus>("/api/vehicle/status", 10, std::bind(&V2XNode::vehicleStatucCallback, this, _1));
-    get_vehicle_dimensions_ = this->create_client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>("/api/vehicle/dimensions");
-    if (get_vehicle_dimensions_->wait_for_service(std::chrono::seconds(60))) {
-      RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is now available.");
-      this->getVehicleDimensions();
-    } else {
-      RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is not available after waiting (timeout=60s).");
-    }
+    gear_report_sub_ = this->create_subscription<autoware_auto_vehicle_msgs::msg::GearReport>("/vehicle/status/gear_status", 10, std::bind(&V2XNode::gearReportCallback, this, _1));
+    steering_report_sub_ = this->create_subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>("/vehicle/status/steering_status", 10, std::bind(&V2XNode::steeringReportCallback, 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});
 
+    cam_rec_pub_ = create_publisher<etsi_its_cam_ts_msgs::msg::CAM>("/v2x/cam_ts/received", rclcpp::QoS{10});
+
     // Declare Parameters
-    this->declare_parameter<std::string>("network_interface", "vmnet1");
+    this->declare_parameter<std::string>("link_layer", "ethernet");
+    this->declare_parameter<std::string>("network_interface", "v2x_testing");
+    this->declare_parameter<std::string>("cube_ip", "127.0.0.1");
     this->declare_parameter<bool>("is_sender", true);
+    this->declare_parameter<std::string>("security", "none");
 
     // Launch V2XApp in a new thread
     app = new V2XApp(this);
@@ -166,6 +175,11 @@ namespace v2x
     cpm_objects_pub_->publish(output_dynamic_object_msg);
   }
 
+  void V2XNode::publishReceivedCam(etsi_its_cam_ts_msgs::msg::CAM &msg) {
+    RCLCPP_INFO(get_logger(), "Publishing CAM to ROS network");
+    cam_rec_pub_->publish(msg);
+  }
+
   void V2XNode::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) {
     rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg.
 
@@ -185,33 +199,37 @@ namespace v2x
     app->velocityReportCallback(msg);
   }
 
-  void V2XNode::vehicleStatucCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
-    app->vehicleStatusCallback(msg);
+  void V2XNode::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
+    app->gearReportCallback(msg);
+  }
+
+  void V2XNode::steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
+    app->steeringReportCallback(msg);
   }
 
   void V2XNode::getVehicleDimensions() {
+    if (!vehicle_dimensions_available_) {
+      RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is not available.");
+      return;
+    }
     RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Sending service request to /api/vehicle/dimensions");
     auto request = std::make_shared<autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Request>();
     auto future_result = get_vehicle_dimensions_->async_send_request(request, [this](rclcpp::Client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>::SharedFuture future) {
-     try
-     {
-       auto response = future.get();
-       RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received response from /api/vehicle/dimensions");
-       try {
-         auto dimensions = response->dimensions;
-         if (dimensions.height == 0 || dimensions.wheel_base == 0 || dimensions.wheel_tread == 0)
-           this->getVehicleDimensions();
-         else
-           app->setVehicleDimensions(dimensions);
-       } catch (const std::exception &e) {
-         RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service response of /api/vehicle/dimensions failed: %s", e.what());
-       }
-     }
-     catch (const std::exception &e)
-     {
-       RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service call of /api/vehicle/dimensions failed: %s", e.what());
-     }
-   });
+      try {
+        auto response = future.get();
+        RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received response from /api/vehicle/dimensions");
+        try {
+          auto dimensions = response->dimensions;
+          RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received vehicle dimensions: height=%f, wheel_base=%f, wheel_tread=%f", dimensions.height, dimensions.wheel_base, dimensions.wheel_tread);
+          app->cam->setVehicleDimensions(dimensions);
+        } catch (const std::exception &e) {
+          RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service response of /api/vehicle/dimensions failed: %s", e.what());
+        }
+      }
+      catch (const std::exception &e) {
+        RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service call of /api/vehicle/dimensions failed: %s", e.what());
+      }
+    });
   }
 }