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

[FORGE] fix 

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

138
src/cam_application.cpp
View File

@ -23,6 +23,7 @@
#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>
@ -159,7 +160,43 @@ 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));
CamApplication::build_etsi_its_cam_ts_from_vanetza(rec_cam, ts_cam); cam_ts_ItsPduHeader_t &header = ts_cam.header;
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));
@ -375,6 +412,58 @@ namespace v2x
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);
RCLCPP_INFO(node_->get_logger(), "Trynna build ros_msg");
cam_ts_CAM_t ts_cam;
std::memset(&ts_cam, 0, sizeof(ts_cam));
RCLCPP_INFO(node_->get_logger(), "Setting header");
cam_ts_ItsPduHeader_t &ros_header = ts_cam.header;
ros_header.protocolVersion = header.protocolVersion;
ros_header.messageId = header.messageID;
ros_header.stationId = header.stationID;
RCLCPP_INFO(node_->get_logger(), "Setting generationDeltaTime");
cam_ts_CamPayload_t &coopAwareness = ts_cam.cam;
coopAwareness.generationDeltaTime = cam.generationDeltaTime;
RCLCPP_INFO(node_->get_logger(), "Settings basic container");
cam_ts_BasicContainer_t &ros_basic_container = coopAwareness.camParameters.basicContainer;
ros_basic_container.stationType = basic_container.stationType;
ros_basic_container.referencePosition.latitude = basic_container.referencePosition.latitude;
ros_basic_container.referencePosition.longitude = basic_container.referencePosition.longitude;
ros_basic_container.referencePosition.altitude.altitudeValue = basic_container.referencePosition.altitude.altitudeValue;
ros_basic_container.referencePosition.altitude.altitudeConfidence = basic_container.referencePosition.altitude.altitudeConfidence;
ros_basic_container.referencePosition.positionConfidenceEllipse.semiMajorAxisLength = basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence;
ros_basic_container.referencePosition.positionConfidenceEllipse.semiMinorAxisLength = basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence;
ros_basic_container.referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation = basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation;
RCLCPP_INFO(node_->get_logger(), "Setting bvc");
cam_ts_BasicVehicleContainerHighFrequency_t &ros_bvc = coopAwareness.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
coopAwareness.camParameters.highFrequencyContainer.present = cam_ts_HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
ros_bvc.heading.headingValue = bvc.heading.headingValue;
ros_bvc.heading.headingConfidence = bvc.heading.headingConfidence;
ros_bvc.speed.speedValue = bvc.speed.speedValue;
ros_bvc.speed.speedConfidence = bvc.speed.speedConfidence;
ros_bvc.driveDirection = bvc.driveDirection;
ros_bvc.vehicleLength.vehicleLengthValue = bvc.vehicleLength.vehicleLengthValue;
ros_bvc.vehicleLength.vehicleLengthConfidenceIndication = bvc.vehicleLength.vehicleLengthConfidenceIndication;
ros_bvc.vehicleWidth = bvc.vehicleWidth;
ros_bvc.longitudinalAcceleration.value = bvc.longitudinalAcceleration.longitudinalAccelerationValue;
ros_bvc.longitudinalAcceleration.confidence = bvc.longitudinalAcceleration.longitudinalAccelerationConfidence;
ros_bvc.curvature.curvatureValue = bvc.curvature.curvatureValue;
ros_bvc.curvature.curvatureConfidence = bvc.curvature.curvatureConfidence;
ros_bvc.curvatureCalculationMode = bvc.curvatureCalculationMode;
ros_bvc.yawRate.yawRateValue = bvc.yawRate.yawRateValue;
ros_bvc.yawRate.yawRateConfidence = bvc.yawRate.yawRateConfidence;
RCLCPP_INFO(node_->get_logger(), "Converting to ROS");
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);
RCLCPP_INFO(node_->get_logger(), "Publishing received CAM");
node_->publishReceivedCam(ros_msg);
Application::DataRequest request; Application::DataRequest request;
request.its_aid = aid::CP; request.its_aid = aid::CP;
request.transport_type = geonet::TransportType::SHB; request.transport_type = geonet::TransportType::SHB;
@ -382,14 +471,6 @@ 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");
} }
@ -403,43 +484,4 @@ 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;
}
} }