From 6a1a940cb2134b0ff05900a4736ea2aae03c5aa9 Mon Sep 17 00:00:00 2001 From: Tiago Garcia Date: Wed, 4 Sep 2024 16:18:20 +0100 Subject: [PATCH] Add link layer support for CubeEVK (by nfiniity) - Change config file - Some other small fixes Signed-off-by: Tiago Garcia --- CMakeLists.txt | 7 +- config/autoware_v2x.param.yaml | 2 + include/autoware_v2x/cam_application.hpp | 16 +-- include/autoware_v2x/cube_evk_link.hpp | 43 +++++++ include/autoware_v2x/link_layer.hpp | 3 +- src/cam_application.cpp | 13 +- src/cube_evk_link.cpp | 148 +++++++++++++++++++++++ src/cube_evk_radio.proto | 78 ++++++++++++ src/link_layer.cpp | 5 +- src/v2x_app.cpp | 10 +- src/v2x_node.cpp | 2 + 11 files changed, 306 insertions(+), 21 deletions(-) create mode 100644 include/autoware_v2x/cube_evk_link.hpp create mode 100644 src/cube_evk_link.cpp create mode 100644 src/cube_evk_radio.proto diff --git a/CMakeLists.txt b/CMakeLists.txt index 46cc59f..3561185 100644 --- a/CMakeLists.txt +++ b/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" diff --git a/config/autoware_v2x.param.yaml b/config/autoware_v2x.param.yaml index a1dd8c5..5aaec51 100644 --- a/config/autoware_v2x.param.yaml +++ b/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: "" diff --git a/include/autoware_v2x/cam_application.hpp b/include/autoware_v2x/cam_application.hpp index 80df55a..eccbb4e 100644 --- a/include/autoware_v2x/cam_application.hpp +++ b/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::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 semiMinorConfidence; - double semiMajorOrientation; + double semiMajorConfidence{}; + double semiMinorConfidence{}; + double semiMajorOrientation{}; }; PositionConfidenceEllipse positionConfidenceEllipse_; @@ -131,7 +131,7 @@ private: VehicleStatus vehicleStatus_; int generationDeltaTime_; - long gdt_timestamp_; + long gdt_timestamp_{}; double objectConfidenceThreshold_; diff --git a/include/autoware_v2x/cube_evk_link.hpp b/include/autoware_v2x/cube_evk_link.hpp new file mode 100644 index 0000000..fccec04 --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include + +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) 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 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); + std::unique_ptr create_link_layer_tx(const vanetza::access::DataRequest&, std::unique_ptr); +}; + +#endif /* CUBE_EVK_LINK_HPP_ */ diff --git a/include/autoware_v2x/link_layer.hpp b/include/autoware_v2x/link_layer.hpp index be24135..ce29b79 100644 --- a/include/autoware_v2x/link_layer.hpp +++ b/include/autoware_v2x/link_layer.hpp @@ -23,7 +23,6 @@ class LinkLayer : public vanetza::access::Interface, public LinkLayerIndication }; std::unique_ptr -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 */ - diff --git a/src/cam_application.cpp b/src/cam_application.cpp index 6faef39..231e4cd 100644 --- a/src/cam_application.cpp +++ b/src/cam_application.cpp @@ -30,9 +30,6 @@ #include -#define _USE_MATH_DEFINES -#include - 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; - if (dt != 0) longitudinal_acceleration = (msg->longitudinal_velocity - velocityReport_.longitudinal_velocity) / dt; + float longitudinal_acceleration = (msg->longitudinal_velocity - velocityReport_.longitudinal_velocity) / dt; velocityReport_.stamp = msg->header.stamp; velocityReport_.heading_rate = msg->heading_rate; diff --git a/src/cube_evk_link.cpp b/src/cube_evk_link.cpp new file mode 100644 index 0000000..a5b1857 --- /dev/null +++ b/src/cube_evk_link.cpp @@ -0,0 +1,148 @@ +#include "autoware_v2x/cube_evk_link.hpp" +#include "cube_evk_radio.pb.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +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{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 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 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 CubeEvkLink::create_link_layer_tx(const vanetza::access::DataRequest& req, std::unique_ptr packet) +{ + using namespace vanetza; + + std::unique_ptr 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()) + { + auto byte_view = create_byte_view(packet->layer(layer)); + payload->append(byte_view.begin(), byte_view.end()); + } + + return transmission; +} diff --git a/src/cube_evk_radio.proto b/src/cube_evk_radio.proto new file mode 100644 index 0000000..97a4e82 --- /dev/null +++ b/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; + } +} diff --git a/src/link_layer.cpp b/src/link_layer.cpp index 21c823a..c23e9ba 100644 --- a/src/link_layer.cpp +++ b/src/link_layer.cpp @@ -1,10 +1,11 @@ #include "autoware_v2x/link_layer.hpp" +#include "autoware_v2x/cube_evk_link.hpp" #include #include #include "autoware_v2x/raw_socket_link.hpp" std::unique_ptr 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 link_layer; @@ -14,6 +15,8 @@ std::unique_ptr 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 diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index 5701b99..57f466c 100644 --- a/src/v2x_app.cpp +++ b/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; diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index 72050bb..8b3cb7a 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -56,7 +56,9 @@ namespace v2x cam_rec_pub_ = create_publisher("/v2x/cam_ts/received", rclcpp::QoS{10}); // Declare Parameters + this->declare_parameter("link_layer", "cube-evk"); this->declare_parameter("network_interface", "v2x_testing"); + this->declare_parameter("cube_ip", "127.0.0.1"); this->declare_parameter("is_sender", true); this->declare_parameter("security", "none");