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

Compare commits

base: TiagoRG:e9b85635a381fd80ae66f7d8a202a4427fbcc1f7
Branches Tags
TiagoRG:feat/camapp
..
compare: TiagoRG:73756de59ec94188a4ea8617916cbddaeb0f309f
Branches Tags
TiagoRG:feat/camapp

No commits in common. "e9b85635a381fd80ae66f7d8a202a4427fbcc1f7" and "73756de59ec94188a4ea8617916cbddaeb0f309f" have entirely different histories.
e9b85635a3 ... 73756de59e

19 changed files with 229 additions and 747 deletions
Show Stats Expand all files Collapse all files

14
CMakeLists.txt
View File

@ -22,17 +22,12 @@ 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 program_options REQUIRED)
find_package(etsi_its_cam_ts_coding REQUIRED)
find_package(etsi_its_cam_ts_conversion REQUIRED)
find_package(etsi_its_msgs_utils REQUIRED)
find_package(Boost COMPONENTS thread REQUIRED)
ament_auto_find_build_dependencies()
find_package(std_msgs REQUIRED)
find_package(Protobuf REQUIRED)
include_directories(
include
${CMAKE_CURRENT_BINARY_DIR}/src
)
ament_auto_add_library(autoware_v2x SHARED
@ -41,7 +36,6 @@ ament_auto_add_library(autoware_v2x SHARED
src/application.cpp
src/cpm_application.cpp
src/cam_application.cpp
src/cube_evk_link.cpp
src/ethernet_device.cpp
src/link_layer.cpp
src/raw_socket_link.cpp
@ -52,9 +46,7 @@ ament_auto_add_library(autoware_v2x SHARED
src/security.cpp
)
protobuf_generate(TARGET autoware_v2x PROTOS src/cube_evk_radio.proto)
target_link_libraries(autoware_v2x Vanetza::vanetza ${GeographicLib_LIBRARIES} Boost::thread Boost::program_options sqlite3 etsi_its_cam_ts_coding::etsi_its_cam_ts_coding etsi_its_cam_ts_conversion::etsi_its_cam_ts_conversion protobuf::libprotobuf)
target_link_libraries(autoware_v2x Vanetza::vanetza ${GeographicLib_LIBRARIES} Boost::thread sqlite3)
rclcpp_components_register_node(autoware_v2x
PLUGIN "v2x::V2XNode"
@ -69,4 +61,4 @@ endif()
ament_auto_package(INSTALL_TO_SHARE
launch
config
)
)

11
config/autoware_v2x.param.yaml
View File

@ -1,11 +1,4 @@
/**:
ros__parameters:
link_layer: "cube-evk"
network_interface: "v2x_testing"
cube_ip: "192.168.94.84"
is_sender: true
security: "none"
certificate: ""
certificate-key: ""
certificate-chain:
- ""
network_interface: "wlp4s0"
is_sender: true

42
docs/design/index.md
View File

@ -17,37 +17,33 @@ The V2XNode launches a ROS2 Node for AutowareV2X. Its main purpose is to act as
### Input
| Name | Type | Description |
|------------------------------------------|--------------------------------------------------------|---------------------------------------------|
| `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Perceived Objects by Autoware |
| `/tf` | `tf2_msgs::msg::TFMessage` | Pose of Ego Vehicle |
| `/vehicle/status/velocity_report` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | Velocity of Ego Vehicle |
| `/api/vehicle/status` | `autoware_adapi_v1_msgs::msg::VehicleStatus` | Vehicle Status (gear, steering angle, etc.) |
| `/api/vehicle/dimensions` | `autoware_adapi_v1_msgs::srv::GetVehicleDimensions` | Service to get Vehicle Dimensions |
| Name | Type | Description |
| -------------------- | ------------------------------- | ---------------- |
| `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Perceived Objects by Autoware |
| `/tf` | `tf2_msgs::msg::TFMessage` | Pose of Ego Vehicle |
| `/vehicle/status/velocity_report` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | Velocity of Ego Vehicle |
| `/api/vehicle/status` | `autoware_adapi_v1_msgs::msg::VehicleStatus` | Vehicle Status (gear, steering angle, etc.) |
| `/api/vehicle/dimensions` | `autoware_adapi_v1_msgs::srv::GetVehicleDimensions` | Service to get Vehicle Dimensions |
### Output
| Name | Type | Description |
|------------------------|--------------------------------------------------------|--------------------------|
| `/v2x/cpm/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Objects received by CPMs |
| `/v2x/cam_ts/received` | `etsi_its_cam_ts_msgs::msg::CAM` | Received CAMs |
| Name | Type | Description |
| -------------------- | ------------------------------- | ---------------- |
| `/v2x/cpm/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Objects received by CPMs |
### Functions
| Name | Description |
|---------------------------------------------------------------------------------------------------|------------------------------------------------------------|
| `objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg)` | Call `V2XApp::objectsCallback` |
| `tfCallback` | Call `V2XApp::tfCallback` |
| `velocityReportCallback` | Call `V2XApp::velocityReportCallback` |
| `vehicleStatusCallback` | Call `V2XApp::vehicleStatusCallback` |
| `getVehicleDimensions` | Sends a request to the service used for vehicle dimensions |
| `publishObjects(std::vector<CpmApplication::Object> *objectsStack, int cpm_num)` | |
| `publishCpmSenderObject` | Not used now |
| `publishReceivedCam(etsi_its_cam_ts_msgs::msg::CAM &msg)` | Publishes a received CAM into the ROS environment |
| Name | Description |
| -------------------- | ---------------- |
| `objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg)` | Call `V2XApp::objectsCallback` |
| `tfCallback` | Call `V2XApp::tfCallback` |
| `velocityReportCallback` | Call `V2XApp::velocityReportCallback` |
| `vehicleStatusCallback` | Call `V2XApp::vehicleStatusCallback` |
| `getVehicleDimensions` | Sends a request to the service used for vehicle dimensions |
| `publishObjects(std::vector<CpmApplication::Object> *objectsStack, int cpm_num)` | |
| `publishCpmSenderObject` | Not used now |
## V2XApp
## CPM Facility
## CAM Facility

69
include/autoware_v2x/cam_application.hpp
View File

@ -2,20 +2,14 @@
#define CAM_APPLICATION_HPP_EUIC2VFR
#include "autoware_v2x/application.hpp"
#include "autoware_v2x/positioning.hpp"
#include "rclcpp/rclcpp.hpp"
#include <vanetza/asn1/cam.hpp>
#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp"
#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
#include <boost/asio/io_service.hpp>
#include <boost/asio/steady_timer.hpp>
#include <etsi_its_cam_ts_coding/cam_ts_CAM.h>
#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp"
#include "autoware_adapi_v1_msgs/msg/vehicle_status.hpp"
#include "autoware_v2x/positioning.hpp"
#include <vanetza/asn1/cam.hpp>
namespace v2x
{
@ -28,34 +22,32 @@ public:
void indicate(const DataIndication &, UpPacketPtr) override;
void set_interval(vanetza::Clock::duration);
void updateMGRS(double *, double *);
void updateRP(const double *, const double *, const double *);
void updateHeading(const double *);
void updateRP(double *, double *, double *);
void updateGenerationDeltaTime(int *, long *);
void updateHeading(double *);
void setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &);
void updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr);
void updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr);
void updateSteeringReport(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr);
void updateVehicleStatus(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr);
void send();
private:
void calc_interval();
void schedule_timer();
void on_timer(vanetza::Clock::time_point);
static void build_etsi_its_cam_ts_from_vanetza(vanetza::asn1::Cam &, cam_ts_CAM_t &);
V2XNode *node_;
vanetza::Runtime &runtime_;
vanetza::Clock::duration cam_interval_{};
vanetza::Clock::duration cam_interval_;
struct VehicleDimensions {
float wheel_radius = 0.0;
float wheel_width = 0.0;
float wheel_base = 0.0;
float wheel_tread = 0.0;
float front_overhang = 0.0;
float rear_overhang = 0.0;
float left_overhang = 0.0;
float right_overhang = 0.0;
float height = 0.0;
float wheel_radius;
float wheel_width;
float wheel_base;
float wheel_tread;
float front_overhang;
float rear_overhang;
float left_overhang;
float right_overhang;
float height;
};
VehicleDimensions vehicleDimensions_;
@ -85,17 +77,17 @@ private:
return deque.size();
}
[[nodiscard]] double getMean() const {
double getMean() {
return this->mean;
}
using iterator = std::deque<double>::const_iterator;
[[nodiscard]] iterator begin() const {
iterator begin() const {
return deque.begin();
}
[[nodiscard]] iterator end() const {
iterator end() const {
return deque.end();
}
@ -117,9 +109,9 @@ private:
PositionsDeque x;
PositionsDeque y;
double semiMajorConfidence{};
double semiMinorConfidence{};
double semiMajorOrientation{};
double semiMajorConfidence;
double semiMinorConfidence;
double semiMajorOrientation;
};
PositionConfidenceEllipse positionConfidenceEllipse_;
@ -128,7 +120,6 @@ private:
float heading_rate;
float lateral_velocity;
float longitudinal_velocity;
float speed;
float longitudinal_acceleration;
};
VelocityReport velocityReport_;
@ -139,10 +130,20 @@ private:
};
VehicleStatus vehicleStatus_;
int generationDeltaTime_;
long gdt_timestamp_;
double objectConfidenceThreshold_;
bool updating_velocity_report_;
bool updating_vehicle_status_;
bool sending_;
bool is_sender_;
bool reflect_packet_;
unsigned long stationId_;
int cam_num_;
int received_cam_num_;
bool use_dynamic_generation_rules_;
};

43
include/autoware_v2x/cube_evk_link.hpp
View File

@ -1,43 +0,0 @@
#ifndef CUBE_EVK_LINK_HPP_
#define CUBE_EVK_LINK_HPP_
#include "autoware_v2x/link_layer.hpp"
#include <boost/asio/io_service.hpp>
#include <boost/asio/ip/udp.hpp>
#include <vanetza/net/chunk_packet.hpp>
#include <array>
#include <cstdint>
#include <memory>
class LinkLayerReception;
class LinkLayerTransmission;
class CubeEvkLink : public LinkLayer
{
public:
CubeEvkLink(boost::asio::io_service&, boost::asio::ip::address_v4);
void handle_packet_received(const boost::system::error_code&, size_t);
void request(const vanetza::access::DataRequest&, std::unique_ptr<vanetza::ChunkPacket>) override;
void indicate(IndicationCallback callback) override;
private:
boost::asio::io_service& io_;
IndicationCallback indicate_to_router_;
boost::asio::ip::udp::socket tx_socket_;
boost::asio::ip::udp::socket rx_socket_;
boost::asio::ip::udp::endpoint host_endpoint_;
std::array<uint8_t, 4096> received_data_;
static constexpr unsigned int cube_evk_radio_port_tx = 33210;
static constexpr unsigned int cube_evk_radio_port_rx = 33211;
void pass_message_to_router(std::unique_ptr<LinkLayerReception>);
std::unique_ptr<LinkLayerTransmission> create_link_layer_tx(const vanetza::access::DataRequest&, std::unique_ptr<vanetza::ChunkPacket>);
};
#endif /* CUBE_EVK_LINK_HPP_ */

3
include/autoware_v2x/link_layer.hpp
View File

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

3
include/autoware_v2x/security.hpp
View File

@ -9,6 +9,7 @@
#include <memory>
std::unique_ptr<vanetza::security::SecurityEntity>
create_security_entity(const boost::program_options::variables_map&, const vanetza::Runtime&, vanetza::PositionProvider&);
create_security_entity(const vanetza::Runtime&, vanetza::PositionProvider&);
#endif /* SECURITY_HPP_FV13ZIYA */

13
include/autoware_v2x/v2x_app.hpp
View File

@ -5,8 +5,9 @@
#include "std_msgs/msg/string.hpp"
#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
#include "autoware_adapi_v1_msgs/srv/get_vehicle_dimensions.hpp"
#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp"
#include "autoware_adapi_v1_msgs/msg/vehicle_status.hpp"
#include "tf2_msgs/msg/tf_message.hpp"
#include <boost/asio/io_service.hpp>
#include "autoware_v2x/cpm_application.hpp"
@ -28,9 +29,9 @@ namespace v2x
V2XApp(V2XNode *);
void start();
void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr);
void setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &);
void velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr);
void gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr);
void steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr);
void vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr);
void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr);
CpmApplication *cp;
@ -44,6 +45,10 @@ namespace v2x
V2XNode* node_;
bool tf_received_;
int tf_interval_;
bool velocity_report_received_;
int velocity_report_interval_;
bool vehicle_status_received_;
int vehicle_status_interval_;
bool vehicle_dimensions_set_;
bool cp_started_;
bool cam_started_;

