From fd33d987bacefe6edb1f20970da08fde77283f88 Mon Sep 17 00:00:00 2001 From: Yu Asabe Date: Thu, 28 Oct 2021 06:56:21 +0900 Subject: [PATCH] newer files --- CMakeLists.txt | 61 ++++++++ cmake/FindGeographicLib.cmake | 54 +++++++ include/autoware_v2x/application.hpp | 41 ++++++ include/autoware_v2x/cpm_application.hpp | 23 +++ include/autoware_v2x/cpm_receiver.hpp | 0 include/autoware_v2x/cpm_sender.hpp | 48 +++++++ include/autoware_v2x/dcc_passthrough.hpp | 26 ++++ include/autoware_v2x/ethernet_device.hpp | 29 ++++ include/autoware_v2x/link_layer.hpp | 29 ++++ include/autoware_v2x/positioning.hpp | 20 +++ include/autoware_v2x/raw_socket_link.hpp | 38 +++++ include/autoware_v2x/router_context.hpp | 51 +++++++ include/autoware_v2x/security.hpp | 15 ++ include/autoware_v2x/time_trigger.hpp | 26 ++++ launch/v2x.launch.xml | 3 + package.xml | 26 ++++ src/application.cpp | 76 ++++++++++ src/cpm_application.cpp | 170 +++++++++++++++++++++++ src/cpm_receiver.cpp | 123 ++++++++++++++++ src/cpm_sender.cpp | 126 +++++++++++++++++ src/dcc_passthrough.cpp | 36 +++++ src/ethernet_device.cpp | 58 ++++++++ src/link_layer.cpp | 22 +++ src/positioning.cpp | 22 +++ src/raw_socket_link.cpp | 72 ++++++++++ src/router_context.cpp | 128 +++++++++++++++++ src/security.cpp | 74 ++++++++++ src/time_trigger.cpp | 42 ++++++ 28 files changed, 1439 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 cmake/FindGeographicLib.cmake create mode 100644 include/autoware_v2x/application.hpp create mode 100644 include/autoware_v2x/cpm_application.hpp create mode 100644 include/autoware_v2x/cpm_receiver.hpp create mode 100644 include/autoware_v2x/cpm_sender.hpp create mode 100644 include/autoware_v2x/dcc_passthrough.hpp create mode 100644 include/autoware_v2x/ethernet_device.hpp create mode 100644 include/autoware_v2x/link_layer.hpp create mode 100644 include/autoware_v2x/positioning.hpp create mode 100644 include/autoware_v2x/raw_socket_link.hpp create mode 100644 include/autoware_v2x/router_context.hpp create mode 100644 include/autoware_v2x/security.hpp create mode 100644 include/autoware_v2x/time_trigger.hpp create mode 100644 launch/v2x.launch.xml create mode 100644 package.xml create mode 100644 src/application.cpp create mode 100644 src/cpm_application.cpp create mode 100644 src/cpm_receiver.cpp create mode 100644 src/cpm_sender.cpp create mode 100644 src/dcc_passthrough.cpp create mode 100644 src/ethernet_device.cpp create mode 100644 src/link_layer.cpp create mode 100644 src/positioning.cpp create mode 100644 src/raw_socket_link.cpp create mode 100644 src/router_context.cpp create mode 100644 src/security.cpp create mode 100644 src/time_trigger.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..b28dbf0 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_v2x) +set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +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) +ament_auto_find_build_dependencies() +find_package(std_msgs REQUIRED) + +include_directories( + include +) + +ament_auto_add_library(autoware_v2x SHARED + src/cpm_sender.cpp + src/application.cpp + src/cpm_application.cpp + src/ethernet_device.cpp + src/link_layer.cpp + src/raw_socket_link.cpp + src/router_context.cpp + src/dcc_passthrough.cpp + src/time_trigger.cpp + src/positioning.cpp + src/security.cpp +) + +target_link_libraries(autoware_v2x Vanetza::vanetza ${GeographicLib_LIBRARIES} Boost::thread) + +rclcpp_components_register_node(autoware_v2x + PLUGIN "v2x::V2XNode" + EXECUTABLE autoware_v2x_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) \ No newline at end of file diff --git a/cmake/FindGeographicLib.cmake b/cmake/FindGeographicLib.cmake new file mode 100644 index 0000000..b2cc003 --- /dev/null +++ b/cmake/FindGeographicLib.cmake @@ -0,0 +1,54 @@ +option(GeographicLib_PREFER_PACKAGE_CONFIG "Prefer CMake config mode" ON) +mark_as_advanced(GeographicLib_PREFER_PACKAGE_CONFIG) + +if(GeographicLib_PREFER_PACKAGE_CONFIG) + find_package(GeographicLib ${GeographicLib_FIND_VERSION} QUIET CONFIG) + if (GeographicLib_FOUND) + message(STATUS "GeographicLib: using found CMake configuration") + return() + endif() +endif() +message(STATUS "GeographicLib: using lookup code by Vanetza") + +find_path(GeographicLib_INCLUDE_DIR NAMES GeographicLib/Config.h DOC "GeographicLib include directory") +find_library(GeographicLib_LIBRARY_RELEASE NAMES Geographic DOC "GeographicLib library (release)") +find_library(GeographicLib_LIBRARY_DEBUG NAMES Geographic_d DOC "GeographicLib library (debug)") + +include(SelectLibraryConfigurations) +select_library_configurations(GeographicLib) + +if(GeographicLib_INCLUDE_DIR) + file(STRINGS ${GeographicLib_INCLUDE_DIR}/GeographicLib/Config.h _config_version REGEX "GEOGRAPHICLIB_VERSION_STRING") + string(REGEX MATCH "\"([0-9.]+)\"$" _match_version ${_config_version}) + set(GeographicLib_VERSION_STRING ${CMAKE_MATCH_1}) +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(GeographicLib + REQUIRED_VARS GeographicLib_INCLUDE_DIR GeographicLib_LIBRARY + FOUND_VAR GeographicLib_FOUND + VERSION_VAR GeographicLib_VERSION_STRING) + +if(GeographicLib_FOUND AND NOT TARGET GeographicLib::GeographicLib) + add_library(GeographicLib::GeographicLib UNKNOWN IMPORTED) + set_target_properties(GeographicLib::GeographicLib PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${GeographicLib_INCLUDE_DIR}" + IMPORTED_LOCATION "${GeographicLib_LIBRARY}" + IMPORTED_LINK_INTERFACE_LANGUAGES "CXX") + if(EXISTS "${GeographicLib_LIBRARY_RELEASE}") + set_property(TARGET GeographicLib::GeographicLib APPEND PROPERTY + IMPORTED_CONFIGURATIONS RELEASE) + set_target_properties(GeographicLib::GeographicLib PROPERTIES + IMPORTED_LOCATION_RELEASE "${GeographicLib_LIBRARY_RELEASE}") + endif() + if(EXISTS "${GeographicLib_LIBRARY_DEBUG}") + set_property(TARGET GeographicLib::GeographicLib APPEND PROPERTY + IMPORTED_CONFIGURATIONS DEBUG) + set_target_properties(GeographicLib::GeographicLib PROPERTIES + IMPORTED_LOCATION_DEBUG "${GeographicLib_LIBRARY_DEBUG}") + endif() +endif() + +mark_as_advanced(GeographicLib_INCLUDE_DIR GeographicLib_LIBRARY) +set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) +# GeographicLib_LIBRARIES is set by SelectLibraryConfigurations diff --git a/include/autoware_v2x/application.hpp b/include/autoware_v2x/application.hpp new file mode 100644 index 0000000..842fd6f --- /dev/null +++ b/include/autoware_v2x/application.hpp @@ -0,0 +1,41 @@ +#ifndef APPLICATION_HPP_PSIGPUTG +#define APPLICATION_HPP_PSIGPUTG + +#include +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +class Application : public vanetza::btp::IndicationInterface +{ +public: + using DataConfirm = vanetza::geonet::DataConfirm; + using DataIndication = vanetza::btp::DataIndication; + using DataRequest = vanetza::btp::DataRequestGeoNetParams; + using DownPacketPtr = vanetza::geonet::Router::DownPacketPtr; + using PortType = vanetza::btp::port_type; + using PromiscuousHook = vanetza::btp::PortDispatcher::PromiscuousHook; + using UpPacketPtr = vanetza::geonet::Router::UpPacketPtr; + + Application() = default; + Application(const Application &) = delete; + Application &operator=(const Application &) = delete; + virtual ~Application() = default; + + virtual PortType port() = 0; + virtual PromiscuousHook *promiscuous_hook(); + +protected: + DataConfirm request(const DataRequest &, DownPacketPtr, rclcpp::Node*); + +private: + friend class RouterContext; + vanetza::geonet::GbcDataRequest request_gbc(const DataRequest &); + vanetza::geonet::ShbDataRequest request_shb(const DataRequest &); + vanetza::geonet::Router *router_; +}; + +#endif /* APPLICATION_HPP_PSIGPUTG */ diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp new file mode 100644 index 0000000..47916d5 --- /dev/null +++ b/include/autoware_v2x/cpm_application.hpp @@ -0,0 +1,23 @@ +#ifndef CPM_APPLICATION_HPP_EUIC2VFR +#define CPM_APPLICATION_HPP_EUIC2VFR + +#include "autoware_v2x/application.hpp" +#include "rclcpp/rclcpp.hpp" +#include +#include +#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" +#include "autoware_v2x/positioning.hpp" + +class CpmApplication : public Application +{ +public: + CpmApplication(rclcpp::Node *node); + PortType port() override; + void indicate(const DataIndication &, UpPacketPtr) override; + void send(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr, rclcpp::Node *, double, double); + +private: + rclcpp::Node* node_; +}; + +#endif /* CPM_APPLICATION_HPP_EUIC2VFR */ diff --git a/include/autoware_v2x/cpm_receiver.hpp b/include/autoware_v2x/cpm_receiver.hpp new file mode 100644 index 0000000..e69de29 diff --git a/include/autoware_v2x/cpm_sender.hpp b/include/autoware_v2x/cpm_sender.hpp new file mode 100644 index 0000000..fb3de5d --- /dev/null +++ b/include/autoware_v2x/cpm_sender.hpp @@ -0,0 +1,48 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include +#include "autoware_v2x/cpm_application.hpp" +#include "autoware_v2x/time_trigger.hpp" +#include "autoware_v2x/link_layer.hpp" +#include "autoware_v2x/ethernet_device.hpp" +#include "autoware_v2x/positioning.hpp" +#include "autoware_v2x/security.hpp" +#include "autoware_v2x/router_context.hpp" + +namespace v2x +{ + class V2XNode : public rclcpp::Node + { + public: + explicit V2XNode(const rclcpp::NodeOptions &node_options); + + private: + void objectsCallback( + const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg); + void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg); + + friend class CpmApplication; + friend class Application; + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr subscription_pos_; + + std::unique_ptr cp_; + std::unique_ptr app_; + boost::asio::io_service io_service_; + TimeTrigger trigger_; + const char *device_name_; + EthernetDevice device_; + vanetza::MacAddress mac_address_; + std::unique_ptr link_layer_; + vanetza::geonet::MIB mib_; + std::unique_ptr positioning_; + std::unique_ptr security_; + RouterContext context_; + + double pos_lat_; + double pos_lon_; + + }; +} \ No newline at end of file diff --git a/include/autoware_v2x/dcc_passthrough.hpp b/include/autoware_v2x/dcc_passthrough.hpp new file mode 100644 index 0000000..d55ed09 --- /dev/null +++ b/include/autoware_v2x/dcc_passthrough.hpp @@ -0,0 +1,26 @@ +#ifndef DCC_PASSTHROUGH_HPP_GSDFESAE +#define DCC_PASSTHROUGH_HPP_GSDFESAE + +#include "autoware_v2x/time_trigger.hpp" +#include +#include +#include +#include + +class DccPassthrough : public vanetza::dcc::RequestInterface +{ +public: + DccPassthrough(vanetza::access::Interface&, TimeTrigger& trigger); + + void request(const vanetza::dcc::DataRequest& request, std::unique_ptr packet) override; + + void allow_packet_flow(bool allow); + bool allow_packet_flow(); + +private: + vanetza::access::Interface& access_; + TimeTrigger& trigger_; + bool allow_packet_flow_ = true; +}; + +#endif /* DCC_PASSTHROUGH_HPP_GSDFESAE */ diff --git a/include/autoware_v2x/ethernet_device.hpp b/include/autoware_v2x/ethernet_device.hpp new file mode 100644 index 0000000..db25ce9 --- /dev/null +++ b/include/autoware_v2x/ethernet_device.hpp @@ -0,0 +1,29 @@ +#ifndef ETHERNET_DEVICE_HPP_NEVC5DAY +#define ETHERNET_DEVICE_HPP_NEVC5DAY + +#include +#include +#include + +class EthernetDevice +{ +public: + using protocol = boost::asio::generic::raw_protocol; + + EthernetDevice(const char* devname); + EthernetDevice(const EthernetDevice&) = delete; + EthernetDevice& operator=(const EthernetDevice&) = delete; + ~EthernetDevice(); + + protocol::endpoint endpoint(int family) const; + vanetza::MacAddress address() const; + +private: + int index() const; + + int local_socket_; + std::string interface_name_; +}; + +#endif /* ETHERNET_DEVICE_HPP_NEVC5DAY */ + diff --git a/include/autoware_v2x/link_layer.hpp b/include/autoware_v2x/link_layer.hpp new file mode 100644 index 0000000..be24135 --- /dev/null +++ b/include/autoware_v2x/link_layer.hpp @@ -0,0 +1,29 @@ +#ifndef LINK_LAYER_HPP_FGEK0QTH +#define LINK_LAYER_HPP_FGEK0QTH + +#include "autoware_v2x/ethernet_device.hpp" +#include +#include +#include +#include +#include +#include + +class LinkLayerIndication +{ +public: + using IndicationCallback = std::function; + + virtual void indicate(IndicationCallback) = 0; + virtual ~LinkLayerIndication() = default; +}; + +class LinkLayer : public vanetza::access::Interface, public LinkLayerIndication +{ +}; + +std::unique_ptr +create_link_layer(boost::asio::io_service&, const EthernetDevice&, const std::string& name); + +#endif /* LINK_LAYER_HPP_FGEK0QTH */ + diff --git a/include/autoware_v2x/positioning.hpp b/include/autoware_v2x/positioning.hpp new file mode 100644 index 0000000..58524e7 --- /dev/null +++ b/include/autoware_v2x/positioning.hpp @@ -0,0 +1,20 @@ +#ifndef POSITIONING_HPP_VZRIW7PB +#define POSITIONING_HPP_VZRIW7PB + +#include +#include +#include +#include +#include +#include +#include + +class PositioningException : public std::runtime_error +{ + using std::runtime_error::runtime_error; +}; + +std::unique_ptr +create_position_provider(boost::asio::io_service &, const vanetza::Runtime &); + +#endif /* POSITIONING_HPP_VZRIW7PB */ diff --git a/include/autoware_v2x/raw_socket_link.hpp b/include/autoware_v2x/raw_socket_link.hpp new file mode 100644 index 0000000..27d9b8e --- /dev/null +++ b/include/autoware_v2x/raw_socket_link.hpp @@ -0,0 +1,38 @@ +#ifndef RAW_SOCKET_LINK_HPP_VUXH507U +#define RAW_SOCKET_LINK_HPP_VUXH507U + +#include "autoware_v2x/link_layer.hpp" +#include +#include +#include +#include +#include +#include + +class RawSocketLink : public LinkLayer +{ +public: + RawSocketLink(boost::asio::generic::raw_protocol::socket&&); + void request(const vanetza::access::DataRequest&, std::unique_ptr) override; + void indicate(IndicationCallback) override; + +protected: + std::size_t transmit(std::unique_ptr); + virtual boost::optional parse_ethernet_header(vanetza::CohesivePacket&) const; + +private: + void do_receive(); + void on_read(const boost::system::error_code&, std::size_t); + void pass_up(vanetza::CohesivePacket&&); + + static constexpr std::size_t layers_ = num_osi_layers(vanetza::OsiLayer::Physical, vanetza::OsiLayer::Application); + + boost::asio::generic::raw_protocol::socket socket_; + std::array buffers_; + IndicationCallback callback_; + vanetza::ByteBuffer receive_buffer_; + boost::asio::generic::raw_protocol::endpoint receive_endpoint_; +}; + +#endif /* RAW_SOCKET_LINK_HPP_VUXH507U */ + diff --git a/include/autoware_v2x/router_context.hpp b/include/autoware_v2x/router_context.hpp new file mode 100644 index 0000000..3aa7a80 --- /dev/null +++ b/include/autoware_v2x/router_context.hpp @@ -0,0 +1,51 @@ +#ifndef ROUTER_CONTEXT_HPP_KIPUYBY2 +#define ROUTER_CONTEXT_HPP_KIPUYBY2 + +#include "autoware_v2x/dcc_passthrough.hpp" +#include "autoware_v2x/link_layer.hpp" +#include +#include +#include +#include +#include +#include +#include + +class Application; +class TimeTrigger; + +class RouterContext +{ +public: + RouterContext(const vanetza::geonet::MIB&, TimeTrigger&, vanetza::PositionProvider&, vanetza::security::SecurityEntity*); + ~RouterContext(); + void enable(Application*); + void disable(Application*); + + /** + * Allow/disallow transmissions without GNSS position fix + * + * \param flag true if transmissions shall be dropped when no GNSS position fix is available + */ + void require_position_fix(bool flag); + + void set_link_layer(LinkLayer*); + void updateMIB(const vanetza::geonet::MIB&); + +private: + void indicate(vanetza::CohesivePacket&& packet, const vanetza::EthernetHeader& hdr); + void log_packet_drop(vanetza::geonet::Router::PacketDropReason); + void update_position_vector(); + void update_packet_flow(const vanetza::geonet::LongPositionVector&); + + vanetza::geonet::MIB mib_; + vanetza::geonet::Router router_; + TimeTrigger& trigger_; + vanetza::PositionProvider& positioning_; + vanetza::btp::PortDispatcher dispatcher_; + std::unique_ptr request_interface_; + std::list applications_; + bool require_position_fix_ = false; +}; + +#endif /* ROUTER_CONTEXT_HPP_KIPUYBY2 */ diff --git a/include/autoware_v2x/security.hpp b/include/autoware_v2x/security.hpp new file mode 100644 index 0000000..4c5f220 --- /dev/null +++ b/include/autoware_v2x/security.hpp @@ -0,0 +1,15 @@ +#ifndef SECURITY_HPP_FV13ZIYA +#define SECURITY_HPP_FV13ZIYA + +#include +#include +#include +#include +#include +#include + +std::unique_ptr +create_security_entity(const vanetza::Runtime&, vanetza::PositionProvider&); + +#endif /* SECURITY_HPP_FV13ZIYA */ + diff --git a/include/autoware_v2x/time_trigger.hpp b/include/autoware_v2x/time_trigger.hpp new file mode 100644 index 0000000..60f4e1e --- /dev/null +++ b/include/autoware_v2x/time_trigger.hpp @@ -0,0 +1,26 @@ +#ifndef TIME_TRIGGER_HPP_XRPGDYXO +#define TIME_TRIGGER_HPP_XRPGDYXO + +#include +#include +#include +#include + +class TimeTrigger +{ +public: + TimeTrigger(boost::asio::io_service &); + vanetza::Runtime & runtime() { return runtime_; } + void schedule(); + +private: + boost::posix_time::ptime now() const; + void on_timeout(const boost::system::error_code &); + void update_runtime(); + + boost::asio::io_service & io_service_; + boost::asio::deadline_timer timer_; + vanetza::ManualRuntime runtime_; +}; + +#endif /* TIME_TRIGGER_HPP_XRPGDYXO */ diff --git a/launch/v2x.launch.xml b/launch/v2x.launch.xml new file mode 100644 index 0000000..07a5bcc --- /dev/null +++ b/launch/v2x.launch.xml @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..0dee846 --- /dev/null +++ b/package.xml @@ -0,0 +1,26 @@ + + + + autoware_v2x + 0.0.0 + TODO: Package description + yuasabe + TODO: License declaration + + ament_cmake_auto + + autoware_perception_msgs + tf2_msgs + geometry_msgs + rclcpp + std_msgs + rclcpp_components + Vanetza + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/application.cpp b/src/application.cpp new file mode 100644 index 0000000..ee2604d --- /dev/null +++ b/src/application.cpp @@ -0,0 +1,76 @@ +#include "autoware_v2x/application.hpp" +#include "rclcpp/rclcpp.hpp" +#include +#include +#include + +using namespace vanetza; + +Application::DataConfirm Application::request(const DataRequest &request, DownPacketPtr packet, rclcpp::Node* node) +{ + // RCLCPP_INFO(node->get_logger(), "Inside Application::request 0"); + DataConfirm confirm(DataConfirm::ResultCode::Rejected_Unspecified); + // RCLCPP_INFO(node->get_logger(), "Inside Application::request 1"); + if (packet) { + // RCLCPP_INFO(node->get_logger(), "Inside Application::request 2"); + } + if (router_ && packet) + { + // RCLCPP_INFO(node->get_logger(), "Inside Application::request 3"); + btp::HeaderB btp_header; + btp_header.destination_port = this->port(); + btp_header.destination_port_info = host_cast(0); + packet->layer(OsiLayer::Transport) = btp_header; + + switch (request.transport_type) + { + case geonet::TransportType::SHB: + // RCLCPP_INFO(node->get_logger(), "Inside Application::request 4"); + confirm = router_->request(request_shb(request), std::move(packet)); + break; + case geonet::TransportType::GBC: + confirm = router_->request(request_gbc(request), std::move(packet)); + break; + default: + // TODO remaining transport types are not implemented + break; + } + } + + return confirm; +} + +void initialize_request(const Application::DataRequest &generic, geonet::DataRequest &geonet) +{ + geonet.upper_protocol = geonet::UpperProtocol::BTP_B; + geonet.communication_profile = generic.communication_profile; + geonet.its_aid = generic.its_aid; + if (generic.maximum_lifetime) + { + geonet.maximum_lifetime = generic.maximum_lifetime.get(); + } + geonet.repetition = generic.repetition; + geonet.traffic_class = generic.traffic_class; +} + +geonet::GbcDataRequest Application::request_gbc(const DataRequest &generic) +{ + assert(router_); + geonet::GbcDataRequest gbc(router_->get_mib()); + initialize_request(generic, gbc); + gbc.destination = boost::get(generic.destination); + return gbc; +} + +geonet::ShbDataRequest Application::request_shb(const DataRequest &generic) +{ + assert(router_); + geonet::ShbDataRequest shb(router_->get_mib()); + initialize_request(generic, shb); + return shb; +} + +Application::PromiscuousHook *Application::promiscuous_hook() +{ + return nullptr; +} diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp new file mode 100644 index 0000000..a8d1111 --- /dev/null +++ b/src/cpm_application.cpp @@ -0,0 +1,170 @@ +#include "autoware_v2x/cpm_application.hpp" +#include "autoware_v2x/positioning.hpp" +#include "autoware_v2x/security.hpp" +#include "autoware_v2x/link_layer.hpp" + +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// This is a very simple application that sends BTP-B messages with the content 0xc0ffee. + +using namespace vanetza; +using namespace vanetza::facilities; +using namespace std::chrono; + +CpmApplication::CpmApplication(rclcpp::Node *node) : node_(node) +{ + RCLCPP_INFO(node_->get_logger(), "CpmApplication started..."); +} + +CpmApplication::PortType CpmApplication::port() +{ + return btp::ports::CPM; +} + +void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet) +{ + asn1::PacketVisitor visitor; + std::shared_ptr cpm = boost::apply_visitor(visitor, *packet); + if (cpm) { + RCLCPP_INFO(node_->get_logger(), "Received decodable CPM content"); + } else { + RCLCPP_INFO(node_->get_logger(), "Received broken content"); + } + asn1::Cpm message = *cpm; + ItsPduHeader_t &header = message->header; + RCLCPP_INFO(node_->get_logger(), "cpm.ItsPduHeader.messageId = %d", header.messageID); + RCLCPP_INFO(node_->get_logger(), "cpm.ItsPduHeader.stationId = %d", header.stationID); + + CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer; + double lat = management.referencePosition.latitude; + double lon = management.referencePosition.longitude; + RCLCPP_INFO(node_->get_logger(), "cpm.(reference position) = %f, %f", lat, lon); +} + +void CpmApplication::send(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg, rclcpp::Node *node, double pos_lat, double pos_lon) +{ + RCLCPP_INFO(node->get_logger(), "Sending CPM..."); + // Send all objects in 1 CPM message + vanetza::asn1::Cpm message; + + // ITS PDU Header + ItsPduHeader_t &header = message->header; + header.protocolVersion = 1; + header.messageID = 14; + header.stationID = 1; + + const auto time_now = duration_cast(system_clock::now().time_since_epoch()); + uint16_t gen_delta_time = time_now.count(); + + CollectivePerceptionMessage_t &cpm = message->cpm; + cpm.generationDeltaTime = gen_delta_time * GenerationDeltaTime_oneMilliSec; + + // auto position = positioning->position_specify(pos_lat, pos_lon); + + CpmManagementContainer_t& management = cpm.cpmParameters.managementContainer; + management.stationType = StationType_passengerCar; + // management.referencePosition.latitude = pos_lat; + // management.referencePosition.longitude = pos_lon; + // management.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 1.0; + // management.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 1.0; + PositionFix fix; + // fix.timestamp = time_now; + fix.latitude = pos_lat * units::degree; + fix.longitude = pos_lon * units::degree; + fix.confidence.semi_major = 1.0 * units::si::meter; + fix.confidence.semi_minor = fix.confidence.semi_major; + copy(fix, management.referencePosition); + + // cpm.cpmParameters.stationDataContainer = NULL; + // cpm.cpmParameters.perceivedObjectContainer = NULL; + cpm.cpmParameters.numberOfPerceivedObjects = msg->objects.size(); + + StationDataContainer_t*& sdc = cpm.cpmParameters.stationDataContainer; + sdc = vanetza::asn1::allocate(); + // RCLCPP_INFO(node->get_logger(), "Allocated sdc"); + sdc->present = StationDataContainer_PR_originatingVehicleContainer; + OriginatingVehicleContainer_t& ovc = sdc->choice.originatingVehicleContainer; + ovc.speed.speedValue = 0; + ovc.speed.speedConfidence = 1; + ovc.heading.headingValue = 0; + ovc.heading.headingConfidence = 1; + + PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer; + poc = vanetza::asn1::allocate(); + // cpm.cpmParameters.perceivedObjectContainer = poc; + // PerceivedObjectContainer_t pocc = *poc; + // RCLCPP_INFO(node->get_logger(), "Allocated poc"); + + for (auto object : msg->objects) { + PerceivedObject *pObj = vanetza::asn1::allocate(); + pObj->objectID = 1; + pObj->timeOfMeasurement = 10; + pObj->xDistance.value = 100; + pObj->xDistance.confidence = 1; + pObj->yDistance.value = 200; + pObj->yDistance.confidence = 1; + pObj->xSpeed.value = 5; + pObj->xSpeed.confidence = 1; + pObj->ySpeed.value = 0; + pObj->ySpeed.confidence = 1; + + ASN_SEQUENCE_ADD(poc, pObj); + // RCLCPP_INFO(node->get_logger(), "Added one object to poc->list"); + } + + + Application::DownPacketPtr packet{new DownPacket()}; + std::unique_ptr payload { new geonet::DownPacket() }; + // std::shared_ptr message_p = std::make_shared(message); + // std::unique_ptr buffer { new convertible::byte_buffer_impl(&message)}; + + payload->layer(OsiLayer::Application) = std::move(message); + + Application::DataRequest request; + request.its_aid = aid::CP; + request.transport_type = geonet::TransportType::SHB; + request.communication_profile = geonet::CommunicationProfile::ITS_G5; + + // RCLCPP_INFO(node->get_logger(), "Packet Size: %d", payload->size()); + + // RCLCPP_INFO(node->get_logger(), "Going to Application::request..."); + Application::DataConfirm confirm = Application::request(request, std::move(payload), node); + if (!confirm.accepted()) + { + throw std::runtime_error("CPM application data request failed"); + } + // RCLCPP_INFO(node->get_logger(), "Application::request END"); +} + +// void HelloApplication::schedule_timer() +// { +// timer_.expires_from_now(interval_); +// timer_.async_wait(std::bind(&HelloApplication::on_timer, this, std::placeholders::_1)); +// } + +// void HelloApplication::on_timer(const boost::system::error_code& ec) +// { +// if (ec != boost::asio::error::operation_aborted) { +// DownPacketPtr packet { new DownPacket() }; +// packet->layer(OsiLayer::Application) = ByteBuffer { 0xC0, 0xFF, 0xEE }; +// DataRequest request; +// request.transport_type = geonet::TransportType::SHB; +// request.communication_profile = geonet::CommunicationProfile::ITS_G5; +// request.its_aid = aid::CA; +// auto confirm = Application::request(request, std::move(packet)); +// if (!confirm.accepted()) { +// throw std::runtime_error("Hello application data request failed"); +// } + +// schedule_timer(); +// } +// } diff --git a/src/cpm_receiver.cpp b/src/cpm_receiver.cpp new file mode 100644 index 0000000..3727d00 --- /dev/null +++ b/src/cpm_receiver.cpp @@ -0,0 +1,123 @@ +#include "autoware_v2x/cpm_sender.hpp" +#include "autoware_v2x/time_trigger.hpp" +#include "autoware_v2x/router_context.hpp" +#include "autoware_v2x/positioning.hpp" +#include "autoware_v2x/security.hpp" +#include "autoware_v2x/link_layer.hpp" +#include "autoware_v2x/cpm_application.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +#include +#include +#include +#include +#include +#include + +namespace gn = vanetza::geonet; + +using namespace vanetza; +using namespace vanetza::facilities; +using namespace std::chrono; + +namespace v2x +{ + V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : + rclcpp::Node("autoware_v2x_node", node_options), + io_service_(), + trigger_(io_service_), + device_name_("wlp4s0"), + device_(device_name_), + mac_address_(device_.address()), + link_layer_(create_link_layer(io_service_, device_, "ethernet")), + positioning_(create_position_provider(io_service_, trigger_.runtime())), + security_(create_security_entity(trigger_.runtime(), *positioning_)), + mib_(), + context_(mib_, trigger_, *positioning_, security_.get()), + cp_(new CpmApplication(this)), + app_() + { + using std::placeholders::_1; + subscription_ = this->create_subscription("/perception/object_recognition/detection/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); + + subscription_pos_ = this->create_subscription("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1)); + RCLCPP_INFO(get_logger(), "V2X Node Launched"); + + // device_name_ = "wlp4s0"; + // device_(device_name_); + // mac_address_ = device_.address(); + + std::stringstream sout; + sout << mac_address_; + RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str()); + + // trigger_(io_service_); + + // link_layer_ = create_link_layer(io_service_, device_, "ethernet"); + mib_.itsGnLocalGnAddr.mid(mac_address_); + mib_.itsGnLocalGnAddr.is_manually_configured(true); + mib_.itsGnLocalAddrConfMethod = geonet::AddrConfMethod::Managed; + mib_.itsGnSecurity = false; + mib_.itsGnProtocolVersion = 1; + + // context_.router_.set_address(mib_.itsGnLocalGnAddr); + context_.updateMIB(mib_); + + // positioning_ = create_position_provider(io_service_, trigger_.runtime()); + // security_ = create_security_entity(trigger_.runtime(), *positioning_); + + // RouterContext context(mib_, trigger_, *positioning_, security_.get()); + // RouterContext context_(mib_, trigger_, *positioning_, security_.get()); + context_.set_link_layer(link_layer_.get()); + + // std::unique_ptr cp_ { new CpmApplication(this) }; + // app_ = std::move(cp_); + + context_.enable(cp_.get()); + + + // io_service_.run(); + + // // Print MAC Address to logger + // std::stringstream sout; + // sout << mac_address; + // RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str()); + } + + void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) + { + // RCLCPP_INFO(get_logger(), "I heard: '%s'", msg->data.c_str()); + RCLCPP_INFO(get_logger(), "V2X: %d objects detected!", msg->objects.size()); + // Send CPM + cp_->send(msg, this, pos_lat_, pos_lon_); + } + + void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { + // RCLCPP_INFO(get_logger(), "Ego Position: (%f, %f, %f)", msg->transforms[0].transform.translation.x, msg->transforms[0].transform.translation.y, msg->transforms[0].transform.translation.z); + + float x = msg->transforms[0].transform.translation.x; + float y = msg->transforms[0].transform.translation.y; + float z = msg->transforms[0].transform.translation.z; + + char mgrs[20]; + int zone, prec; + bool northp; + double x_mgrs, y_mgrs; + double lat, lon; + sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y); + RCLCPP_INFO(get_logger(), "MGRS: %s", mgrs); + + GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec); + GeographicLib::UTMUPS::Reverse(zone, northp, x_mgrs, y_mgrs, lat, lon); + + pos_lat_ = lat; + pos_lon_ = lon; + + RCLCPP_INFO(get_logger(), "Ego Position Lat/Lon: %f, %f", lat, lon); + } +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(v2x::V2XNode) \ No newline at end of file diff --git a/src/cpm_sender.cpp b/src/cpm_sender.cpp new file mode 100644 index 0000000..d735702 --- /dev/null +++ b/src/cpm_sender.cpp @@ -0,0 +1,126 @@ +#include "autoware_v2x/cpm_sender.hpp" +#include "autoware_v2x/time_trigger.hpp" +#include "autoware_v2x/router_context.hpp" +#include "autoware_v2x/positioning.hpp" +#include "autoware_v2x/security.hpp" +#include "autoware_v2x/link_layer.hpp" +#include "autoware_v2x/cpm_application.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace gn = vanetza::geonet; + +using namespace vanetza; +using namespace vanetza::facilities; +using namespace std::chrono; + +namespace v2x +{ + V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : + rclcpp::Node("autoware_v2x_node", node_options), + io_service_(), + trigger_(io_service_), + device_name_("wlp4s0"), + device_(device_name_), + mac_address_(device_.address()), + link_layer_(create_link_layer(io_service_, device_, "ethernet")), + positioning_(create_position_provider(io_service_, trigger_.runtime())), + security_(create_security_entity(trigger_.runtime(), *positioning_)), + mib_(), + cp_(new CpmApplication(this)), + context_(mib_, trigger_, *positioning_, security_.get()), + app_() + { + using std::placeholders::_1; + subscription_ = this->create_subscription("/perception/object_recognition/detection/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); + + subscription_pos_ = this->create_subscription("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1)); + RCLCPP_INFO(get_logger(), "V2X Node Launched"); + + // device_name_ = "wlp4s0"; + // device_(device_name_); + // mac_address_ = device_.address(); + + std::stringstream sout; + sout << mac_address_; + RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str()); + + // trigger_(io_service_); + + // link_layer_ = create_link_layer(io_service_, device_, "ethernet"); + mib_.itsGnLocalGnAddr.mid(mac_address_); + mib_.itsGnLocalGnAddr.is_manually_configured(true); + mib_.itsGnLocalAddrConfMethod = geonet::AddrConfMethod::Managed; + mib_.itsGnSecurity = false; + mib_.itsGnProtocolVersion = 1; + + // context_(mib_, trigger_, *positioning_, security_.get()), + // context_.router_.set_address(mib_.itsGnLocalGnAddr); + context_.updateMIB(mib_); + + // positioning_ = create_position_provider(io_service_, trigger_.runtime()); + // security_ = create_security_entity(trigger_.runtime(), *positioning_); + + // RouterContext context_(mib_, trigger_, *positioning_, security_.get()); + // RouterContext context_(mib_, trigger_, *positioning_, security_.get()); + context_.set_link_layer(link_layer_.get()); + + // std::unique_ptr cp_ { new CpmApplication(this) }; + // app_ = std::move(cp_); + + context_.enable(cp_.get()); + + + // io_service_.run(); + boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_)); + + // // Print MAC Address to logger + // std::stringstream sout; + // sout << mac_address; + // RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str()); + } + + void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) + { + // RCLCPP_INFO(get_logger(), "I heard: '%s'", msg->data.c_str()); + RCLCPP_INFO(get_logger(), "V2X: %d objects detected!", msg->objects.size()); + // Send CPM + cp_->send(msg, this, pos_lat_, pos_lon_); + } + + void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { + // RCLCPP_INFO(get_logger(), "Ego Position: (%f, %f, %f)", msg->transforms[0].transform.translation.x, msg->transforms[0].transform.translation.y, msg->transforms[0].transform.translation.z); + + float x = msg->transforms[0].transform.translation.x; + float y = msg->transforms[0].transform.translation.y; + float z = msg->transforms[0].transform.translation.z; + + char mgrs[20]; + int zone, prec; + bool northp; + double x_mgrs, y_mgrs; + double lat, lon; + sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y); + RCLCPP_INFO(get_logger(), "MGRS: %s", mgrs); + + GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec); + GeographicLib::UTMUPS::Reverse(zone, northp, x_mgrs, y_mgrs, lat, lon); + + pos_lat_ = lat; + pos_lon_ = lon; + + RCLCPP_INFO(get_logger(), "Ego Position Lat/Lon: %f, %f", lat, lon); + } +} + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(v2x::V2XNode) \ No newline at end of file diff --git a/src/dcc_passthrough.cpp b/src/dcc_passthrough.cpp new file mode 100644 index 0000000..987a781 --- /dev/null +++ b/src/dcc_passthrough.cpp @@ -0,0 +1,36 @@ +#include "autoware_v2x/dcc_passthrough.hpp" +#include +#include +#include +#include +#include +#include +#include "autoware_v2x/time_trigger.hpp" + +using namespace vanetza; + +DccPassthrough::DccPassthrough(access::Interface & access, TimeTrigger & trigger) +: access_(access), trigger_(trigger) +{ +} + +void DccPassthrough::request(const dcc::DataRequest & request, std::unique_ptr packet) +{ + if (!allow_packet_flow_) { + std::cout << "ignored request because packet flow is suppressed\n"; + return; + } + + trigger_.schedule(); + + access::DataRequest acc_req; + acc_req.ether_type = request.ether_type; + acc_req.source_addr = request.source; + acc_req.destination_addr = request.destination; + acc_req.access_category = dcc::map_profile_onto_ac(request.dcc_profile); + access_.request(acc_req, std::move(packet)); +} + +void DccPassthrough::allow_packet_flow(bool allow) { allow_packet_flow_ = allow; } + +bool DccPassthrough::allow_packet_flow() { return allow_packet_flow_; } diff --git a/src/ethernet_device.cpp b/src/ethernet_device.cpp new file mode 100644 index 0000000..107c067 --- /dev/null +++ b/src/ethernet_device.cpp @@ -0,0 +1,58 @@ +#include "autoware_v2x/ethernet_device.hpp" +#include +#include +#include +#include +#include +#include +#include + +static void initialize(ifreq& request, const char* interface_name) +{ + std::memset(&request, 0, sizeof(ifreq)); + std::strncpy(request.ifr_name, interface_name, IF_NAMESIZE); + request.ifr_name[IF_NAMESIZE - 1] = '\0'; +} + +EthernetDevice::EthernetDevice(const char* devname) : + local_socket_(::socket(AF_LOCAL, SOCK_DGRAM, 0)), + interface_name_(devname) +{ + if (!local_socket_) { + throw std::system_error(errno, std::system_category()); + } +} + +EthernetDevice::~EthernetDevice() +{ + if (local_socket_ >= 0) + ::close(local_socket_); +} + +EthernetDevice::protocol::endpoint EthernetDevice::endpoint(int family) const +{ + sockaddr_ll socket_address = {0}; + socket_address.sll_family = family; + socket_address.sll_protocol = htons(ETH_P_ALL); + socket_address.sll_ifindex = index(); + return protocol::endpoint(&socket_address, sizeof(sockaddr_ll)); +} + +int EthernetDevice::index() const +{ + ifreq data; + initialize(data, interface_name_.c_str()); + ::ioctl(local_socket_, SIOCGIFINDEX, &data); + return data.ifr_ifindex; +} + +vanetza::MacAddress EthernetDevice::address() const +{ + ifreq data; + initialize(data, interface_name_.c_str()); + ::ioctl(local_socket_, SIOCGIFHWADDR, &data); + + vanetza::MacAddress addr; + std::copy_n(data.ifr_hwaddr.sa_data, addr.octets.size(), addr.octets.data()); + return addr; +} diff --git a/src/link_layer.cpp b/src/link_layer.cpp new file mode 100644 index 0000000..21c823a --- /dev/null +++ b/src/link_layer.cpp @@ -0,0 +1,22 @@ +#include "autoware_v2x/link_layer.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) +{ + std::unique_ptr link_layer; + + 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)}); + } + + // Removed Cohda and UDP Support + + return link_layer; +} diff --git a/src/positioning.cpp b/src/positioning.cpp new file mode 100644 index 0000000..5a72e43 --- /dev/null +++ b/src/positioning.cpp @@ -0,0 +1,22 @@ +#include "autoware_v2x/positioning.hpp" +#include + +using namespace vanetza; +namespace po = boost::program_options; + +std::unique_ptr create_position_provider(boost::asio::io_service &io_service, const Runtime &runtime) +{ + std::unique_ptr positioning; + + std::unique_ptr stored{new StoredPositionProvider()}; + PositionFix fix; + fix.timestamp = runtime.now(); + fix.latitude = 10.0 * units::degree; + fix.longitude = 10.0 * units::degree; + fix.confidence.semi_major = 1.0 * units::si::meter; + fix.confidence.semi_minor = fix.confidence.semi_major; + stored->position_fix(fix); + positioning = std::move(stored); + + return positioning; +} \ No newline at end of file diff --git a/src/raw_socket_link.cpp b/src/raw_socket_link.cpp new file mode 100644 index 0000000..802ae62 --- /dev/null +++ b/src/raw_socket_link.cpp @@ -0,0 +1,72 @@ +#include "autoware_v2x/raw_socket_link.hpp" +#include +#include +#include + +using namespace vanetza; + +RawSocketLink::RawSocketLink(boost::asio::generic::raw_protocol::socket && socket) +: socket_(std::move(socket)), + receive_buffer_(2048, 0x00), + receive_endpoint_(socket_.local_endpoint()) +{ + do_receive(); +} + +void RawSocketLink::request( + const access::DataRequest & request, std::unique_ptr packet) +{ + packet->layer(OsiLayer::Link) = + create_ethernet_header(request.destination_addr, request.source_addr, request.ether_type); + transmit(std::move(packet)); +} + +std::size_t RawSocketLink::transmit(std::unique_ptr packet) +{ + std::array const_buffers; + for (auto & layer : osi_layer_range()) { + const auto index = distance(OsiLayer::Physical, layer); + packet->layer(layer).convert(buffers_[index]); + const_buffers[index] = boost::asio::buffer(buffers_[index]); + } + + return socket_.send(const_buffers); +} + +void RawSocketLink::indicate(IndicationCallback callback) { callback_ = callback; } + +void RawSocketLink::do_receive() +{ + namespace sph = std::placeholders; + socket_.async_receive_from( + boost::asio::buffer(receive_buffer_), receive_endpoint_, + std::bind(&RawSocketLink::on_read, this, sph::_1, sph::_2)); +} + +void RawSocketLink::on_read(const boost::system::error_code & ec, std::size_t read_bytes) +{ + if (!ec) { + ByteBuffer buffer(receive_buffer_.begin(), receive_buffer_.begin() + read_bytes); + CohesivePacket packet(std::move(buffer), OsiLayer::Physical); + boost::optional eth = parse_ethernet_header(packet); + if (callback_ && eth) { + callback_(std::move(packet), *eth); + } + do_receive(); + } +} + +boost::optional RawSocketLink::parse_ethernet_header( + vanetza::CohesivePacket & packet) const +{ + packet.set_boundary(OsiLayer::Physical, 0); + if (packet.size(OsiLayer::Link) < EthernetHeader::length_bytes) { + std::cerr << "Router dropped invalid packet (too short for Ethernet header)\n"; + } else { + packet.set_boundary(OsiLayer::Link, EthernetHeader::length_bytes); + auto link_range = packet[OsiLayer::Link]; + return decode_ethernet_header(link_range.begin(), link_range.end()); + } + + return boost::none; +} diff --git a/src/router_context.cpp b/src/router_context.cpp new file mode 100644 index 0000000..5b278c5 --- /dev/null +++ b/src/router_context.cpp @@ -0,0 +1,128 @@ +#include "autoware_v2x/application.hpp" +#include "autoware_v2x/dcc_passthrough.hpp" +#include "autoware_v2x/ethernet_device.hpp" +#include "autoware_v2x/router_context.hpp" +#include "autoware_v2x/time_trigger.hpp" +#include +#include +#include +#include +#include + +using namespace vanetza; + +RouterContext::RouterContext(const geonet::MIB &mib, TimeTrigger &trigger, vanetza::PositionProvider &positioning, vanetza::security::SecurityEntity *security_entity) : mib_(mib), router_(trigger.runtime(), mib_), trigger_(trigger), positioning_(positioning) +{ + router_.packet_dropped = std::bind(&RouterContext::log_packet_drop, this, std::placeholders::_1); + router_.set_address(mib_.itsGnLocalGnAddr); + router_.set_transport_handler(geonet::UpperProtocol::BTP_B, &dispatcher_); + router_.set_security_entity(security_entity); + update_position_vector(); + + trigger_.schedule(); +} + +RouterContext::~RouterContext() +{ + for (auto *app : applications_) + { + disable(app); + } +} + +void RouterContext::updateMIB(const geonet::MIB &mib) { + mib_ = mib; + router_.set_address(mib_.itsGnLocalGnAddr); +} + +void RouterContext::log_packet_drop(geonet::Router::PacketDropReason reason) +{ + auto reason_string = stringify(reason); + std::cout << "Router dropped packet because of " << reason_string << " (" << static_cast(reason) << ")\n"; +} + +void RouterContext::set_link_layer(LinkLayer *link_layer) +{ + namespace dummy = std::placeholders; + + if (link_layer) + { + request_interface_.reset(new DccPassthrough{*link_layer, trigger_}); + router_.set_access_interface(request_interface_.get()); + link_layer->indicate(std::bind(&RouterContext::indicate, this, dummy::_1, dummy::_2)); + update_packet_flow(router_.get_local_position_vector()); + } + else + { + router_.set_access_interface(nullptr); + request_interface_.reset(); + } +} + +void RouterContext::indicate(CohesivePacket &&packet, const EthernetHeader &hdr) +{ + if (hdr.source != mib_.itsGnLocalGnAddr.mid() && hdr.type == access::ethertype::GeoNetworking) + { + std::cout << "received packet from " << hdr.source << " (" << packet.size() << " bytes)\n"; + std::unique_ptr up{new PacketVariant(std::move(packet))}; + trigger_.schedule(); // ensure the clock is up-to-date for the security entity + router_.indicate(std::move(up), hdr.source, hdr.destination); + trigger_.schedule(); // schedule packet forwarding + } +} + +void RouterContext::enable(Application *app) +{ + app->router_ = &router_; + + dispatcher_.add_promiscuous_hook(app->promiscuous_hook()); + if (app->port() != btp::port_type(0)) + { + dispatcher_.set_non_interactive_handler(app->port(), app); + } +} + +void RouterContext::disable(Application *app) +{ + if (app->port() != btp::port_type(0)) + { + dispatcher_.set_non_interactive_handler(app->port(), nullptr); + } + dispatcher_.remove_promiscuous_hook(app->promiscuous_hook()); + + app->router_ = nullptr; +} + +void RouterContext::require_position_fix(bool flag) +{ + require_position_fix_ = flag; + update_packet_flow(router_.get_local_position_vector()); +} + +void RouterContext::update_position_vector() +{ + router_.update_position(positioning_.position_fix()); + vanetza::Runtime::Callback callback = [this](vanetza::Clock::time_point) + { this->update_position_vector(); }; + vanetza::Clock::duration next = std::chrono::seconds(1); + trigger_.runtime().schedule(next, callback); + trigger_.schedule(); + + update_packet_flow(router_.get_local_position_vector()); +} + +void RouterContext::update_packet_flow(const geonet::LongPositionVector &lpv) +{ + if (request_interface_) + { + if (require_position_fix_) + { + // Skip all requests until a valid GPS position is available + request_interface_->allow_packet_flow(lpv.position_accuracy_indicator); + } + else + { + request_interface_->allow_packet_flow(true); + } + } +} diff --git a/src/security.cpp b/src/security.cpp new file mode 100644 index 0000000..3db3d55 --- /dev/null +++ b/src/security.cpp @@ -0,0 +1,74 @@ +#include "autoware_v2x/security.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace vanetza; +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) + { + } + + security::EncapConfirm encapsulate_packet(security::EncapRequest &&request) override + { + if (!entity) + { + throw std::runtime_error("security entity is not ready"); + } + return entity->encapsulate_packet(std::move(request)); + } + + security::DecapConfirm decapsulate_packet(security::DecapRequest &&request) override + { + if (!entity) + { + throw std::runtime_error("security entity is not ready"); + } + return entity->decapsulate_packet(std::move(request)); + } + + void build_entity() + { + 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}); + } + + const Runtime &runtime; + PositionProvider &positioning; + std::unique_ptr backend; + std::unique_ptr entity; + std::unique_ptr cert_provider; + security::DefaultSignHeaderPolicy sign_header_policy; + security::TrustStore trust_store; + security::CertificateCache cert_cache; + security::DefaultCertificateValidator cert_validator; +}; + +std::unique_ptr +create_security_entity(const Runtime &runtime, PositionProvider &positioning) +{ + std::unique_ptr security; + + return security; +} diff --git a/src/time_trigger.cpp b/src/time_trigger.cpp new file mode 100644 index 0000000..d492b8f --- /dev/null +++ b/src/time_trigger.cpp @@ -0,0 +1,42 @@ +#include "autoware_v2x/time_trigger.hpp" +#include +#include +#include + +namespace asio = boost::asio; +namespace posix_time = boost::posix_time; +using namespace vanetza; + +TimeTrigger::TimeTrigger(asio::io_service & io_service) +: io_service_(io_service), timer_(io_service), runtime_(Clock::at(now())) +{ + std::cout << "Starting runtime at " << now() << "\n"; + schedule(); +} + +posix_time::ptime TimeTrigger::now() const { return posix_time::microsec_clock::universal_time(); } + +void TimeTrigger::schedule() +{ + update_runtime(); + auto next = runtime_.next(); + if (next < Clock::time_point::max()) { + timer_.expires_at(Clock::at(next)); + timer_.async_wait(std::bind(&TimeTrigger::on_timeout, this, std::placeholders::_1)); + } else { + timer_.cancel(); + } +} + +void TimeTrigger::on_timeout(const boost::system::error_code & ec) +{ + if (asio::error::operation_aborted != ec) { + schedule(); + } +} + +void TimeTrigger::update_runtime() +{ + auto current_time = now(); + runtime_.trigger(Clock::at(current_time)); +}