Compare commits
No commits in common. "e9b85635a381fd80ae66f7d8a202a4427fbcc1f7" and "73756de59ec94188a4ea8617916cbddaeb0f309f" have entirely different histories.
e9b85635a3
...
73756de59e
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -1,11 +1,4 @@
|
|||
/**:
|
||||
ros__parameters:
|
||||
link_layer: "cube-evk"
|
||||
network_interface: "v2x_testing"
|
||||
cube_ip: "192.168.94.84"
|
||||
network_interface: "wlp4s0"
|
||||
is_sender: true
|
||||
security: "none"
|
||||
certificate: ""
|
||||
certificate-key: ""
|
||||
certificate-chain:
|
||||
- ""
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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_ */
|
||||
|
|
@ -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 */
|
||||
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
};
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,9 +1,6 @@
|
|||
<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>
|
||||
|
|
|
|||
|
|
@ -19,7 +19,6 @@
|
|||
<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>
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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(>_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;
|
||||
|
|
@ -440,6 +439,7 @@ namespace v2x {
|
|||
}
|
||||
|
||||
void CpmApplication::send() {
|
||||
|
||||
if (is_sender_) {
|
||||
|
||||
sending_ = true;
|
||||
|
|
@ -448,8 +448,6 @@ namespace v2x {
|
|||
|
||||
// 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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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
124
src/security.cpp
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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, ×tamp_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());
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue