From 049057fdf27ee52b02671e94f74b2f83ec8b1bca Mon Sep 17 00:00:00 2001 From: Tiago Garcia Date: Fri, 22 Nov 2024 23:27:30 +0000 Subject: [PATCH] Add cohda support Signed-off-by: Tiago Garcia --- CMakeLists.txt | 30 ++++++++++- config/autoware_v2x.param.yaml | 5 +- include/autoware_v2x/cohda_link.hpp | 17 ++++++ include/autoware_v2x/link_layer.hpp | 4 +- launch/v2x.launch.xml | 4 +- src/cohda_link.cpp | 81 +++++++++++++++++++++++++++++ src/link_layer.cpp | 45 +++++++++++----- src/v2x_app.cpp | 52 +++++++++++++----- src/v2x_node.cpp | 3 +- 9 files changed, 205 insertions(+), 36 deletions(-) create mode 100644 include/autoware_v2x/cohda_link.hpp create mode 100644 src/cohda_link.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 3561185..7002759 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,7 +34,7 @@ include_directories( ${CMAKE_CURRENT_BINARY_DIR}/src ) -ament_auto_add_library(autoware_v2x SHARED +set(SOURCES src/v2x_node.cpp src/v2x_app.cpp src/application.cpp @@ -51,6 +51,34 @@ ament_auto_add_library(autoware_v2x SHARED src/security.cpp ) +if(DEFINED BUILD_COHDA) + set(SEARCH_PATHS + "/usr/include/linux/cohda/llc" + "/usr/include/cohda/llc" + ) + find_file(COHDA_LLC_API_HEADER llc-api.h + PATHS ${SEARCH_PATHS} + CMAKE_FIND_ROOT_PATH_BOTH + DOC "Cohda LLC API header" + ) + mark_as_advanced(COHDA_LLC_API_HEADER) + if(COHDA_LLC_API_HEADER) + get_filename_component(COHDA_LLC_INCLUDE_DIR ${COHDA_LLC_API_HEADER} DIRECTORY) + include_directories( + include + ${CMAKE_CURRENT_BINARY_DIR}/src + ${COHDA_LLC_INCLUDE_DIR} + ) + list(APPEND SOURCES src/cohda_link.cpp) + add_compile_definitions(BUILD_COHDA) + else() + string(REPLACE ";" " " SEARCH_PATHS_STR "${SEARCH_PATHS}") + message(WARNING "Cohda LLC API header not found, could not enable Cohda support.\nSearched in: ${SEARCH_PATHS_STR}") + endif() +endif() + +ament_auto_add_library(autoware_v2x SHARED ${SOURCES}) + 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) diff --git a/config/autoware_v2x.param.yaml b/config/autoware_v2x.param.yaml index a111343..dd57a18 100644 --- a/config/autoware_v2x.param.yaml +++ b/config/autoware_v2x.param.yaml @@ -1,8 +1,7 @@ /**: ros__parameters: - link_layer: "cube-evk" - network_interface: "v2x_testing" - cube_ip: "192.168.94.84" + link_layer: "ethernet" # "ethernet" | "cube-evk" | "cohda" + target_device: "lo" # Should be the IP address of the target device or the network interface name is_sender: true publish_own_cams: false security: "none" diff --git a/include/autoware_v2x/cohda_link.hpp b/include/autoware_v2x/cohda_link.hpp new file mode 100644 index 0000000..32978f3 --- /dev/null +++ b/include/autoware_v2x/cohda_link.hpp @@ -0,0 +1,17 @@ +#ifndef COHDA_LINK_HPP_IXOCQ5RH +#define COHDA_LINK_HPP_IXOCQ5RH + +#include "raw_socket_link.hpp" + +class CohdaLink : public RawSocketLink +{ +public: + using RawSocketLink::RawSocketLink; + + void request(const vanetza::access::DataRequest&, std::unique_ptr) override; + +protected: + boost::optional parse_ethernet_header(vanetza::CohesivePacket&) const override; +}; + +#endif /* COHDA_LINK_HPP_IXOCQ5RH */ \ No newline at end of file diff --git a/include/autoware_v2x/link_layer.hpp b/include/autoware_v2x/link_layer.hpp index ce29b79..36751a3 100644 --- a/include/autoware_v2x/link_layer.hpp +++ b/include/autoware_v2x/link_layer.hpp @@ -22,7 +22,7 @@ class LinkLayer : public vanetza::access::Interface, public LinkLayerIndication { }; -std::unique_ptr -create_link_layer(boost::asio::io_service&, const EthernetDevice&, const std::string&, const std::string&); +std::unique_ptr create_link_layer(boost::asio::io_service &io_service, const std::string &name, const EthernetDevice &target_device); +std::unique_ptr create_link_layer(boost::asio::io_service &io_service, const std::string &name, const std::string &target_device); #endif /* LINK_LAYER_HPP_FGEK0QTH */ diff --git a/launch/v2x.launch.xml b/launch/v2x.launch.xml index 9e9fc01..8641b65 100644 --- a/launch/v2x.launch.xml +++ b/launch/v2x.launch.xml @@ -1,8 +1,8 @@ - - + + diff --git a/src/cohda_link.cpp b/src/cohda_link.cpp new file mode 100644 index 0000000..95c1b9e --- /dev/null +++ b/src/cohda_link.cpp @@ -0,0 +1,81 @@ +#include +#include +#include "autoware_v2x/cohda_link.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace vanetza; + +void CohdaLink::request(const vanetza::access::DataRequest& request, std::unique_ptr packet) +{ + access::G5LinkLayer link_layer; + access::ieee802::dot11::QosDataHeader& mac_header = link_layer.mac_header; + mac_header.destination = request.destination_addr; + mac_header.source = request.source_addr; + mac_header.qos_control.user_priority(request.access_category); + + ByteBuffer link_layer_buffer; + serialize_into_buffer(link_layer, link_layer_buffer); + assert(link_layer_buffer.size() == access::G5LinkLayer::length_bytes); + packet->layer(OsiLayer::Link) = std::move(link_layer_buffer); + + const std::size_t payload_size = packet->size(); + const std::size_t total_size = sizeof(tMKxTxPacket) + payload_size; + + tMKxTxPacket phy = { 0 }; + phy.Hdr.Type = MKXIF_TXPACKET; + phy.Hdr.Len = total_size; + phy.TxPacketData.TxAntenna = MKX_ANT_DEFAULT; + phy.TxPacketData.TxFrameLength = payload_size; + auto phy_ptr = reinterpret_cast(&phy); + packet->layer(OsiLayer::Physical) = ByteBuffer { phy_ptr, phy_ptr + sizeof(tMKxTxPacket) }; + + transmit(std::move(packet)); +} + +boost::optional CohdaLink::parse_ethernet_header(vanetza::CohesivePacket& packet) const +{ + static const std::size_t min_length = sizeof(tMKxRxPacket) + access::G5LinkLayer::length_bytes + access::ieee802::dot11::fcs_length_bytes; + if (packet.size(OsiLayer::Physical) < min_length) { + return boost::none; + } + + packet.set_boundary(OsiLayer::Physical, sizeof(tMKxRxPacket)); + auto phy = reinterpret_cast(&*packet[OsiLayer::Physical].begin()); + if (phy->Hdr.Type != MKXIF_RXPACKET) { + return boost::none; + } + + if (phy->Hdr.Len != packet.size() || phy->RxPacketData.RxFrameLength != packet.size() - sizeof(tMKxRxPacket)) { + return boost::none; + } + + if (!phy->RxPacketData.FCSPass) { + return boost::none; + } + + packet.trim(OsiLayer::Link, packet.size() - access::ieee802::dot11::fcs_length_bytes); + packet.set_boundary(OsiLayer::Link, access::G5LinkLayer::length_bytes); + access::G5LinkLayer link_layer; + deserialize_from_range(link_layer, packet[OsiLayer::Link]); + if (!access::check_fixed_fields(link_layer)) { + return boost::none; + } + + EthernetHeader eth; + eth.destination = link_layer.mac_header.destination; + eth.source = link_layer.mac_header.source; + eth.type = link_layer.llc_snap_header.protocol_id; + return eth; +} \ No newline at end of file diff --git a/src/link_layer.cpp b/src/link_layer.cpp index c23e9ba..aaaf1ed 100644 --- a/src/link_layer.cpp +++ b/src/link_layer.cpp @@ -1,25 +1,46 @@ #include "autoware_v2x/link_layer.hpp" +#ifdef BUILD_COHDA +#include "autoware_v2x/cohda_link.hpp" +#endif #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, const std::string &cube_ip) +#include + +#include + +std::unique_ptr create_link_layer(boost::asio::io_service &io_service, const std::string &name, const EthernetDevice &target_device) { std::unique_ptr link_layer; + boost::asio::generic::raw_protocol raw_protocol( + AF_PACKET, vanetza::access::ethertype::GeoNetworking.net()); + boost::asio::generic::raw_protocol::socket raw_socket(io_service, raw_protocol); + raw_socket.bind(target_device.endpoint(AF_PACKET)); if (name == "ethernet") { - boost::asio::generic::raw_protocol raw_protocol( - AF_PACKET, vanetza::access::ethertype::GeoNetworking.net()); - 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 +#ifdef BUILD_COHDA + else if (name == "cohda") { + link_layer.reset(new CohdaLink{std::move(raw_socket)}); + } +#endif + else { + throw std::runtime_error("Unknown link layer: " + name); + } return link_layer; } + +std::unique_ptr create_link_layer(boost::asio::io_service &io_service, const std::string &name, const std::string &target_device) +{ + std::unique_ptr link_layer; + + if (name == "cube-evk") { + link_layer.reset(new CubeEvkLink{ io_service, boost::asio::ip::address_v4::from_string(target_device) }); + } else { + throw std::runtime_error("Unknown link layer: " + name); + } + + return link_layer; +} \ No newline at end of file diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index f423e19..9880787 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -133,16 +134,45 @@ namespace v2x 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()); +#ifdef BUILD_COHDA + std::vector valid_link_layers = {"ethernet", "cube-evk", "cohda"}; +#else + std::vector valid_link_layers = {"ethernet", "cube-evk"}; +#endif + if (std::find(valid_link_layers.begin(), valid_link_layers.end(), link_layer_name) == valid_link_layers.end()) { + throw std::runtime_error("Invalid link layer: " + link_layer_name); + } - EthernetDevice device(network_interface.c_str()); - vanetza::MacAddress mac_address = device.address(); + std::string target_device; + node_->get_parameter("target_device", target_device); + const std::regex ip_pattern(R"(^((25[0-5]|(2[0-4]|1\d|[1-9]|)\d)\.?\b){4}$)"); + bool is_ip = std::regex_match(target_device, ip_pattern); + RCLCPP_INFO(node_->get_logger(), "IS_IP: %d, Target Device: %s", is_ip, target_device.c_str()); + if (!is_ip && !(link_layer_name == "ethernet" || link_layer_name == "cohda")) { + throw std::runtime_error("Invalid target device: " + target_device + "\nMust use ethernet link layer for network interface"); + } - std::stringstream sout; - sout << mac_address; - RCLCPP_INFO(node_->get_logger(), "MAC Address: '%s'", sout.str().c_str()); + vanetza::MacAddress mac_address; + std::unique_ptr link_layer; + if (!is_ip) { + EthernetDevice device(target_device.c_str()); + mac_address = device.address(); + + std::stringstream sout; + sout << mac_address; + RCLCPP_INFO(node_->get_logger(), "MAC Address: '%s'", sout.str().c_str()); + + link_layer = create_link_layer(io_service, link_layer_name, device); + RCLCPP_INFO(node_->get_logger(), "Ethernet Device: %s", target_device.c_str()); + } else { + if (link_layer_name == "cube-evk") { + link_layer = create_link_layer(io_service, link_layer_name, target_device); + RCLCPP_INFO(node_->get_logger(), "CubeEVK IP: %s", target_device.c_str()); + } else { + throw std::runtime_error("Invalid link layer: " + link_layer_name); + } + mac_address = parse_mac_address("00:00:00:00:00:00").value(); + } // Geonetworking Management Infirmation Base (MIB) defines the GN protocol constants. gn::MIB mib; @@ -152,12 +182,6 @@ 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, 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 98af784..65e5f5d 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -61,8 +61,7 @@ namespace v2x // Declare Parameters this->declare_parameter("link_layer", "ethernet"); - this->declare_parameter("network_interface", "v2x_testing"); - this->declare_parameter("cube_ip", "127.0.0.1"); + this->declare_parameter("target_device", "lo"); this->declare_parameter("is_sender", true); this->declare_parameter("publish_own_cams", true); this->declare_parameter("security", "none");