TiagoRG
/
AutowareV2X
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add cohda support 

Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>
This commit is contained in:
Tiago Garcia 2024-11-22 23:27:30 +00:00
parent 6fd55d1bb9
commit 049057fdf2
No known key found for this signature in database
GPG Key ID: A3B1096AD19E1688
9 changed files with 205 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
CMakeLists.txt
View File

@ -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)

5
config/autoware_v2x.param.yaml
View File

@ -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"

17
include/autoware_v2x/cohda_link.hpp Normal file
View File

@ -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<vanetza::ChunkPacket>) override;
protected:
boost::optional<vanetza::EthernetHeader> parse_ethernet_header(vanetza::CohesivePacket&) const override;
};
#endif /* COHDA_LINK_HPP_IXOCQ5RH */

4
include/autoware_v2x/link_layer.hpp
View File

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

4
launch/v2x.launch.xml
View File

@ -1,8 +1,8 @@
<launch>
<arg name="link_layer" default="ethernet"/>
<arg name="network_interface" default="v2x_testing"/>
<arg name="cube_ip" default="127.0.0.1"/>
<arg name="target_device" default="lo"/>
<arg name="is_sender" default="true"/>
<arg name="publish_own_cams" default="false"/>
<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"/>

81
src/cohda_link.cpp Normal file
View File

@ -0,0 +1,81 @@
#include <memory>
#include <boost/optional.hpp>
#include "autoware_v2x/cohda_link.hpp"
#include <boost/optional/optional.hpp>
#include <vanetza/access/access_category.hpp>
#include <vanetza/access/data_request.hpp>
#include <vanetza/access/g5_link_layer.hpp>
#include <vanetza/common/byte_buffer.hpp>
#include <vanetza/common/serialization_buffer.hpp>
#include <vanetza/common/clock.hpp>
#include <vanetza/dcc/mapping.hpp>
#include <vanetza/net/chunk_packet.hpp>
#include <vanetza/net/cohesive_packet.hpp>
#include <vanetza/net/ethernet_header.hpp>
#include <cassert>
#include <llc-api.h>
using namespace vanetza;
void CohdaLink::request(const vanetza::access::DataRequest& request, std::unique_ptr<vanetza::ChunkPacket> 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<const uint8_t*>(&phy);
packet->layer(OsiLayer::Physical) = ByteBuffer { phy_ptr, phy_ptr + sizeof(tMKxTxPacket) };
transmit(std::move(packet));
}
boost::optional<vanetza::EthernetHeader> 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<const tMKxRxPacket*>(&*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;
}

45
src/link_layer.cpp
View File

@ -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 <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, const std::string &cube_ip)
#include <vanetza/access/ethertype.hpp>
#include <boost/asio/generic/raw_protocol.hpp>
std::unique_ptr<LinkLayer> create_link_layer(boost::asio::io_service &io_service, const std::string &name, const EthernetDevice &target_device)
{
std::unique_ptr<LinkLayer> 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<LinkLayer> create_link_layer(boost::asio::io_service &io_service, const std::string &name, const std::string &target_device)
{
std::unique_ptr<LinkLayer> 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;
}

52
src/v2x_app.cpp
View File

@ -16,6 +16,7 @@
#include <vanetza/asn1/cpm.hpp>
#include <vanetza/asn1/cam.hpp>
#include <regex>
#include <sstream>
#include <memory>
#include <GeographicLib/UTMUPS.hpp>
@ -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<std::string> valid_link_layers = {"ethernet", "cube-evk", "cohda"};
#else
std::vector<std::string> 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<LinkLayer> 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;

3
src/v2x_node.cpp
View File

@ -61,8 +61,7 @@ namespace v2x
// Declare Parameters
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<std::string>("target_device", "lo");
this->declare_parameter<bool>("is_sender", true);
this->declare_parameter<bool>("publish_own_cams", true);
this->declare_parameter<std::string>("security", "none");