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