19
include/autoware_v2x/v2x_node.hpp
View File

@ -5,10 +5,7 @@
#include "std_msgs/msg/string.hpp"
#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
#include "autoware_adapi_v1_msgs/srv/get_vehicle_dimensions.hpp"
#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp"
#include "autoware_adapi_v1_msgs/msg/vehicle_status.hpp"
#include "tf2_msgs/msg/tf_message.hpp"
#include <boost/asio/io_service.hpp>
#include "autoware_v2x/v2x_app.hpp"
@ -22,8 +19,6 @@
#include "autoware_v2x/router_context.hpp"
#include <fstream>
#include <etsi_its_cam_ts_msgs/msg/cam.hpp>
namespace v2x
{
class V2XNode : public rclcpp::Node
@ -33,32 +28,26 @@ namespace v2x
V2XApp *app;
void publishObjects(std::vector<CpmApplication::Object> *, int cpm_num);
void publishCpmSenderObject(double, double, double);
void publishReceivedCam(etsi_its_cam_ts_msgs::msg::CAM &);
void getVehicleDimensions();
std::ofstream latency_log_file;
private:
void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg);
void velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg);
void gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg);
void steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg);
void vehicleStatucCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg);
void getVehicleDimensions();
void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg);
rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr objects_sub_;
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>::SharedPtr velocity_report_sub_;
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::GearReport>::SharedPtr gear_report_sub_;
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>::SharedPtr steering_report_sub_;
rclcpp::Subscription<autoware_adapi_v1_msgs::msg::VehicleStatus>::SharedPtr vehicle_status_sub_;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_sub_;
rclcpp::Client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>::SharedPtr get_vehicle_dimensions_;
rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cpm_objects_pub_;
rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cpm_sender_pub_;
rclcpp::Publisher<etsi_its_cam_ts_msgs::msg::CAM>::SharedPtr cam_rec_pub_;
double pos_lat_;
double pos_lon_;
bool vehicle_dimensions_available_;
};
}

7
launch/v2x.launch.xml
View File

@ -1,10 +1,7 @@
<launch>
<arg name="link_layer" default="ethernet"/>
<arg name="network_interface" default="v2x_testing"/>
<arg name="cube_ip" default="127.0.0.1"/>
<arg name="network_interface" default="wlp5s0"/>
<arg name="is_sender" default="true"/>
<arg name="security" default="none"/>
<node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen">
<param from="$(find-pkg-share autoware_v2x)/config/autoware_v2x.param.yaml"/>
</node>
</launch>
</launch>

3
package.xml
View File

@ -12,14 +12,13 @@
<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_adapi_v1_msgs</depend>
<depend>tf2_msgs</depend>
<depend>tf2_msgs</depend>
<depend>tf2</depend>
<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>rclcpp_components</depend>
<depend>Vanetza</depend>
<depend>etsi_its_messages</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

215
src/cam_application.cpp
View File

@ -11,7 +11,6 @@
#include <vanetza/btp/ports.hpp>
#include <vanetza/asn1/cam.hpp>
#include <vanetza/asn1/packet_visitor.hpp>
#include <vanetza/facilities/cam_functions.hpp>
#include <chrono>
#include <functional>
#include <iostream>
@ -22,14 +21,14 @@
#include <GeographicLib/MGRS.hpp>
#include <string>
#include <etsi_its_cam_ts_msgs/msg/cam.hpp>
#include <etsi_its_cam_ts_conversion/convertCAM.h>
#include <boost/units/cmath.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include <sqlite3.h>
#define _USE_MATH_DEFINES
#include <math.h>
using namespace vanetza;
using namespace std::chrono;
@ -38,18 +37,24 @@ namespace v2x
CamApplication::CamApplication(V2XNode * node, Runtime & rt, bool is_sender)
: node_(node),
runtime_(rt),
cam_interval_(milliseconds(1000)),
vehicleDimensions_(),
ego_(),
positionConfidenceEllipse_(),
velocityReport_(),
vehicleStatus_(),
generationDeltaTime_(0),
updating_velocity_report_(false),
updating_vehicle_status_(false),
sending_(false),
is_sender_(is_sender),
use_dynamic_generation_rules_(true)
reflect_packet_(false),
objectConfidenceThreshold_(0.0),
cam_num_(0),
received_cam_num_(0),
use_dynamic_generation_rules_(false)
{
RCLCPP_INFO(node_->get_logger(), "CamApplication started. is_sender: %d", is_sender_);
set_interval(cam_interval_);
set_interval(milliseconds(100));
//createTables();
// Generate ID for this station
@ -57,8 +62,6 @@ namespace v2x
std::mt19937 gen(rd());
std::uniform_int_distribution<unsigned long> dis(0, 4294967295);
stationId_ = dis(gen);
node_->getVehicleDimensions();
}
void CamApplication::set_interval(Clock::duration interval) {
@ -67,72 +70,11 @@ namespace v2x
schedule_timer();
}
static double calc_haversine(double lat1, double lon1, double lat2, double lon2) {
const double R = 6371e3;
double dLat = (lat2 - lat1) * M_PI / 180;
double dLon = (lon2 - lon1) * M_PI / 180;
lat1 = lat1 * (M_PI / 180);
lat2 = lat2 * (M_PI / 180);
double a = std::sin(dLat / 2) * std::sin(dLat / 2) +
std::sin(dLon / 2) * std::sin(dLon / 2) * std::cos(lat1) * std::cos(lat2);
double c = 2 * std::atan2(std::sqrt(a), std::sqrt(1 - a));
return R * c;
}
void CamApplication::calc_interval() {
if (use_dynamic_generation_rules_) {
static struct {
double speed;
double heading;
double latitude;
double longitude;
} lastCam = {0.0, 0.0, 0.0, 0.0};
double latitude = ego_.latitude;
double longitude = ego_.longitude;
double heading = ego_.heading;
double speed = velocityReport_.speed;
double deltaSpeed = std::fabs(speed - lastCam.speed);
double deltaHeading = std::fabs(heading - lastCam.heading);
if (deltaHeading > 180.0) deltaHeading = 360.0 - deltaHeading;
double distance = calc_haversine(lastCam.latitude, lastCam.longitude, latitude, longitude);
const double distance_threshold = 4.0;
const double speed_threshold = 0.5;
const double heading_threshold = 4.0;
if (distance > distance_threshold || deltaSpeed > speed_threshold || deltaHeading > heading_threshold) {
int interval = static_cast<int>((distance / speed) * 1000);
if (interval < 100) {
cam_interval_ = milliseconds(100);
} else if (interval > 1000) {
cam_interval_ = milliseconds(1000);
} else {
cam_interval_ = milliseconds(interval);
}
lastCam.speed = speed;
lastCam.heading = heading;
lastCam.latitude = latitude;
lastCam.longitude = longitude;
} else {
cam_interval_ = milliseconds(1000);
}
}
}
void CamApplication::schedule_timer() {
runtime_.schedule(cam_interval_, std::bind(&CamApplication::on_timer, this, std::placeholders::_1));
}
void CamApplication::on_timer(vanetza::Clock::time_point) {
calc_interval();
schedule_timer();
send();
}
@ -141,31 +83,9 @@ namespace v2x
return btp::ports::CAM;
}
void CamApplication::indicate(const Application::DataIndication &indication, Application::UpPacketPtr packet)
void CamApplication::indicate(const Application::DataIndication &, Application::UpPacketPtr)
{
asn1::PacketVisitor<asn1::Cam> visitor;
std::shared_ptr<const asn1::Cam> rec_cam_ptr = boost::apply_visitor(visitor, *packet);
if (!rec_cam_ptr) {
RCLCPP_INFO(node_->get_logger(), "[CamApplication::indicate] Received invalid CAM");
return;
}
asn1::Cam rec_cam = *rec_cam_ptr;
std::chrono::milliseconds now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
RCLCPP_INFO(node_->get_logger(), "[CamApplication::indicate] Received CAM from station with ID #%ld at %ld epoch time", rec_cam->header.stationID, now_ms.count());
vanetza::facilities::print_indented(std::cout, rec_cam, " ", 0);
cam_ts_CAM_t ts_cam;
std::memset(&ts_cam, 0, sizeof(ts_cam));
CamApplication::build_etsi_its_cam_ts_from_vanetza(rec_cam, ts_cam);
etsi_its_cam_ts_msgs::msg::CAM ros_msg;
std::memset(&ros_msg, 0, sizeof(ros_msg));
etsi_its_cam_ts_conversion::toRos_CAM(ts_cam, ros_msg);
node_->publishReceivedCam(ros_msg);
// TODO: implement
}
void CamApplication::updateMGRS(double *x, double *y) {
@ -173,7 +93,7 @@ namespace v2x
ego_.mgrs_y = *y;
}
void CamApplication::updateRP(const double *lat, const double *lon, const double *altitude) {
void CamApplication::updateRP(double *lat, double *lon, double *altitude) {
ego_.latitude = *lat;
ego_.longitude = *lon;
ego_.altitude = *altitude;
@ -182,16 +102,16 @@ namespace v2x
positionConfidenceEllipse_.y.insert(*lon);
}
void CamApplication::updateHeading(const double *yaw) {
void CamApplication::updateGenerationDeltaTime(int *gdt, long *gdt_timestamp) {
generationDeltaTime_ = *gdt;
gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
}
void CamApplication::updateHeading(double *yaw) {
ego_.heading = *yaw;
}
void CamApplication::setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &msg) {
if (msg == autoware_adapi_v1_msgs::msg::VehicleDimensions{}) {
RCLCPP_WARN(node_->get_logger(), "[CamApplication::getVehicleDimensions] Vehicle dimensions not available");
return;
}
vehicleDimensions_.wheel_radius = msg.wheel_radius;
vehicleDimensions_.wheel_width = msg.wheel_width;
vehicleDimensions_.wheel_base = msg.wheel_base;
@ -204,30 +124,40 @@ namespace v2x
}
void CamApplication::updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
if (updating_velocity_report_) {
return;
}
updating_velocity_report_ = true;
rclcpp::Time msg_stamp(msg->header.stamp.sec, msg->header.stamp.nanosec);
double dt = msg_stamp.seconds() - velocityReport_.stamp.seconds();
float dt = msg_stamp.seconds() - velocityReport_.stamp.seconds();
if (dt == 0) {
RCLCPP_WARN(node_->get_logger(), "[CamApplication::updateVelocityReport] deltaTime is 0");
return;
}
float longitudinal_acceleration = (msg->longitudinal_velocity - velocityReport_.longitudinal_velocity) / dt;
float speed = std::sqrt(std::pow(msg->longitudinal_velocity, 2) + std::pow(msg->lateral_velocity, 2));
velocityReport_.stamp = msg->header.stamp;
velocityReport_.heading_rate = msg->heading_rate;
velocityReport_.lateral_velocity = msg->lateral_velocity;
velocityReport_.longitudinal_velocity = msg->longitudinal_velocity;
velocityReport_.speed = speed;
velocityReport_.longitudinal_acceleration = longitudinal_acceleration;
updating_velocity_report_ = false;
}
void CamApplication::updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
rclcpp::Time msg_stamp(msg->stamp.sec, msg->stamp.nanosec);
vehicleStatus_.gear = msg->report;
}
void CamApplication::updateVehicleStatus(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
if (updating_vehicle_status_) {
return;
}
void CamApplication::updateSteeringReport(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
updating_vehicle_status_ = true;
vehicleStatus_.gear = msg->gear.status;
vehicleStatus_.steering_tire_angle = msg->steering_tire_angle;
updating_vehicle_status_ = false;
}
void CamApplication::send() {
@ -240,7 +170,7 @@ namespace v2x
sending_ = true;
std::chrono::milliseconds now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] cam_num: %d", cam_num_);
vanetza::asn1::Cam message;
@ -251,8 +181,8 @@ namespace v2x
CoopAwareness_t &cam = message->cam;
// Convert Unix timestamp to ETSI epoch (2004-01-01 00:00:00)
cam.generationDeltaTime = (now_ms.count() - 1072915200000) % 65536;
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] %ld", gdt_timestamp_);
cam.generationDeltaTime = generationDeltaTime_;
BasicContainer_t &basic_container = cam.camParameters.basicContainer;
basic_container.stationType = StationType_passengerCar;
@ -327,13 +257,13 @@ namespace v2x
uint8_t gearStatus = vehicleStatus_.gear;
float steering_tire_angle = vehicleStatus_.steering_tire_angle;
long speed = std::lround(velocityReport_.speed * 100);
long speed = std::lround(std::sqrt(std::pow(longitudinal_velocity, 2) + std::pow(lateral_velocity, 2)) * 100);
if (0 <= speed && speed <= 16382) bvc.speed.speedValue = speed;
else bvc.speed.speedValue = SpeedValue_unavailable;
if ((gearStatus >= 2 && gearStatus <= 19) || (gearStatus == 23 || gearStatus == 24))
if (gearStatus == 2 || gearStatus == 5)
bvc.driveDirection = DriveDirection_forward;
else if (gearStatus == 20 || gearStatus == 21)
else if (gearStatus == 3)
bvc.driveDirection = DriveDirection_backward;
else
bvc.driveDirection = DriveDirection_unavailable;
@ -370,8 +300,7 @@ namespace v2x
bvc.yawRate.yawRateConfidence = YawRateConfidence_unavailable;
// ------------------------------
RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM from station with ID %ld with generationDeltaTime %ld, latitude %f, longitude %f, altitude %f",
stationId_, cam.generationDeltaTime, ego_.latitude, ego_.longitude, ego_.altitude);
RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM from station with ID %ld", stationId_);
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
payload->layer(OsiLayer::Application) = std::move(message);
@ -382,64 +311,18 @@ namespace v2x
Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
cam_ts_CAM_t ts_cam;
std::memset(&ts_cam, 0, sizeof(ts_cam));
CamApplication::build_etsi_its_cam_ts_from_vanetza(message, ts_cam);
etsi_its_cam_ts_msgs::msg::CAM ros_msg;
std::memset(&ros_msg, 0, sizeof(ros_msg));
etsi_its_cam_ts_conversion::toRos_CAM(ts_cam, ros_msg);
node_->publishReceivedCam(ros_msg);
if (!confirm.accepted()) {
throw std::runtime_error("[CamApplication::send] CAM application data request failed");
}
RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Successfully sent");
sending_ = false;
std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds> (
std::chrono::system_clock::now().time_since_epoch()
);
node_->latency_log_file << "T_depart," << cam_num_ << "," << ms.count() << std::endl;
++cam_num_;
}
void CamApplication::build_etsi_its_cam_ts_from_vanetza(vanetza::asn1::Cam &in, cam_ts_CAM_t &out) {
cam_ts_ItsPduHeader_t &header = out.header;
header.protocolVersion = in->header.protocolVersion;
header.messageId = in->header.messageID;
header.stationId = in->header.stationID;
cam_ts_CamPayload_t &coopAwareness = out.cam;
coopAwareness.generationDeltaTime = in->cam.generationDeltaTime;
cam_ts_BasicContainer_t &basic_container = coopAwareness.camParameters.basicContainer;
BasicContainer_t &rec_basic_container = in->cam.camParameters.basicContainer;
basic_container.stationType = rec_basic_container.stationType;
basic_container.referencePosition.latitude = rec_basic_container.referencePosition.latitude;
basic_container.referencePosition.longitude = rec_basic_container.referencePosition.longitude;
basic_container.referencePosition.altitude.altitudeValue = rec_basic_container.referencePosition.altitude.altitudeValue;
basic_container.referencePosition.altitude.altitudeConfidence = rec_basic_container.referencePosition.altitude.altitudeConfidence;
basic_container.referencePosition.positionConfidenceEllipse.semiMajorAxisLength = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence;
basic_container.referencePosition.positionConfidenceEllipse.semiMinorAxisLength = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence;
basic_container.referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation = rec_basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation;
cam_ts_BasicVehicleContainerHighFrequency_t &bvc = coopAwareness.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
coopAwareness.camParameters.highFrequencyContainer.present = cam_ts_HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
BasicVehicleContainerHighFrequency_t &rec_bvc = in->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
bvc.heading.headingValue = rec_bvc.heading.headingValue;
bvc.heading.headingConfidence = rec_bvc.heading.headingConfidence;
bvc.speed.speedValue = rec_bvc.speed.speedValue;
bvc.speed.speedConfidence = rec_bvc.speed.speedConfidence;
bvc.driveDirection = rec_bvc.driveDirection;
bvc.vehicleLength.vehicleLengthValue = rec_bvc.vehicleLength.vehicleLengthValue;
bvc.vehicleLength.vehicleLengthConfidenceIndication = rec_bvc.vehicleLength.vehicleLengthConfidenceIndication;
bvc.vehicleWidth = rec_bvc.vehicleWidth;
bvc.longitudinalAcceleration.value = rec_bvc.longitudinalAcceleration.longitudinalAccelerationValue;
bvc.longitudinalAcceleration.confidence = rec_bvc.longitudinalAcceleration.longitudinalAccelerationConfidence;
bvc.curvature.curvatureValue = rec_bvc.curvature.curvatureValue;
bvc.curvature.curvatureConfidence = rec_bvc.curvature.curvatureConfidence;
bvc.curvatureCalculationMode = rec_bvc.curvatureCalculationMode;
bvc.yawRate.yawRateValue = rec_bvc.yawRate.yawRateValue;
bvc.yawRate.yawRateConfidence = rec_bvc.yawRate.yawRateConfidence;
}
}

34
src/cpm_application.cpp
View File

@ -32,7 +32,7 @@ using namespace vanetza;
using namespace std::chrono;
namespace v2x {
CpmApplication::CpmApplication(V2XNode *node, Runtime &rt, bool is_sender) :
CpmApplication::CpmApplication(V2XNode *node, Runtime &rt, bool is_sender) :
node_(node),
runtime_(rt),
ego_(),
@ -101,8 +101,7 @@ namespace v2x {
// Calculate GDT and get GDT from CPM and calculate the "Age of CPM"
TimestampIts_t gt_cpm;
asn_long2INTEGER(&gt_cpm, (long) message->cpm.generationDeltaTime);
TimestampIts_t gt_cpm = message->cpm.generationTime;
// const auto time_now = duration_cast<milliseconds> (runtime_.now().time_since_epoch());
// uint16_t gdt = time_now.count();
// int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536;
@ -157,7 +156,7 @@ namespace v2x {
x1 = x1 / 100.0;
y1 = y1 / 100.0;
object.position_x = x_mgrs + (cos(orientation) * x1 - sin(orientation) * y1);
object.position_y = y_mgrs + (sin(orientation) * x1 + cos(orientation) * y1);
object.position_y = y_mgrs + (sin(orientation) * x1 + cos(orientation) * y1);
object.shape_x = poc->list.array[i]->planarObjectDimension2->value;
object.shape_y = poc->list.array[i]->planarObjectDimension1->value;
object.shape_z = poc->list.array[i]->verticalObjectDimension->value;
@ -237,7 +236,7 @@ namespace v2x {
});
if (found_object == objectsList.end()) {
} else {
found_object->to_send = true;
}
@ -318,7 +317,7 @@ namespace v2x {
++cpm_object_id_;
} else {
// Object was already in internal memory
// Object belongs to class person or animal
@ -328,7 +327,7 @@ namespace v2x {
found_object->to_send = true;
found_object->to_send_trigger = 5;
}
// Object has not been included in a CPM in the past 500 ms.
if (runtime_.now().time_since_epoch().count() - found_object->timestamp.time_since_epoch().count() > 500000) {
// Include all objects of class person or animal in the current CPM
@ -350,7 +349,7 @@ namespace v2x {
found_object->to_send = true;
found_object->to_send_trigger = 1;
} else {
}
// Absolute speed changed by more than 0.5 m/s
@ -360,7 +359,7 @@ namespace v2x {
if (speed > 0.5) {
found_object->to_send = true;
found_object->to_send_trigger = 2;
}
}
// Orientation of speed vector changed by more than 4 degrees
double twist_angular_x_diff = (obj.kinematics.initial_twist_with_covariance.twist.angular.x - found_object->twist_angular_x) * 180 / M_PI;
@ -435,21 +434,20 @@ namespace v2x {
} else {
RCLCPP_INFO(node_->get_logger(), "[objectsList] %d,,,,", cpm_num);
}
// RCLCPP_INFO(node_->get_logger(), "------------------------");
}
void CpmApplication::send() {
if (is_sender_) {
sending_ = true;
printObjectsList(cpm_num_);
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM...");
std::chrono::milliseconds now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
vanetza::asn1::Cpm message;
// ITS PDU Header
@ -462,7 +460,7 @@ namespace v2x {
// Set GenerationTime
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] %ld", gdt_timestamp_);
cpm.generationDeltaTime = (now_ms.count() - 1072915200000) % 65536;
asn_long2INTEGER(&cpm.generationTime, (long) gdt_timestamp_);
CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer;
management.stationType = StationType_passengerCar;
@ -580,7 +578,7 @@ namespace v2x {
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM with %d objects", perceivedObjectsCount);
insertCpmToCpmTable(message, (char*) "cpm_sent");
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
payload->layer(OsiLayer::Application) = std::move(message);
@ -620,7 +618,7 @@ namespace v2x {
}
char* sql_command;
sql_command = (char*) "create table if not exists cpm_sent(id INTEGER PRIMARY KEY, timestamp BIGINT, perceivedObjectCount INTEGER);";
ret = sqlite3_exec(db, sql_command, NULL, NULL, &err);
@ -663,7 +661,7 @@ namespace v2x {
}
std::stringstream sql_command;
sql_command << "insert into " << table_name << " (timestamp, perceivedObjectCount) values (" << timestamp << ", " << perceivedObjectCount << ");";
ret = sqlite3_exec(db, sql_command.str().c_str(), NULL, NULL, &err);
@ -679,4 +677,4 @@ namespace v2x {
sqlite3_close(db);
// RCLCPP_INFO(node_->get_logger(), "CpmApplication::insertCpmToCpmTable Finished");
}
}
}

148
src/cube_evk_link.cpp
View File

@ -1,148 +0,0 @@
#include "autoware_v2x/cube_evk_link.hpp"
#include "cube_evk_radio.pb.h"
#include <iostream>
#include <vanetza/net/osi_layer.hpp>
#include <vanetza/net/packet_variant.hpp>
#include <vanetza/net/cohesive_packet.hpp>
#include <vanetza/access/data_request.hpp>
#include <vanetza/common/byte_view.hpp>
#include <vanetza/access/ethertype.hpp>
#include <boost/asio/placeholders.hpp>
#include <boost/bind/bind.hpp>
CubeEvkLink::CubeEvkLink(boost::asio::io_service& io, boost::asio::ip::address_v4 radio_ip)
: io_(io), tx_socket_(io), rx_socket_(io)
{
const boost::asio::ip::udp::endpoint radio_endpoint_tx(radio_ip, cube_evk_radio_port_tx);
tx_socket_.connect(radio_endpoint_tx);
boost::asio::ip::udp::endpoint radio_endpoint_rx(boost::asio::ip::udp::v4(), cube_evk_radio_port_rx);
rx_socket_.open(radio_endpoint_rx.protocol());
rx_socket_.bind(radio_endpoint_rx);
rx_socket_.async_receive_from(
boost::asio::buffer(received_data_), host_endpoint_,
boost::bind(&CubeEvkLink::handle_packet_received, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
}
void CubeEvkLink::handle_packet_received(const boost::system::error_code& ec, size_t bytes)
{
if (!ec)
{
vanetza::ByteBuffer buf(received_data_.begin(), received_data_.begin() + bytes);
GossipMessage gossipMessage;
gossipMessage.ParseFromArray(buf.data(), buf.size());
switch (gossipMessage.kind_case())
{
case GossipMessage::KindCase::kCbr:
{
// got CBR; use this for your DCC
// const ChannelBusyRatio& cbr = gossipMessage.cbr();
// vanetza::dcc::ChannelLoad(cbr.busy(), cbr.total())
break;
}
case GossipMessage::KindCase::kLinklayerRx:
{
pass_message_to_router(std::unique_ptr<LinkLayerReception>{gossipMessage.release_linklayer_rx()});
break;
}
default:
{
std::cerr << "Received GossipMessage of unknown kind " << gossipMessage.kind_case() << std::endl;
}
}
rx_socket_.async_receive_from(
boost::asio::buffer(received_data_), host_endpoint_,
boost::bind(&CubeEvkLink::handle_packet_received, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
}
else
{
std::cerr << "CubeEvkLink::handle_packet_received went wrong: " << ec << std::endl;
}
}
void CubeEvkLink::request(const vanetza::access::DataRequest& request, std::unique_ptr<vanetza::ChunkPacket> packet)
{
CommandRequest command;
command.set_allocated_linklayer_tx(create_link_layer_tx(request, std::move(packet)).release());
std::string serializedTransmission;
command.SerializeToString(&serializedTransmission);
tx_socket_.send(boost::asio::buffer(serializedTransmission));
}
void CubeEvkLink::indicate(IndicationCallback callback)
{
indicate_to_router_ = callback;
}
void CubeEvkLink::pass_message_to_router(std::unique_ptr<LinkLayerReception> packet)
{
if (packet->source().size() != vanetza::MacAddress::length_bytes)
{
std::cerr << "received packet's source MAC address is invalid" << std::endl;
}
else if (packet->destination().size() != vanetza::MacAddress::length_bytes)
{
std::cerr << "received packet's destination MAC address is invalid" << std::endl;
}
else
{
vanetza::EthernetHeader ethernet_header;
std::copy_n(packet->source().begin(), vanetza::MacAddress::length_bytes, ethernet_header.source.octets.begin());
std::copy_n(packet->destination().begin(), vanetza::MacAddress::length_bytes, ethernet_header.destination.octets.begin());
ethernet_header.type = vanetza::access::ethertype::GeoNetworking;
vanetza::ByteBuffer buffer(packet->payload().begin(), packet->payload().end());
vanetza::CohesivePacket packet(std::move(buffer), vanetza::OsiLayer::Network);
indicate_to_router_(std::move(packet), ethernet_header);
}
}
std::unique_ptr<LinkLayerTransmission> CubeEvkLink::create_link_layer_tx(const vanetza::access::DataRequest& req, std::unique_ptr<vanetza::ChunkPacket> packet)
{
using namespace vanetza;
std::unique_ptr<LinkLayerTransmission> transmission{new LinkLayerTransmission()};
transmission->set_source(req.source_addr.octets.data(), req.source_addr.octets.size());
transmission->set_destination(req.destination_addr.octets.data(), req.destination_addr.octets.size());
LinkLayerPriority prio = LinkLayerPriority::BEST_EFFORT;
switch (req.access_category)
{
case access::AccessCategory::VO:
prio = LinkLayerPriority::VOICE;
break;
case access::AccessCategory::VI:
prio = LinkLayerPriority::VIDEO;
break;
case access::AccessCategory::BE:
prio = LinkLayerPriority::BEST_EFFORT;
break;
case access::AccessCategory::BK:
prio = LinkLayerPriority::BACKGROUND;
break;
default:
std::cerr << "Unknown access category requested, falling back to best effort!" << std::endl;
break;
}
transmission->set_priority(prio);
std::string* payload = transmission->mutable_payload();
for (auto& layer : osi_layer_range<OsiLayer::Network, OsiLayer::Application>())
{
auto byte_view = create_byte_view(packet->layer(layer));
payload->append(byte_view.begin(), byte_view.end());
}
return transmission;
}

78
src/cube_evk_radio.proto
View File

@ -1,78 +0,0 @@
syntax = "proto2";
message CommandRequest {
oneof kind {
LifecycleAction lifecycle = 1;
LinkLayerTransmission linklayer_tx = 2;
RadioConfiguration radio_cfg = 3;
}
}
message CommandResponse {
enum Status {
SUCCESS = 0;
FAILURE = 1;
UNKNOWN = 2;
NOT_IMPLEMENTED = 3;
}
required Status status = 1;
optional string message = 2;
optional CommandResponseData data = 3;
}
message CommandResponseData {
oneof kind {
RadioConfiguration radio_cfg = 1;
}
}
enum LifecycleAction {
SOFT_RESET = 0;
HARD_RESET = 1;
}
enum LinkLayerPriority {
BACKGROUND = 0;
BEST_EFFORT = 1;
VIDEO = 2;
VOICE = 3;
}
message RadioConfiguration {
optional bytes address = 1;
optional uint32 channel_frequency_mhz = 2;
optional bool filter_unicast_destination = 3;
optional sint32 default_tx_power_cbm = 4;
optional uint32 default_tx_datarate_500kbps = 5;
}
message LinkLayerTransmission {
optional bytes source = 1;
required bytes destination = 2;
required LinkLayerPriority priority = 3;
optional uint32 channel = 4;
optional uint32 datarate_500kbps = 5;
optional sint32 power_cbm = 6; // centi Bel mW = 10 * dBm
required bytes payload = 10;
}
message LinkLayerReception {
required bytes source = 1;
required bytes destination = 2;
optional uint32 channel = 4;
optional sint32 power_cbm = 6;
required bytes payload = 10;
}
message ChannelBusyRatio {
required uint32 busy = 1;
required uint32 total = 2;
}
message GossipMessage {
oneof kind {
ChannelBusyRatio cbr = 1;
LinkLayerReception linklayer_rx = 2;
}
}

5
src/link_layer.cpp
View File

@ -1,11 +1,10 @@
#include "autoware_v2x/link_layer.hpp"
#include "autoware_v2x/cube_evk_link.hpp"
#include <boost/asio/generic/raw_protocol.hpp>
#include <vanetza/access/ethertype.hpp>
#include "autoware_v2x/raw_socket_link.hpp"
std::unique_ptr<LinkLayer> create_link_layer(
boost::asio::io_service &io_service, const EthernetDevice &device, const std::string &name, const std::string &cube_ip)
boost::asio::io_service & io_service, const EthernetDevice & device, const std::string & name)
{
std::unique_ptr<LinkLayer> link_layer;
@ -15,8 +14,6 @@ std::unique_ptr<LinkLayer> create_link_layer(
boost::asio::generic::raw_protocol::socket raw_socket(io_service, raw_protocol);
raw_socket.bind(device.endpoint(AF_PACKET));
link_layer.reset(new RawSocketLink{std::move(raw_socket)});
} else if (name == "cube-evk") {
link_layer.reset(new CubeEvkLink{ io_service, boost::asio::ip::address_v4::from_string(cube_ip) });
}
// Removed Cohda and UDP Support

124
src/security.cpp
View File

@ -1,27 +1,13 @@
/**
* From Vanetza (https://github.com/riebl/Vanetza)
* Using Security V3
*
* https://github.com/riebl/vanetza/tree/master/vanetza/security
* https://github.com/riebl/vanetza/tree/master/tools/socktap/security.cpp
*/
#include "autoware_v2x/security.hpp"
#include <memory>
#include <vanetza/security/certificate_cache.hpp>
#include <vanetza/security/default_certificate_validator.hpp>
#include <vanetza/security/delegating_security_entity.hpp>
#include <vanetza/security/straight_verify_service.hpp>
#include "vanetza/security/v2/certificate_provider.hpp"
#include <vanetza/security/v2/certificate_cache.hpp>
#include <vanetza/security/v2/default_certificate_validator.hpp>
#include <vanetza/security/v2/sign_service.hpp>
#include <vanetza/security/v3/certificate_cache.hpp>
#include <vanetza/security/v3/naive_certificate_provider.hpp>
#include <vanetza/security/v3/persistence.hpp>
#include <vanetza/security/v3/sign_header_policy.hpp>
#include <vanetza/security/v3/sign_service.hpp>
#include <vanetza/security/v3/static_certificate_provider.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;
@ -30,18 +16,18 @@ 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()
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) {
if (!entity)
{
throw std::runtime_error("security entity is not ready");
}
return entity->encapsulate_packet(std::move(request));
@ -49,7 +35,8 @@ public:
security::DecapConfirm decapsulate_packet(security::DecapRequest &&request) override
{
if (!entity) {
if (!entity)
{
throw std::runtime_error("security entity is not ready");
}
return entity->decapsulate_packet(std::move(request));
@ -57,78 +44,31 @@ public:
void build_entity()
{
if (!cert_provider) {
if (!cert_provider)
{
throw std::runtime_error("certificate provider is missing");
}
std::unique_ptr<security::SignService> sign_service{new security::v3::StraightSignService(*cert_provider, *backend, sign_header_policy)};
std::unique_ptr<security::StraightVerifyService> verify_service{new security::StraightVerifyService(runtime, *backend, positioning)};
verify_service->use_certificate_cache(&cert_cache);
entity = std::make_unique<security::DelegatingSecurityEntity>(std::move(sign_service), std::move(verify_service));
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;
const Runtime &runtime;
PositionProvider &positioning;
std::unique_ptr<security::Backend> backend;
std::unique_ptr<security::SecurityEntity> entity;
std::unique_ptr<security::v3::CertificateProvider> cert_provider;
security::v3::DefaultSignHeaderPolicy sign_header_policy;
security::v3::CertificateCache cert_cache;
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::v3::CertificateProvider> load_certificates(
const std::string &cert_path, const std::string &cert_key_path,
const std::vector<std::string> &cert_chain_path, security::v3::CertificateCache &cert_cache)
{
auto authorization_ticket = security::v3::load_certificate_from_file(cert_path);
auto authorization_ticket_key = security::v3::load_private_key_from_file(cert_key_path);
std::list<security::v3::Certificate> chain;
for (auto & chain_path : cert_chain_path) {
auto chain_certificate = security::v3::load_certificate_from_file(chain_path);
chain.push_back(chain_certificate);
cert_cache.store(chain_certificate);
}
return std::make_unique<security::v3::StaticCertificateProvider>(authorization_ticket, authorization_ticket_key.private_key, chain);
}
std::unique_ptr<security::SecurityEntity> create_security_entity(const po::variables_map &options, const Runtime &runtime, PositionProvider &positioning)
std::unique_ptr<security::SecurityEntity>
create_security_entity(const Runtime &runtime, PositionProvider &positioning)
{
std::unique_ptr<security::SecurityEntity> security;
const std::string name = options["security"].as<std::string>();
if (name.empty() || name == "none") {
// no operation
} else if (name == "certs") {
if (options.count("certificate") ^ options.count("certificate-key")) {
throw std::runtime_error("Either certificate and certificate-key must be present or none.");
}
if (options.count("certificate") && options.count("certificate-key")) {
const auto &cert_path = options["certificate"].as<std::string>();
const auto &cert_key_path = options["certificate-key"].as<std::string>();
std::vector<std::string> chain_paths;
if (options.count("certificate-chain")) {
chain_paths = options["certificate-chain"].as<std::vector<std::string>>();
}
auto context = std::make_unique<SecurityContext>(runtime, positioning);
context->cert_provider = load_certificates(cert_path, cert_key_path, chain_paths, context->cert_cache);
context->build_entity();
security = std::move(context);
} else {
auto context = std::make_unique<SecurityContext>(runtime, positioning);
context->cert_provider = std::make_unique<security::v3::NaiveCertificateProvider>(runtime);
context->build_entity();
security = std::move(context);
}
if (!security) {
throw std::runtime_error("internal failure setting up security entity");
}
} else {
throw std::runtime_error("Unknown security entity requested");
}
return security;
}

67
src/v2x_app.cpp
View File

@ -24,7 +24,6 @@
#include <boost/date_time/posix_time/posix_time.hpp>
namespace gn = vanetza::geonet;
namespace po = boost::program_options;
using namespace vanetza;
using namespace std::chrono;
@ -51,22 +50,31 @@ namespace v2x
}
}
void V2XApp::setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &msg) {
if (cam_started_)
cam->setVehicleDimensions(msg);
else
this->setVehicleDimensions(msg);
}
void V2XApp::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
if (cam_started_)
if (!velocity_report_received_) {
RCLCPP_WARN(node_->get_logger(), "[V2XApp::velocityReportCallback] VelocityReport not received yet");
}
if (velocity_report_received_ && cam_started_) {
cam->updateVelocityReport(msg);
}
}
void V2XApp::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
if (cam_started_)
cam->updateGearReport(msg);
void V2XApp::vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
if (!vehicle_status_received_) {
RCLCPP_WARN(node_->get_logger(), "[V2XApp::vehicleStatusCallback] VehicleStatus not received yet");
}
if (vehicle_status_received_ && cam_started_) {
cam->updateVehicleStatus(msg);
}
}
void V2XApp::steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
if (cam_started_)
cam->updateSteeringReport(msg);
}
void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
tf_received_ = true;
@ -120,6 +128,7 @@ namespace v2x
cam->updateMGRS(&x, &y);
cam->updateRP(&lat, &lon, &z);
cam->updateHeading(&yaw);
cam->updateGenerationDeltaTime(&gdt, &timestamp_msec);
}
}
@ -129,10 +138,6 @@ namespace v2x
boost::asio::io_service io_service;
TimeTrigger trigger(io_service);
std::string link_layer_name;
node_->get_parameter("link_layer", link_layer_name);
RCLCPP_INFO(node_->get_logger(), "Link Layer: %s", link_layer_name.c_str());
std::string network_interface;
node_->get_parameter("network_interface", network_interface);
RCLCPP_INFO(node_->get_logger(), "Network Interface: %s", network_interface.c_str());
@ -152,38 +157,10 @@ namespace v2x
mib.itsGnSecurity = false;
mib.itsGnProtocolVersion = 1;
std::string cube_ip;
node_->get_parameter("cube_ip", cube_ip);
RCLCPP_INFO(node_->get_logger(), "CubeEVK IP: %s", cube_ip.c_str());
// Create raw socket on device and LinkLayer object
auto link_layer = create_link_layer(io_service, device, link_layer_name, cube_ip);
auto link_layer = create_link_layer(io_service, device, "ethernet");
auto positioning = create_position_provider(io_service, trigger.runtime());
po::variables_map security_options;
std::string entity;
std::string certificate;
std::string certificate_key;
std::vector<std::string> certificate_chain;
// Grab the values from the node parameters
node_->get_parameter("security", entity);
security_options.insert(std::make_pair("security", po::variable_value(entity, false)));
if (node_->has_parameter("certificate")) {
node_->get_parameter("certificate", certificate);
security_options.insert(std::make_pair("certificate", po::variable_value(certificate, false)));
}
if (node_->has_parameter("certificate-key")) {
node_->get_parameter("certificate-key", certificate_key);
security_options.insert(std::make_pair("certificate-key", po::variable_value(certificate_key, false)));
}
if (node_->has_parameter("certificate-chain")) {
node_->get_parameter("certificate-chain", certificate_chain);
security_options.insert(std::make_pair("certificate-chain", po::variable_value(certificate_chain, false)));
}
auto security = create_security_entity(security_options, trigger.runtime(), *positioning);
RCLCPP_INFO(node_->get_logger(), "Security layer: %s", entity == "certs" ? "Certificates" : "None");
auto security = create_security_entity(trigger.runtime(), *positioning);
RouterContext context(mib, trigger, *positioning, security.get());
context.set_link_layer(link_layer.get());

78
src/v2x_node.cpp
View File

@ -36,35 +36,26 @@ namespace v2x
V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) {
using std::placeholders::_1;
int timeout = 10;
get_vehicle_dimensions_ = this->create_client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>("/api/vehicle/dimensions");
if (get_vehicle_dimensions_->wait_for_service(std::chrono::seconds(timeout))) {
RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] Service /api/vehicle/dimensions is now available.");
vehicle_dimensions_available_ = true;
} else {
RCLCPP_ERROR(get_logger(), "[V2XNode::V2XNode] Service /api/vehicle/dimensions is not available after waiting (timeout=%ds).", timeout);
vehicle_dimensions_available_ = false;
}
objects_sub_ = this->create_subscription<autoware_auto_perception_msgs::msg::PredictedObjects>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
tf_sub_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));
// Topic subscriptions for CAMApplication
velocity_report_sub_ = this->create_subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>("/vehicle/status/velocity_status", 10, std::bind(&V2XNode::velocityReportCallback, this, _1));
gear_report_sub_ = this->create_subscription<autoware_auto_vehicle_msgs::msg::GearReport>("/vehicle/status/gear_status", 10, std::bind(&V2XNode::gearReportCallback, this, _1));
steering_report_sub_ = this->create_subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>("/vehicle/status/steering_status", 10, std::bind(&V2XNode::steeringReportCallback, this, _1));
vehicle_status_sub_ = this->create_subscription<autoware_adapi_v1_msgs::msg::VehicleStatus>("/api/vehicle/status", 10, std::bind(&V2XNode::vehicleStatucCallback, this, _1));
get_vehicle_dimensions_ = this->create_client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>("/api/vehicle/dimensions");
if (get_vehicle_dimensions_->wait_for_service(std::chrono::seconds(60))) {
RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is now available.");
this->getVehicleDimensions();
} else {
RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is not available after waiting (timeout=60s).");
}
cpm_objects_pub_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/objects", rclcpp::QoS{10});
// cpm_sender_pub_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/sender", rclcpp::QoS{10});
cam_rec_pub_ = create_publisher<etsi_its_cam_ts_msgs::msg::CAM>("/v2x/cam_ts/received", rclcpp::QoS{10});
// Declare Parameters
this->declare_parameter<std::string>("link_layer", "ethernet");
this->declare_parameter<std::string>("network_interface", "v2x_testing");
this->declare_parameter<std::string>("cube_ip", "127.0.0.1");
this->declare_parameter<std::string>("network_interface", "vmnet1");
this->declare_parameter<bool>("is_sender", true);
this->declare_parameter<std::string>("security", "none");
// Launch V2XApp in a new thread
app = new V2XApp(this);
@ -175,11 +166,6 @@ namespace v2x
cpm_objects_pub_->publish(output_dynamic_object_msg);
}
void V2XNode::publishReceivedCam(etsi_its_cam_ts_msgs::msg::CAM &msg) {
RCLCPP_INFO(get_logger(), "Publishing CAM to ROS network");
cam_rec_pub_->publish(msg);
}
void V2XNode::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) {
rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg.
@ -199,37 +185,33 @@ namespace v2x
app->velocityReportCallback(msg);
}
void V2XNode::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
app->gearReportCallback(msg);
}
void V2XNode::steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
app->steeringReportCallback(msg);
void V2XNode::vehicleStatucCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
app->vehicleStatusCallback(msg);
}
void V2XNode::getVehicleDimensions() {
if (!vehicle_dimensions_available_) {
RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is not available.");
return;
}
RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Sending service request to /api/vehicle/dimensions");
auto request = std::make_shared<autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Request>();
auto future_result = get_vehicle_dimensions_->async_send_request(request, [this](rclcpp::Client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>::SharedFuture future) {
try {
auto response = future.get();
RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received response from /api/vehicle/dimensions");
try {
auto dimensions = response->dimensions;
RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received vehicle dimensions: height=%f, wheel_base=%f, wheel_tread=%f", dimensions.height, dimensions.wheel_base, dimensions.wheel_tread);
app->cam->setVehicleDimensions(dimensions);
} catch (const std::exception &e) {
RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service response of /api/vehicle/dimensions failed: %s", e.what());
}
}
catch (const std::exception &e) {
RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service call of /api/vehicle/dimensions failed: %s", e.what());
}
});
try
{
auto response = future.get();
RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received response from /api/vehicle/dimensions");
try {
auto dimensions = response->dimensions;
if (dimensions.height == 0 || dimensions.wheel_base == 0 || dimensions.wheel_tread == 0)
this->getVehicleDimensions();
else
app->setVehicleDimensions(dimensions);
} catch (const std::exception &e) {
RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service response of /api/vehicle/dimensions failed: %s", e.what());
}
}
catch (const std::exception &e)
{
RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service call of /api/vehicle/dimensions failed: %s", e.what());
}
});
}
}