Add link layer support for CubeEVK (by nfiniity)

- Change config file
- Some other small fixes

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

CMakeLists.txt

@@ -27,9 +27,11 @@ find_package(etsi_its_cam_ts_coding REQUIRED)
 find_package(etsi_its_cam_ts_conversion 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
@@ -38,6 +40,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
@@ -48,7 +51,9 @@ ament_auto_add_library(autoware_v2x SHARED
   src/security.cpp
 )
 
-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_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,6 +1,8 @@
 /**:
   ros__parameters:
+    link_layer: "ethernet"
     network_interface: "v2x_testing"
+    cube_ip: "127.0.0.1"
     is_sender: true
     security: "none"
     certificate: ""

include/autoware_v2x/cam_application.hpp

@@ -36,7 +36,7 @@ private:
 
   V2XNode *node_;
   vanetza::Runtime &runtime_;
-  vanetza::Clock::duration cam_interval_;
+  vanetza::Clock::duration cam_interval_{};
 
   struct VehicleDimensions {
     float wheel_radius;
@@ -77,17 +77,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 +109,9 @@ private:
     PositionsDeque x;
     PositionsDeque y;
 
-    double semiMajorConfidence;
+    double semiMajorConfidence{};
-    double semiMinorConfidence;
+    double semiMinorConfidence{};
-    double semiMajorOrientation;
+    double semiMajorOrientation{};
   };
   PositionConfidenceEllipse positionConfidenceEllipse_;
 
@@ -131,7 +131,7 @@ private:
   VehicleStatus vehicleStatus_;
 
   int generationDeltaTime_;
-  long gdt_timestamp_;
+  long gdt_timestamp_{};
 
   double objectConfidenceThreshold_;
 

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 */
-

src/cam_application.cpp

@@ -30,9 +30,6 @@
 
 #include <sqlite3.h>
 
-#define _USE_MATH_DEFINES
-#include <math.h>
-
 using namespace vanetza;
 using namespace std::chrono;
 
@@ -41,24 +38,25 @@ namespace v2x
   CamApplication::CamApplication(V2XNode * node, Runtime & rt, bool is_sender)
   : node_(node),
     runtime_(rt),
+    cam_interval_(milliseconds(100)),
     vehicleDimensions_(),
     ego_(),
     positionConfidenceEllipse_(),
     velocityReport_(),
     vehicleStatus_(),
     generationDeltaTime_(0),
+    objectConfidenceThreshold_(0.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)
   {
     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
@@ -190,13 +188,12 @@ namespace v2x
     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;
+    float longitudinal_acceleration = (msg->longitudinal_velocity - velocityReport_.longitudinal_velocity) / dt;
-    if (dt != 0) longitudinal_acceleration = (msg->longitudinal_velocity - velocityReport_.longitudinal_velocity) / dt;
 
     velocityReport_.stamp = msg->header.stamp;
     velocityReport_.heading_rate = msg->heading_rate;

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/v2x_app.cpp

@@ -139,6 +139,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());
@@ -158,8 +162,12 @@ 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());
 
     po::variables_map security_options;

src/v2x_node.cpp

@@ -56,7 +56,9 @@ namespace v2x
     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>("link_layer", "cube-evk");
     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");