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

[FORGE] test 

Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>
This commit is contained in:
Tiago Garcia 2024-10-28 10:19:01 +00:00
parent f96f8c6418
commit e9b85635a3
No known key found for this signature in database
GPG Key ID: A3B1096AD19E1688
4 changed files with 63 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -25,6 +25,7 @@ find_package(GeographicLib 1.37 REQUIRED)
find_package(Boost COMPONENTS thread program_options REQUIRED) find_package(Boost COMPONENTS thread program_options REQUIRED)
find_package(etsi_its_cam_ts_coding REQUIRED) find_package(etsi_its_cam_ts_coding REQUIRED)
find_package(etsi_its_cam_ts_conversion 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) find_package(Protobuf REQUIRED)

4
config/autoware_v2x.param.yaml
View File

@ -1,8 +1,8 @@
/**: /**:
ros__parameters: ros__parameters:
link_layer: "ethernet" link_layer: "cube-evk"
network_interface: "v2x_testing" network_interface: "v2x_testing"
cube_ip: "127.0.0.1" cube_ip: "192.168.94.84"
is_sender: true is_sender: true
security: "none" security: "none"
certificate: "" certificate: ""

18
include/autoware_v2x/cam_application.hpp
View File

@ -2,15 +2,20 @@
#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 <boost/asio/io_service.hpp>
#include <boost/asio/steady_timer.hpp> #include <vanetza/asn1/cam.hpp>
#include "autoware_auto_vehicle_msgs/msg/velocity_report.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/gear_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp" #include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
#include "autoware_v2x/positioning.hpp"
#include <vanetza/asn1/cam.hpp> #include <boost/asio/io_service.hpp>
#include <boost/asio/steady_timer.hpp>
#include <etsi_its_cam_ts_coding/cam_ts_CAM.h>
namespace v2x namespace v2x
{ {
@ -35,6 +40,7 @@ private:
void calc_interval(); 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_;

86
src/cam_application.cpp
View File

@ -23,7 +23,6 @@
#include <string> #include <string>
#include <etsi_its_cam_ts_msgs/msg/cam.hpp> #include <etsi_its_cam_ts_msgs/msg/cam.hpp>
#include <etsi_its_cam_ts_coding/cam_ts_CAM.h>
#include <etsi_its_cam_ts_conversion/convertCAM.h> #include <etsi_its_cam_ts_conversion/convertCAM.h>
#include <boost/units/cmath.hpp> #include <boost/units/cmath.hpp>
@ -160,43 +159,7 @@ namespace v2x
cam_ts_CAM_t ts_cam; cam_ts_CAM_t ts_cam;
std::memset(&ts_cam, 0, sizeof(ts_cam)); std::memset(&ts_cam, 0, sizeof(ts_cam));
cam_ts_ItsPduHeader_t &header = ts_cam.header; CamApplication::build_etsi_its_cam_ts_from_vanetza(rec_cam, ts_cam);
header.protocolVersion = rec_cam->header.protocolVersion;
header.messageId = rec_cam->header.messageID;
header.stationId = rec_cam->header.stationID;
cam_ts_CamPayload_t &coopAwareness = ts_cam.cam;
coopAwareness.generationDeltaTime = rec_cam->cam.generationDeltaTime;
cam_ts_BasicContainer_t &basic_container = coopAwareness.camParameters.basicContainer;
BasicContainer_t &rec_basic_container = rec_cam->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 = rec_cam->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;
etsi_its_cam_ts_msgs::msg::CAM ros_msg; etsi_its_cam_ts_msgs::msg::CAM ros_msg;
std::memset(&ros_msg, 0, sizeof(ros_msg)); std::memset(&ros_msg, 0, sizeof(ros_msg));
@ -419,6 +382,14 @@ 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");
} }
@ -432,4 +403,43 @@ namespace v2x
); );
} }
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;
}
} }