newer files
This commit is contained in:
Yu Asabe
commit
fd33d987ba
|
|
@ -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
|
||||||
|
)
|
||||||
|
|
@ -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
|
||||||
|
|
@ -0,0 +1,41 @@
|
||||||
|
#ifndef APPLICATION_HPP_PSIGPUTG
|
||||||
|
#define APPLICATION_HPP_PSIGPUTG
|
||||||
|
|
||||||
|
#include <vanetza/btp/data_interface.hpp>
|
||||||
|
#include <vanetza/btp/data_indication.hpp>
|
||||||
|
#include <vanetza/btp/data_request.hpp>
|
||||||
|
#include <vanetza/btp/port_dispatcher.hpp>
|
||||||
|
#include <vanetza/geonet/data_confirm.hpp>
|
||||||
|
#include <vanetza/geonet/router.hpp>
|
||||||
|
#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 */
|
||||||
|
|
@ -0,0 +1,23 @@
|
||||||
|
#ifndef CPM_APPLICATION_HPP_EUIC2VFR
|
||||||
|
#define CPM_APPLICATION_HPP_EUIC2VFR
|
||||||
|
|
||||||
|
#include "autoware_v2x/application.hpp"
|
||||||
|
#include "rclcpp/rclcpp.hpp"
|
||||||
|
#include <boost/asio/io_service.hpp>
|
||||||
|
#include <boost/asio/steady_timer.hpp>
|
||||||
|
#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 */
|
||||||
|
|
@ -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 <boost/asio/io_service.hpp>
|
||||||
|
#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<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr subscription_;
|
||||||
|
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_pos_;
|
||||||
|
|
||||||
|
std::unique_ptr<CpmApplication> cp_;
|
||||||
|
std::unique_ptr<Application> app_;
|
||||||
|
boost::asio::io_service io_service_;
|
||||||
|
TimeTrigger trigger_;
|
||||||
|
const char *device_name_;
|
||||||
|
EthernetDevice device_;
|
||||||
|
vanetza::MacAddress mac_address_;
|
||||||
|
std::unique_ptr<LinkLayer> link_layer_;
|
||||||
|
vanetza::geonet::MIB mib_;
|
||||||
|
std::unique_ptr<vanetza::PositionProvider> positioning_;
|
||||||
|
std::unique_ptr<vanetza::security::SecurityEntity> security_;
|
||||||
|
RouterContext context_;
|
||||||
|
|
||||||
|
double pos_lat_;
|
||||||
|
double pos_lon_;
|
||||||
|
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,26 @@
|
||||||
|
#ifndef DCC_PASSTHROUGH_HPP_GSDFESAE
|
||||||
|
#define DCC_PASSTHROUGH_HPP_GSDFESAE
|
||||||
|
|
||||||
|
#include "autoware_v2x/time_trigger.hpp"
|
||||||
|
#include <vanetza/access/interface.hpp>
|
||||||
|
#include <vanetza/dcc/data_request.hpp>
|
||||||
|
#include <vanetza/dcc/interface.hpp>
|
||||||
|
#include <vanetza/net/cohesive_packet.hpp>
|
||||||
|
|
||||||
|
class DccPassthrough : public vanetza::dcc::RequestInterface
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
DccPassthrough(vanetza::access::Interface&, TimeTrigger& trigger);
|
||||||
|
|
||||||
|
void request(const vanetza::dcc::DataRequest& request, std::unique_ptr<vanetza::ChunkPacket> 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 */
|
||||||
|
|
@ -0,0 +1,29 @@
|
||||||
|
#ifndef ETHERNET_DEVICE_HPP_NEVC5DAY
|
||||||
|
#define ETHERNET_DEVICE_HPP_NEVC5DAY
|
||||||
|
|
||||||
|
#include <vanetza/net/mac_address.hpp>
|
||||||
|
#include <boost/asio/generic/raw_protocol.hpp>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
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 */
|
||||||
|
|
||||||
|
|
@ -0,0 +1,29 @@
|
||||||
|
#ifndef LINK_LAYER_HPP_FGEK0QTH
|
||||||
|
#define LINK_LAYER_HPP_FGEK0QTH
|
||||||
|
|
||||||
|
#include "autoware_v2x/ethernet_device.hpp"
|
||||||
|
#include <vanetza/access/interface.hpp>
|
||||||
|
#include <vanetza/net/cohesive_packet.hpp>
|
||||||
|
#include <vanetza/net/ethernet_header.hpp>
|
||||||
|
#include <boost/asio/io_service.hpp>
|
||||||
|
#include <memory>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
class LinkLayerIndication
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
using IndicationCallback = std::function<void(vanetza::CohesivePacket&&, const vanetza::EthernetHeader&)>;
|
||||||
|
|
||||||
|
virtual void indicate(IndicationCallback) = 0;
|
||||||
|
virtual ~LinkLayerIndication() = default;
|
||||||
|
};
|
||||||
|
|
||||||
|
class LinkLayer : public vanetza::access::Interface, public LinkLayerIndication
|
||||||
|
{
|
||||||
|
};
|
||||||
|
|
||||||
|
std::unique_ptr<LinkLayer>
|
||||||
|
create_link_layer(boost::asio::io_service&, const EthernetDevice&, const std::string& name);
|
||||||
|
|
||||||
|
#endif /* LINK_LAYER_HPP_FGEK0QTH */
|
||||||
|
|
||||||
|
|
@ -0,0 +1,20 @@
|
||||||
|
#ifndef POSITIONING_HPP_VZRIW7PB
|
||||||
|
#define POSITIONING_HPP_VZRIW7PB
|
||||||
|
|
||||||
|
#include <vanetza/common/position_provider.hpp>
|
||||||
|
#include <vanetza/common/runtime.hpp>
|
||||||
|
#include <boost/asio/io_service.hpp>
|
||||||
|
#include <boost/program_options/options_description.hpp>
|
||||||
|
#include <boost/program_options/variables_map.hpp>
|
||||||
|
#include <memory>
|
||||||
|
#include <stdexcept>
|
||||||
|
|
||||||
|
class PositioningException : public std::runtime_error
|
||||||
|
{
|
||||||
|
using std::runtime_error::runtime_error;
|
||||||
|
};
|
||||||
|
|
||||||
|
std::unique_ptr<vanetza::PositionProvider>
|
||||||
|
create_position_provider(boost::asio::io_service &, const vanetza::Runtime &);
|
||||||
|
|
||||||
|
#endif /* POSITIONING_HPP_VZRIW7PB */
|
||||||
|
|
@ -0,0 +1,38 @@
|
||||||
|
#ifndef RAW_SOCKET_LINK_HPP_VUXH507U
|
||||||
|
#define RAW_SOCKET_LINK_HPP_VUXH507U
|
||||||
|
|
||||||
|
#include "autoware_v2x/link_layer.hpp"
|
||||||
|
#include <vanetza/access/interface.hpp>
|
||||||
|
#include <vanetza/net/ethernet_header.hpp>
|
||||||
|
#include <boost/asio/generic/raw_protocol.hpp>
|
||||||
|
#include <boost/optional/optional.hpp>
|
||||||
|
#include <array>
|
||||||
|
#include <functional>
|
||||||
|
|
||||||
|
class RawSocketLink : public LinkLayer
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
RawSocketLink(boost::asio::generic::raw_protocol::socket&&);
|
||||||
|
void request(const vanetza::access::DataRequest&, std::unique_ptr<vanetza::ChunkPacket>) override;
|
||||||
|
void indicate(IndicationCallback) override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
std::size_t transmit(std::unique_ptr<vanetza::ChunkPacket>);
|
||||||
|
virtual boost::optional<vanetza::EthernetHeader> 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<vanetza::ByteBuffer, layers_> buffers_;
|
||||||
|
IndicationCallback callback_;
|
||||||
|
vanetza::ByteBuffer receive_buffer_;
|
||||||
|
boost::asio::generic::raw_protocol::endpoint receive_endpoint_;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif /* RAW_SOCKET_LINK_HPP_VUXH507U */
|
||||||
|
|
||||||
|
|
@ -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 <vanetza/btp/port_dispatcher.hpp>
|
||||||
|
#include <vanetza/common/position_provider.hpp>
|
||||||
|
#include <vanetza/geonet/mib.hpp>
|
||||||
|
#include <vanetza/geonet/router.hpp>
|
||||||
|
#include <array>
|
||||||
|
#include <list>
|
||||||
|
#include <memory>
|
||||||
|
|
||||||
|
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<DccPassthrough> request_interface_;
|
||||||
|
std::list<Application*> applications_;
|
||||||
|
bool require_position_fix_ = false;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif /* ROUTER_CONTEXT_HPP_KIPUYBY2 */
|
||||||
|
|
@ -0,0 +1,15 @@
|
||||||
|
#ifndef SECURITY_HPP_FV13ZIYA
|
||||||
|
#define SECURITY_HPP_FV13ZIYA
|
||||||
|
|
||||||
|
#include <vanetza/common/position_provider.hpp>
|
||||||
|
#include <vanetza/common/runtime.hpp>
|
||||||
|
#include <vanetza/security/security_entity.hpp>
|
||||||
|
#include <boost/program_options/options_description.hpp>
|
||||||
|
#include <boost/program_options/variables_map.hpp>
|
||||||
|
#include <memory>
|
||||||
|
|
||||||
|
std::unique_ptr<vanetza::security::SecurityEntity>
|
||||||
|
create_security_entity(const vanetza::Runtime&, vanetza::PositionProvider&);
|
||||||
|
|
||||||
|
#endif /* SECURITY_HPP_FV13ZIYA */
|
||||||
|
|
||||||
|
|
@ -0,0 +1,26 @@
|
||||||
|
#ifndef TIME_TRIGGER_HPP_XRPGDYXO
|
||||||
|
#define TIME_TRIGGER_HPP_XRPGDYXO
|
||||||
|
|
||||||
|
#include <boost/asio/deadline_timer.hpp>
|
||||||
|
#include <boost/asio/io_service.hpp>
|
||||||
|
#include <boost/date_time/posix_time/posix_time_types.hpp>
|
||||||
|
#include <vanetza/common/manual_runtime.hpp>
|
||||||
|
|
||||||
|
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 */
|
||||||
|
|
@ -0,0 +1,3 @@
|
||||||
|
<launch>
|
||||||
|
<node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen" emulate_tty="true" />
|
||||||
|
</launch>
|
||||||
|
|
@ -0,0 +1,26 @@
|
||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>autoware_v2x</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>TODO: Package description</description>
|
||||||
|
<maintainer email="yuasabe@g.ecc.u-tokyo.ac.jp">yuasabe</maintainer>
|
||||||
|
<license>TODO: License declaration</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake_auto</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>autoware_perception_msgs</depend>
|
||||||
|
<depend>tf2_msgs</depend>
|
||||||
|
<depend>geometry_msgs</depend>
|
||||||
|
<depend>rclcpp</depend>
|
||||||
|
<depend>std_msgs</depend>
|
||||||
|
<depend>rclcpp_components</depend>
|
||||||
|
<depend>Vanetza</depend>
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
|
|
@ -0,0 +1,76 @@
|
||||||
|
#include "autoware_v2x/application.hpp"
|
||||||
|
#include "rclcpp/rclcpp.hpp"
|
||||||
|
#include <vanetza/btp/header.hpp>
|
||||||
|
#include <vanetza/btp/header_conversion.hpp>
|
||||||
|
#include <cassert>
|
||||||
|
|
||||||
|
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<uint16_t>(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<geonet::Area>(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;
|
||||||
|
}
|
||||||
|
|
@ -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 <vanetza/btp/ports.hpp>
|
||||||
|
#include <vanetza/asn1/cpm.hpp>
|
||||||
|
#include <vanetza/asn1/packet_visitor.hpp>
|
||||||
|
#include <vanetza/facilities/cpm_functions.hpp>
|
||||||
|
#include <chrono>
|
||||||
|
#include <functional>
|
||||||
|
#include <iostream>
|
||||||
|
#include <sstream>
|
||||||
|
#include <exception>
|
||||||
|
|
||||||
|
// 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<asn1::Cpm> visitor;
|
||||||
|
std::shared_ptr<const asn1::Cpm> 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<milliseconds>(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<StationDataContainer_t>();
|
||||||
|
// 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<PerceivedObjectContainer_t>();
|
||||||
|
// 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<PerceivedObject>();
|
||||||
|
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<geonet::DownPacket> payload { new geonet::DownPacket() };
|
||||||
|
// std::shared_ptr<asn1::Cpm> message_p = std::make_shared<asn1::Cpm>(message);
|
||||||
|
// std::unique_ptr<convertible::byte_buffer> buffer { new convertible::byte_buffer_impl<asn1::Cpm>(&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();
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
|
@ -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 <vanetza/asn1/cpm.hpp>
|
||||||
|
#include <vanetza/facilities/cpm_functions.hpp>
|
||||||
|
#include <sstream>
|
||||||
|
#include <memory>
|
||||||
|
#include <GeographicLib/UTMUPS.hpp>
|
||||||
|
#include <GeographicLib/MGRS.hpp>
|
||||||
|
|
||||||
|
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<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/detection/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
|
||||||
|
|
||||||
|
subscription_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/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<CpmApplication> 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)
|
||||||
|
|
@ -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 <vanetza/asn1/cpm.hpp>
|
||||||
|
#include <vanetza/facilities/cpm_functions.hpp>
|
||||||
|
#include <sstream>
|
||||||
|
#include <memory>
|
||||||
|
#include <GeographicLib/UTMUPS.hpp>
|
||||||
|
#include <GeographicLib/MGRS.hpp>
|
||||||
|
#include <boost/thread.hpp>
|
||||||
|
|
||||||
|
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<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/detection/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
|
||||||
|
|
||||||
|
subscription_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/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<CpmApplication> 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)
|
||||||
|
|
@ -0,0 +1,36 @@
|
||||||
|
#include "autoware_v2x/dcc_passthrough.hpp"
|
||||||
|
#include <iostream>
|
||||||
|
#include <vanetza/access/data_request.hpp>
|
||||||
|
#include <vanetza/dcc/data_request.hpp>
|
||||||
|
#include <vanetza/dcc/interface.hpp>
|
||||||
|
#include <vanetza/dcc/mapping.hpp>
|
||||||
|
#include <vanetza/net/chunk_packet.hpp>
|
||||||
|
#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<ChunkPacket> 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_; }
|
||||||
|
|
@ -0,0 +1,58 @@
|
||||||
|
#include "autoware_v2x/ethernet_device.hpp"
|
||||||
|
#include <algorithm>
|
||||||
|
#include <cstring>
|
||||||
|
#include <system_error>
|
||||||
|
#include <linux/if_ether.h>
|
||||||
|
#include <linux/if_packet.h>
|
||||||
|
#include <net/if.h>
|
||||||
|
#include <sys/ioctl.h>
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,22 @@
|
||||||
|
#include "autoware_v2x/link_layer.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)
|
||||||
|
{
|
||||||
|
std::unique_ptr<LinkLayer> 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;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,22 @@
|
||||||
|
#include "autoware_v2x/positioning.hpp"
|
||||||
|
#include <vanetza/common/stored_position_provider.hpp>
|
||||||
|
|
||||||
|
using namespace vanetza;
|
||||||
|
namespace po = boost::program_options;
|
||||||
|
|
||||||
|
std::unique_ptr<vanetza::PositionProvider> create_position_provider(boost::asio::io_service &io_service, const Runtime &runtime)
|
||||||
|
{
|
||||||
|
std::unique_ptr<vanetza::PositionProvider> positioning;
|
||||||
|
|
||||||
|
std::unique_ptr<StoredPositionProvider> 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;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,72 @@
|
||||||
|
#include "autoware_v2x/raw_socket_link.hpp"
|
||||||
|
#include <iostream>
|
||||||
|
#include <vanetza/access/data_request.hpp>
|
||||||
|
#include <vanetza/net/ethernet_header.hpp>
|
||||||
|
|
||||||
|
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<ChunkPacket> 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<ChunkPacket> packet)
|
||||||
|
{
|
||||||
|
std::array<boost::asio::const_buffer, layers_> const_buffers;
|
||||||
|
for (auto & layer : osi_layer_range<OsiLayer::Physical, OsiLayer::Application>()) {
|
||||||
|
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<EthernetHeader> eth = parse_ethernet_header(packet);
|
||||||
|
if (callback_ && eth) {
|
||||||
|
callback_(std::move(packet), *eth);
|
||||||
|
}
|
||||||
|
do_receive();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
boost::optional<EthernetHeader> 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;
|
||||||
|
}
|
||||||
|
|
@ -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 <vanetza/access/ethertype.hpp>
|
||||||
|
#include <vanetza/dcc/data_request.hpp>
|
||||||
|
#include <vanetza/dcc/interface.hpp>
|
||||||
|
#include <iostream>
|
||||||
|
#include <vanetza/common/byte_order.hpp>
|
||||||
|
|
||||||
|
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<int>(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<PacketVariant> 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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,74 @@
|
||||||
|
#include "autoware_v2x/security.hpp"
|
||||||
|
#include <vanetza/security/certificate_cache.hpp>
|
||||||
|
#include <vanetza/security/default_certificate_validator.hpp>
|
||||||
|
#include <vanetza/security/delegating_security_entity.hpp>
|
||||||
|
#include <vanetza/security/naive_certificate_provider.hpp>
|
||||||
|
#include <vanetza/security/null_certificate_validator.hpp>
|
||||||
|
#include <vanetza/security/persistence.hpp>
|
||||||
|
#include <vanetza/security/sign_header_policy.hpp>
|
||||||
|
#include <vanetza/security/static_certificate_provider.hpp>
|
||||||
|
#include <vanetza/security/trust_store.hpp>
|
||||||
|
#include <stdexcept>
|
||||||
|
|
||||||
|
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<security::Backend> backend;
|
||||||
|
std::unique_ptr<security::SecurityEntity> entity;
|
||||||
|
std::unique_ptr<security::CertificateProvider> cert_provider;
|
||||||
|
security::DefaultSignHeaderPolicy sign_header_policy;
|
||||||
|
security::TrustStore trust_store;
|
||||||
|
security::CertificateCache cert_cache;
|
||||||
|
security::DefaultCertificateValidator cert_validator;
|
||||||
|
};
|
||||||
|
|
||||||
|
std::unique_ptr<security::SecurityEntity>
|
||||||
|
create_security_entity(const Runtime &runtime, PositionProvider &positioning)
|
||||||
|
{
|
||||||
|
std::unique_ptr<security::SecurityEntity> security;
|
||||||
|
|
||||||
|
return security;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,42 @@
|
||||||
|
#include "autoware_v2x/time_trigger.hpp"
|
||||||
|
#include <boost/date_time/posix_time/posix_time.hpp>
|
||||||
|
#include <functional>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
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));
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue