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

12
CMakeLists.txt
View File

@ -22,17 +22,12 @@ set(VANETZA_INSTALL ON)
option(BUILD_SHARED_LIBS "Build shared libraries" ON) option(BUILD_SHARED_LIBS "Build shared libraries" ON)
find_package(Vanetza REQUIRED) find_package(Vanetza REQUIRED)
find_package(GeographicLib 1.37 REQUIRED) find_package(GeographicLib 1.37 REQUIRED)
find_package(Boost COMPONENTS thread program_options REQUIRED) find_package(Boost COMPONENTS thread REQUIRED)
find_package(etsi_its_cam_ts_coding REQUIRED)
find_package(etsi_its_cam_ts_conversion REQUIRED)
find_package(etsi_its_msgs_utils REQUIRED)
ament_auto_find_build_dependencies() ament_auto_find_build_dependencies()
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
find_package(Protobuf REQUIRED)
include_directories( include_directories(
include include
${CMAKE_CURRENT_BINARY_DIR}/src
) )
ament_auto_add_library(autoware_v2x SHARED ament_auto_add_library(autoware_v2x SHARED
@ -41,7 +36,6 @@ ament_auto_add_library(autoware_v2x SHARED
src/application.cpp src/application.cpp
src/cpm_application.cpp src/cpm_application.cpp
src/cam_application.cpp src/cam_application.cpp
src/cube_evk_link.cpp
src/ethernet_device.cpp src/ethernet_device.cpp
src/link_layer.cpp src/link_layer.cpp
src/raw_socket_link.cpp src/raw_socket_link.cpp
@ -52,9 +46,7 @@ ament_auto_add_library(autoware_v2x SHARED
src/security.cpp 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 sqlite3)
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)
rclcpp_components_register_node(autoware_v2x rclcpp_components_register_node(autoware_v2x
PLUGIN "v2x::V2XNode" PLUGIN "v2x::V2XNode"

9
config/autoware_v2x.param.yaml
View File

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

10
docs/design/index.md
View File

@ -18,7 +18,7 @@ The V2XNode launches a ROS2 Node for AutowareV2X. Its main purpose is to act as
### Input ### Input
| Name | Type | Description | | Name | Type | Description |
|------------------------------------------|--------------------------------------------------------|---------------------------------------------| | -------------------- | ------------------------------- | ---------------- |
| `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Perceived Objects by Autoware | | `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Perceived Objects by Autoware |
| `/tf` | `tf2_msgs::msg::TFMessage` | Pose of Ego Vehicle | | `/tf` | `tf2_msgs::msg::TFMessage` | Pose of Ego Vehicle |
| `/vehicle/status/velocity_report` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | Velocity of Ego Vehicle | | `/vehicle/status/velocity_report` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | Velocity of Ego Vehicle |
@ -28,14 +28,13 @@ The V2XNode launches a ROS2 Node for AutowareV2X. Its main purpose is to act as
### Output ### Output
| Name | Type | Description | | Name | Type | Description |
|------------------------|--------------------------------------------------------|--------------------------| | -------------------- | ------------------------------- | ---------------- |
| `/v2x/cpm/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Objects received by CPMs | | `/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 |
### Functions ### Functions
| Name | Description | | Name | Description |
|---------------------------------------------------------------------------------------------------|------------------------------------------------------------| | -------------------- | ---------------- |
| `objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg)` | Call `V2XApp::objectsCallback` | | `objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg)` | Call `V2XApp::objectsCallback` |
| `tfCallback` | Call `V2XApp::tfCallback` | | `tfCallback` | Call `V2XApp::tfCallback` |
| `velocityReportCallback` | Call `V2XApp::velocityReportCallback` | | `velocityReportCallback` | Call `V2XApp::velocityReportCallback` |
@ -43,11 +42,8 @@ The V2XNode launches a ROS2 Node for AutowareV2X. Its main purpose is to act as
| `getVehicleDimensions` | Sends a request to the service used for vehicle dimensions | | `getVehicleDimensions` | Sends a request to the service used for vehicle dimensions |
| `publishObjects(std::vector<CpmApplication::Object> *objectsStack, int cpm_num)` | | | `publishObjects(std::vector<CpmApplication::Object> *objectsStack, int cpm_num)` | |
| `publishCpmSenderObject` | Not used now | | `publishCpmSenderObject` | Not used now |
| `publishReceivedCam(etsi_its_cam_ts_msgs::msg::CAM &msg)` | Publishes a received CAM into the ROS environment |
## V2XApp ## V2XApp
## CPM Facility ## CPM Facility
## CAM Facility

69
include/autoware_v2x/cam_application.hpp
View File

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

3
include/autoware_v2x/security.hpp
View File

@ -9,6 +9,7 @@
#include <memory> #include <memory>
std::unique_ptr<vanetza::security::SecurityEntity> 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 */ #endif /* SECURITY_HPP_FV13ZIYA */

13
include/autoware_v2x/v2x_app.hpp
View File

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

19
include/autoware_v2x/v2x_node.hpp
View File

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

5
launch/v2x.launch.xml
View File

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

1
package.xml
View File

@ -19,7 +19,6 @@
<depend>std_msgs</depend> <depend>std_msgs</depend>
<depend>rclcpp_components</depend> <depend>rclcpp_components</depend>
<depend>Vanetza</depend> <depend>Vanetza</depend>
<depend>etsi_its_messages</depend>
<test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_common</test_depend>

213
src/cam_application.cpp
View File

@ -11,7 +11,6 @@
#include <vanetza/btp/ports.hpp> #include <vanetza/btp/ports.hpp>
#include <vanetza/asn1/cam.hpp> #include <vanetza/asn1/cam.hpp>
#include <vanetza/asn1/packet_visitor.hpp> #include <vanetza/asn1/packet_visitor.hpp>
#include <vanetza/facilities/cam_functions.hpp>
#include <chrono> #include <chrono>
#include <functional> #include <functional>
#include <iostream> #include <iostream>
@ -22,14 +21,14 @@
#include <GeographicLib/MGRS.hpp> #include <GeographicLib/MGRS.hpp>
#include <string> #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/cmath.hpp>
#include <boost/units/systems/si/prefixes.hpp> #include <boost/units/systems/si/prefixes.hpp>
#include <sqlite3.h> #include <sqlite3.h>
#define _USE_MATH_DEFINES
#include <math.h>
using namespace vanetza; using namespace vanetza;
using namespace std::chrono; using namespace std::chrono;
@ -38,18 +37,24 @@ namespace v2x
CamApplication::CamApplication(V2XNode * node, Runtime & rt, bool is_sender) CamApplication::CamApplication(V2XNode * node, Runtime & rt, bool is_sender)
: node_(node), : node_(node),
runtime_(rt), runtime_(rt),
cam_interval_(milliseconds(1000)),
vehicleDimensions_(), vehicleDimensions_(),
ego_(), ego_(),
positionConfidenceEllipse_(), positionConfidenceEllipse_(),
velocityReport_(), velocityReport_(),
vehicleStatus_(), vehicleStatus_(),
generationDeltaTime_(0),
updating_velocity_report_(false),
updating_vehicle_status_(false),
sending_(false), sending_(false),
is_sender_(is_sender), 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_); RCLCPP_INFO(node_->get_logger(), "CamApplication started. is_sender: %d", is_sender_);
set_interval(cam_interval_); set_interval(milliseconds(100));
//createTables(); //createTables();
// Generate ID for this station // Generate ID for this station
@ -57,8 +62,6 @@ namespace v2x
std::mt19937 gen(rd()); std::mt19937 gen(rd());
std::uniform_int_distribution<unsigned long> dis(0, 4294967295); std::uniform_int_distribution<unsigned long> dis(0, 4294967295);
stationId_ = dis(gen); stationId_ = dis(gen);
node_->getVehicleDimensions();
} }
void CamApplication::set_interval(Clock::duration interval) { void CamApplication::set_interval(Clock::duration interval) {
@ -67,72 +70,11 @@ namespace v2x
schedule_timer(); 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() { void CamApplication::schedule_timer() {
runtime_.schedule(cam_interval_, std::bind(&CamApplication::on_timer, this, std::placeholders::_1)); runtime_.schedule(cam_interval_, std::bind(&CamApplication::on_timer, this, std::placeholders::_1));
} }
void CamApplication::on_timer(vanetza::Clock::time_point) { void CamApplication::on_timer(vanetza::Clock::time_point) {
calc_interval();
schedule_timer(); schedule_timer();
send(); send();
} }
@ -141,31 +83,9 @@ namespace v2x
return btp::ports::CAM; 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; // TODO: implement
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);
} }
void CamApplication::updateMGRS(double *x, double *y) { void CamApplication::updateMGRS(double *x, double *y) {
@ -173,7 +93,7 @@ namespace v2x
ego_.mgrs_y = *y; 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_.latitude = *lat;
ego_.longitude = *lon; ego_.longitude = *lon;
ego_.altitude = *altitude; ego_.altitude = *altitude;
@ -182,16 +102,16 @@ namespace v2x
positionConfidenceEllipse_.y.insert(*lon); 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; ego_.heading = *yaw;
} }
void CamApplication::setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &msg) { 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_radius = msg.wheel_radius;
vehicleDimensions_.wheel_width = msg.wheel_width; vehicleDimensions_.wheel_width = msg.wheel_width;
vehicleDimensions_.wheel_base = msg.wheel_base; vehicleDimensions_.wheel_base = msg.wheel_base;
@ -204,30 +124,40 @@ namespace v2x
} }
void CamApplication::updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) { 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); 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) { if (dt == 0) {
RCLCPP_WARN(node_->get_logger(), "[CamApplication::updateVelocityReport] deltaTime is 0"); RCLCPP_WARN(node_->get_logger(), "[CamApplication::updateVelocityReport] deltaTime is 0");
return; return;
} }
float longitudinal_acceleration = (msg->longitudinal_velocity - velocityReport_.longitudinal_velocity) / dt; 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_.stamp = msg->header.stamp;
velocityReport_.heading_rate = msg->heading_rate; velocityReport_.heading_rate = msg->heading_rate;
velocityReport_.lateral_velocity = msg->lateral_velocity; velocityReport_.lateral_velocity = msg->lateral_velocity;
velocityReport_.longitudinal_velocity = msg->longitudinal_velocity; velocityReport_.longitudinal_velocity = msg->longitudinal_velocity;
velocityReport_.speed = speed;
velocityReport_.longitudinal_acceleration = longitudinal_acceleration; velocityReport_.longitudinal_acceleration = longitudinal_acceleration;
updating_velocity_report_ = false;
} }
void CamApplication::updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) { void CamApplication::updateVehicleStatus(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
rclcpp::Time msg_stamp(msg->stamp.sec, msg->stamp.nanosec); if (updating_vehicle_status_) {
vehicleStatus_.gear = msg->report; 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; vehicleStatus_.steering_tire_angle = msg->steering_tire_angle;
updating_vehicle_status_ = false;
} }
void CamApplication::send() { void CamApplication::send() {
@ -240,7 +170,7 @@ namespace v2x
sending_ = true; 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; vanetza::asn1::Cam message;
@ -251,8 +181,8 @@ namespace v2x
CoopAwareness_t &cam = message->cam; CoopAwareness_t &cam = message->cam;
// Convert Unix timestamp to ETSI epoch (2004-01-01 00:00:00) RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] %ld", gdt_timestamp_);
cam.generationDeltaTime = (now_ms.count() - 1072915200000) % 65536; cam.generationDeltaTime = generationDeltaTime_;
BasicContainer_t &basic_container = cam.camParameters.basicContainer; BasicContainer_t &basic_container = cam.camParameters.basicContainer;
basic_container.stationType = StationType_passengerCar; basic_container.stationType = StationType_passengerCar;
@ -327,13 +257,13 @@ namespace v2x
uint8_t gearStatus = vehicleStatus_.gear; uint8_t gearStatus = vehicleStatus_.gear;
float steering_tire_angle = vehicleStatus_.steering_tire_angle; 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; if (0 <= speed && speed <= 16382) bvc.speed.speedValue = speed;
else bvc.speed.speedValue = SpeedValue_unavailable; else bvc.speed.speedValue = SpeedValue_unavailable;
if ((gearStatus >= 2 && gearStatus <= 19) || (gearStatus == 23 || gearStatus == 24)) if (gearStatus == 2 || gearStatus == 5)
bvc.driveDirection = DriveDirection_forward; bvc.driveDirection = DriveDirection_forward;
else if (gearStatus == 20 || gearStatus == 21) else if (gearStatus == 3)
bvc.driveDirection = DriveDirection_backward; bvc.driveDirection = DriveDirection_backward;
else else
bvc.driveDirection = DriveDirection_unavailable; bvc.driveDirection = DriveDirection_unavailable;
@ -370,8 +300,7 @@ namespace v2x
bvc.yawRate.yawRateConfidence = YawRateConfidence_unavailable; 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", RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM from station with ID %ld", stationId_);
stationId_, cam.generationDeltaTime, ego_.latitude, ego_.longitude, ego_.altitude);
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()}; std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
payload->layer(OsiLayer::Application) = std::move(message); payload->layer(OsiLayer::Application) = std::move(message);
@ -382,64 +311,18 @@ namespace v2x
Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); 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()) { if (!confirm.accepted()) {
throw std::runtime_error("[CamApplication::send] CAM application data request failed"); throw std::runtime_error("[CamApplication::send] CAM application data request failed");
} }
RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Successfully sent");
sending_ = false; sending_ = false;
std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds> ( std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds> (
std::chrono::system_clock::now().time_since_epoch() 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;
}
} }

8
src/cpm_application.cpp
View File

@ -101,8 +101,7 @@ namespace v2x {
// Calculate GDT and get GDT from CPM and calculate the "Age of CPM" // Calculate GDT and get GDT from CPM and calculate the "Age of CPM"
TimestampIts_t gt_cpm; TimestampIts_t gt_cpm = message->cpm.generationTime;
asn_long2INTEGER(&gt_cpm, (long) message->cpm.generationDeltaTime);
// const auto time_now = duration_cast<milliseconds> (runtime_.now().time_since_epoch()); // const auto time_now = duration_cast<milliseconds> (runtime_.now().time_since_epoch());
// uint16_t gdt = time_now.count(); // uint16_t gdt = time_now.count();
// int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536; // int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536;
@ -440,6 +439,7 @@ namespace v2x {
} }
void CpmApplication::send() { void CpmApplication::send() {
if (is_sender_) { if (is_sender_) {
sending_ = true; sending_ = true;
@ -448,8 +448,6 @@ namespace v2x {
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM..."); // 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; vanetza::asn1::Cpm message;
// ITS PDU Header // ITS PDU Header
@ -462,7 +460,7 @@ namespace v2x {
// Set GenerationTime // Set GenerationTime
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] %ld", gdt_timestamp_); 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; CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer;
management.stationType = StationType_passengerCar; management.stationType = StationType_passengerCar;

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/link_layer.hpp"
#include "autoware_v2x/cube_evk_link.hpp"
#include <boost/asio/generic/raw_protocol.hpp> #include <boost/asio/generic/raw_protocol.hpp>
#include <vanetza/access/ethertype.hpp> #include <vanetza/access/ethertype.hpp>
#include "autoware_v2x/raw_socket_link.hpp" #include "autoware_v2x/raw_socket_link.hpp"
std::unique_ptr<LinkLayer> create_link_layer( 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; 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); boost::asio::generic::raw_protocol::socket raw_socket(io_service, raw_protocol);
raw_socket.bind(device.endpoint(AF_PACKET)); raw_socket.bind(device.endpoint(AF_PACKET));
link_layer.reset(new RawSocketLink{std::move(raw_socket)}); 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 // Removed Cohda and UDP Support

120
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 "autoware_v2x/security.hpp"
#include <vanetza/security/certificate_cache.hpp>
#include <memory> #include <vanetza/security/default_certificate_validator.hpp>
#include <vanetza/security/delegating_security_entity.hpp> #include <vanetza/security/delegating_security_entity.hpp>
#include <vanetza/security/straight_verify_service.hpp> #include <vanetza/security/naive_certificate_provider.hpp>
#include "vanetza/security/v2/certificate_provider.hpp" #include <vanetza/security/null_certificate_validator.hpp>
#include <vanetza/security/v2/certificate_cache.hpp> #include <vanetza/security/persistence.hpp>
#include <vanetza/security/v2/default_certificate_validator.hpp> #include <vanetza/security/sign_header_policy.hpp>
#include <vanetza/security/v2/sign_service.hpp> #include <vanetza/security/static_certificate_provider.hpp>
#include <vanetza/security/v3/certificate_cache.hpp> #include <vanetza/security/trust_store.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 <stdexcept> #include <stdexcept>
using namespace vanetza; using namespace vanetza;
@ -30,18 +16,18 @@ namespace po = boost::program_options;
class SecurityContext : public security::SecurityEntity class SecurityContext : public security::SecurityEntity
{ {
public: public:
SecurityContext(const Runtime &runtime, PositionProvider &positioning) SecurityContext(const Runtime &runtime, PositionProvider &positioning) : runtime(runtime), positioning(positioning),
: runtime(runtime),
positioning(positioning),
backend(security::create_backend("default")), backend(security::create_backend("default")),
sign_header_policy(runtime, positioning), sign_header_policy(runtime, positioning),
cert_cache() cert_cache(runtime),
cert_validator(*backend, cert_cache, trust_store)
{ {
} }
security::EncapConfirm encapsulate_packet(security::EncapRequest &&request) override security::EncapConfirm encapsulate_packet(security::EncapRequest &&request) override
{ {
if (!entity) { if (!entity)
{
throw std::runtime_error("security entity is not ready"); throw std::runtime_error("security entity is not ready");
} }
return entity->encapsulate_packet(std::move(request)); return entity->encapsulate_packet(std::move(request));
@ -49,7 +35,8 @@ public:
security::DecapConfirm decapsulate_packet(security::DecapRequest &&request) override security::DecapConfirm decapsulate_packet(security::DecapRequest &&request) override
{ {
if (!entity) { if (!entity)
{
throw std::runtime_error("security entity is not ready"); throw std::runtime_error("security entity is not ready");
} }
return entity->decapsulate_packet(std::move(request)); return entity->decapsulate_packet(std::move(request));
@ -57,78 +44,31 @@ public:
void build_entity() void build_entity()
{ {
if (!cert_provider) { if (!cert_provider)
{
throw std::runtime_error("certificate provider is missing"); 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)}; security::SignService sign_service = straight_sign_service(*cert_provider, *backend, sign_header_policy);
std::unique_ptr<security::StraightVerifyService> verify_service{new security::StraightVerifyService(runtime, *backend, positioning)}; security::VerifyService verify_service = straight_verify_service(runtime, *cert_provider, cert_validator,
verify_service->use_certificate_cache(&cert_cache); *backend, cert_cache, sign_header_policy, positioning);
entity = std::make_unique<security::DelegatingSecurityEntity>(std::move(sign_service), std::move(verify_service)); entity.reset(new security::DelegatingSecurityEntity{sign_service, verify_service});
} }
const Runtime & runtime; const Runtime &runtime;
PositionProvider & positioning; PositionProvider &positioning;
std::unique_ptr<security::Backend> backend; std::unique_ptr<security::Backend> backend;
std::unique_ptr<security::SecurityEntity> entity; std::unique_ptr<security::SecurityEntity> entity;
std::unique_ptr<security::v3::CertificateProvider> cert_provider; std::unique_ptr<security::CertificateProvider> cert_provider;
security::v3::DefaultSignHeaderPolicy sign_header_policy; security::DefaultSignHeaderPolicy sign_header_policy;
security::v3::CertificateCache cert_cache; security::TrustStore trust_store;
security::CertificateCache cert_cache;
security::DefaultCertificateValidator cert_validator;
}; };
std::unique_ptr<security::v3::CertificateProvider> load_certificates( std::unique_ptr<security::SecurityEntity>
const std::string &cert_path, const std::string &cert_key_path, create_security_entity(const Runtime &runtime, PositionProvider &positioning)
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> security; 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; return security;
} }

65
src/v2x_app.cpp
View File

@ -24,7 +24,6 @@
#include <boost/date_time/posix_time/posix_time.hpp> #include <boost/date_time/posix_time/posix_time.hpp>
namespace gn = vanetza::geonet; namespace gn = vanetza::geonet;
namespace po = boost::program_options;
using namespace vanetza; using namespace vanetza;
using namespace std::chrono; using namespace std::chrono;
@ -51,21 +50,30 @@ namespace v2x
} }
} }
void V2XApp::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) { void V2XApp::setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &msg) {
if (cam_started_) if (cam_started_)
cam->setVehicleDimensions(msg);
else
this->setVehicleDimensions(msg);
}
void V2XApp::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
if (!velocity_report_received_) {
RCLCPP_WARN(node_->get_logger(), "[V2XApp::velocityReportCallback] VelocityReport not received yet");
}
if (velocity_report_received_ && cam_started_) {
cam->updateVelocityReport(msg); cam->updateVelocityReport(msg);
} }
void V2XApp::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
if (cam_started_)
cam->updateGearReport(msg);
} }
void V2XApp::steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) { void V2XApp::vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
if (cam_started_) if (!vehicle_status_received_) {
cam->updateSteeringReport(msg); RCLCPP_WARN(node_->get_logger(), "[V2XApp::vehicleStatusCallback] VehicleStatus not received yet");
}
if (vehicle_status_received_ && cam_started_) {
cam->updateVehicleStatus(msg);
}
} }
void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
@ -120,6 +128,7 @@ namespace v2x
cam->updateMGRS(&x, &y); cam->updateMGRS(&x, &y);
cam->updateRP(&lat, &lon, &z); cam->updateRP(&lat, &lon, &z);
cam->updateHeading(&yaw); cam->updateHeading(&yaw);
cam->updateGenerationDeltaTime(&gdt, &timestamp_msec);
} }
} }
@ -129,10 +138,6 @@ namespace v2x
boost::asio::io_service io_service; boost::asio::io_service io_service;
TimeTrigger trigger(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; std::string network_interface;
node_->get_parameter("network_interface", network_interface); node_->get_parameter("network_interface", network_interface);
RCLCPP_INFO(node_->get_logger(), "Network Interface: %s", network_interface.c_str()); RCLCPP_INFO(node_->get_logger(), "Network Interface: %s", network_interface.c_str());
@ -152,38 +157,10 @@ namespace v2x
mib.itsGnSecurity = false; mib.itsGnSecurity = false;
mib.itsGnProtocolVersion = 1; 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 // 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()); auto positioning = create_position_provider(io_service, trigger.runtime());
auto security = create_security_entity(trigger.runtime(), *positioning);
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");
RouterContext context(mib, trigger, *positioning, security.get()); RouterContext context(mib, trigger, *positioning, security.get());
context.set_link_layer(link_layer.get()); context.set_link_layer(link_layer.get());

56
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) { V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) {
using std::placeholders::_1; 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)); 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)); tf_sub_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));
// Topic subscriptions for CAMApplication // 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)); 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)); vehicle_status_sub_ = this->create_subscription<autoware_adapi_v1_msgs::msg::VehicleStatus>("/api/vehicle/status", 10, std::bind(&V2XNode::vehicleStatucCallback, 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)); 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_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}); // 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 // Declare Parameters
this->declare_parameter<std::string>("link_layer", "ethernet"); this->declare_parameter<std::string>("network_interface", "vmnet1");
this->declare_parameter<std::string>("network_interface", "v2x_testing");
this->declare_parameter<std::string>("cube_ip", "127.0.0.1");
this->declare_parameter<bool>("is_sender", true); this->declare_parameter<bool>("is_sender", true);
this->declare_parameter<std::string>("security", "none");
// Launch V2XApp in a new thread // Launch V2XApp in a new thread
app = new V2XApp(this); app = new V2XApp(this);
@ -175,11 +166,6 @@ namespace v2x
cpm_objects_pub_->publish(output_dynamic_object_msg); 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) { 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. rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg.
@ -199,34 +185,30 @@ namespace v2x
app->velocityReportCallback(msg); app->velocityReportCallback(msg);
} }
void V2XNode::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) { void V2XNode::vehicleStatucCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
app->gearReportCallback(msg); app->vehicleStatusCallback(msg);
}
void V2XNode::steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
app->steeringReportCallback(msg);
} }
void V2XNode::getVehicleDimensions() { 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"); 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 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) { auto future_result = get_vehicle_dimensions_->async_send_request(request, [this](rclcpp::Client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>::SharedFuture future) {
try { try
{
auto response = future.get(); auto response = future.get();
RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received response from /api/vehicle/dimensions"); RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received response from /api/vehicle/dimensions");
try { try {
auto dimensions = response->dimensions; 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); if (dimensions.height == 0 || dimensions.wheel_base == 0 || dimensions.wheel_tread == 0)
app->cam->setVehicleDimensions(dimensions); this->getVehicleDimensions();
else
app->setVehicleDimensions(dimensions);
} catch (const std::exception &e) { } catch (const std::exception &e) {
RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service response of /api/vehicle/dimensions failed: %s", e.what()); RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service response of /api/vehicle/dimensions failed: %s", e.what());
} }
} }
catch (const std::exception &e) { catch (const std::exception &e)
{
RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service call of /api/vehicle/dimensions failed: %s", e.what()); RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service call of /api/vehicle/dimensions failed: %s", e.what());
} }
}); });