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

Add formatted values 

- Missing confidence values

Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>
This commit is contained in:
Tiago Garcia 2024-06-28 14:11:04 +01:00
parent a2a54ba37f
commit 5510a362dc
No known key found for this signature in database
GPG Key ID: A3B1096AD19E1688
8 changed files with 123 additions and 162 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
config/autoware_v2x.param.yaml
View File

@ -1,4 +1,4 @@
/**: /**:
ros__parameters: ros__parameters:
network_interface: "enx5414a7c7d3af" network_interface: "v2x_testing"
is_sender: true is_sender: true

73
include/autoware_v2x/cam_application.hpp
View File

@ -7,6 +7,7 @@
#include <boost/asio/steady_timer.hpp> #include <boost/asio/steady_timer.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_auto_vehicle_msgs/msg/gear_report.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_dimensions.hpp"
#include "autoware_v2x/positioning.hpp" #include "autoware_v2x/positioning.hpp"
#include <vanetza/asn1/cam.hpp> #include <vanetza/asn1/cam.hpp>
@ -19,71 +20,18 @@ class CamApplication : public Application
public: public:
CamApplication(V2XNode *node, vanetza::Runtime &, bool is_sender); CamApplication(V2XNode *node, vanetza::Runtime &, bool is_sender);
PortType port() override; PortType port() override;
//std::string uuidToHexString(const unique_identifier_msgs::msg::UUID&);
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 setAllObjectsOfPersonsAnimalsToSend(const autoware_auto_control_msgs::msg::LongitudinalCommand::ConstSharedPtr);
void getVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions); void getVehicleDimensions(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 updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr);
void updateSteeringReport(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr);
void updateMGRS(double *, double *); void updateMGRS(double *, double *);
void updateRP(double *, double *, double *); void updateRP(double *, double *, double *);
void updateGenerationTime(int *, long *); void updateGenerationTime(int *, long *);
void updateHeading(double *); void updateHeading(double *);
//void printObjectsList(int);
void send(); void send();
/** Creates the database schema.
* Creates tables for cam_sent, cam_received
*/
//void createTables();
/** Inserts CAM into the database.
* Constructs and executes queries for inserting the specified CAM data.
* @param cam The CAM to be inserted
* @param table_name The table to insert the CAM into (cam_sent or cam_received)
*/
//void insertCamToCamTable(vanetza::asn1::Cam, char*);
struct Object {
std::string uuid;
int vehicleID;
vanetza::Clock::time_point timestamp;
rclcpp::Time timestamp_ros;
// BasicContainer
long stationType;
long latitude; // Obtained from tfMessage (same value as CPMs)
long longitude; // Obtained from tfMessage (same value as CPMs)
long semiMajorConfidence; // Confidence (?)
long semiMinorConfidence; // Confidence (?)
long semiMajorOrientation; // Confidence (?)
long altitude; // Obtained from tfMessage (same value as CPMs)
long altitudeConfidence; // Confidence (?)
// BasicVehicleContainerHighFrequency
long heading; // Obtained from tfMessage (same value as CPMs)
long headingConfidence; // Confidence (?)
long speed; // Obtained from velocityReport after calculation
long speedConfidence; // Confidence (?)
long driveDirection; // Obtained from gearReport
long vehicleLength;
long vehicleLengthConfidence; // Confidence (?)
long vehicleWidth;
long longitudinalAcceleration; // Obtained from velocityReport after calculation over time
long longitudinalAccelerationConfidence; // Confidence (?)
long curvature; // Obtained from velocityReport after calculation
long curvatureConfidence; // Confidence (?)
long curvatureCalculationMode; // Manually set
long yawRate; // Obtained from velocityReport
long yawRateConfidence; // Confidence (?)
vanetza::PositionFix position;
int timeOfMeasurement;
bool to_send;
int to_send_trigger;
};
private: private:
void schedule_timer(); void schedule_timer();
void on_timer(vanetza::Clock::time_point); void on_timer(vanetza::Clock::time_point);
@ -123,9 +71,19 @@ private:
float longitudinal_acceleration; float longitudinal_acceleration;
}; };
VelocityReport velocityReport_; VelocityReport velocityReport_;
uint8_t gearReport_; struct GearReport {
rclcpp::Time stamp;
uint8_t report;
};
GearReport gearReport_;
struct SteeringReport {
rclcpp::Time stamp;
float steering_tire_angle;
};
SteeringReport steeringReport_;
int generationTime_; int generationTime_;
long gdt_timestamp_; long gdt_timestamp_;
@ -133,6 +91,7 @@ private:
bool updating_velocity_report_; bool updating_velocity_report_;
bool updating_gear_report_; bool updating_gear_report_;
bool updating_steering_report_;
bool sending_; bool sending_;
bool is_sender_; bool is_sender_;
bool reflect_packet_; bool reflect_packet_;

4
include/autoware_v2x/v2x_app.hpp
View File

@ -6,6 +6,7 @@
#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_auto_vehicle_msgs/msg/gear_report.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_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>
@ -31,6 +32,7 @@ namespace v2x
void getVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions); void getVehicleDimensions(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 gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::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;
@ -48,6 +50,8 @@ namespace v2x
int velocity_report_interval_; int velocity_report_interval_;
bool gear_report_received_; bool gear_report_received_;
int gear_report_interval_; int gear_report_interval_;
bool steering_report_received_;
int steering_report_interval_;
bool vehicle_dimensions_set_; bool vehicle_dimensions_set_;
bool cp_started_; bool cp_started_;
bool cam_started_; bool cam_started_;

3
include/autoware_v2x/v2x_node.hpp
View File

@ -6,6 +6,7 @@
#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_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/srv/get_vehicle_dimensions.hpp"
#include "autoware_adapi_v1_msgs/msg/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"
@ -37,12 +38,14 @@ namespace v2x
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 gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg);
void steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg);
void getVehicleDimensionsCallback(const autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response::ConstSharedPtr msg); void getVehicleDimensionsCallback(const autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response::ConstSharedPtr msg);
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_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::srv::GetVehicleDimensions::Response>::SharedPtr get_vehicle_dimensions_; rclcpp::Subscription<autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response>::SharedPtr get_vehicle_dimensions_;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_sub_; rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_sub_;
rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cpm_objects_pub_; rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cpm_objects_pub_;

4
launch/v2x.launch.xml
View File

@ -1,7 +1,7 @@
<launch> <launch>
<arg name="network_interface" default="enx5414a7c7d3af"/> <arg name="network_interface" default="v2x_testing"/>
<arg name="is_sender" default="true"/> <arg name="is_sender" default="true"/>
<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>
</launch> </launch>

167
src/cam_application.cpp
View File

@ -40,9 +40,11 @@ namespace v2x
ego_(), ego_(),
velocityReport_(), velocityReport_(),
gearReport_(), gearReport_(),
steeringReport_(),
generationTime_(0), generationTime_(0),
updating_velocity_report_(false), updating_velocity_report_(false),
updating_gear_report_(false), updating_gear_report_(false),
updating_steering_report_(false),
sending_(false), sending_(false),
is_sender_(is_sender), is_sender_(is_sender),
reflect_packet_(false), reflect_packet_(false),
@ -71,19 +73,11 @@ namespace v2x
schedule_timer(); schedule_timer();
send(); send();
} }
CamApplication::PortType CamApplication::port() { CamApplication::PortType CamApplication::port() {
return btp::ports::CAM; return btp::ports::CAM;
} }
//std::string CamApplication::uuidToHexString(const unique_identifier_msgs::msg::UUID &id) {
// std::stringstream ss;
// for (int i = 0; i < id.uuid.size(); i++) {
// ss << std::hex << std::setfill('0') << std::setw(2) << (int)id.uuid[i];
// }
// return ss.str();
//}
void CamApplication::indicate(const Application::DataIndication &, Application::UpPacketPtr) void CamApplication::indicate(const Application::DataIndication &, Application::UpPacketPtr)
{ {
// TODO: implement // TODO: implement
@ -93,13 +87,13 @@ namespace v2x
ego_.mgrs_x = *x; ego_.mgrs_x = *x;
ego_.mgrs_y = *y; ego_.mgrs_y = *y;
} }
void CamApplication::updateRP(double *lat, double *lon, 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;
} }
void CamApplication::updateGenerationTime(int *gdt, long *gdt_timestamp) { void CamApplication::updateGenerationTime(int *gdt, long *gdt_timestamp) {
generationTime_ = *gdt; generationTime_ = *gdt;
gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
@ -109,23 +103,6 @@ namespace v2x
ego_.heading = *yaw; ego_.heading = *yaw;
} }
//void CamApplication::setAllObjectsOfPersonsAnimalsToSend(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) {
// if (msg->objects.size() > 0) {
// for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) {
// std::string object_uuid = uuidToHexString(obj.object_id);
// auto found_object = std::find_if(objectsList.begin(), objectsList.end(), [&](auto const &e) {
// return !strcmp(e.uuid.c_str(), object_uuid.c_str());
// });
//
// if (found_object == objectsList.end()) {
//
// } else {
// found_object->to_send = true;
// }
// }
// }
//}
void CamApplication::getVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions msg) { void CamApplication::getVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions msg) {
vehicleDimensions_.wheel_radius = msg.wheel_radius; vehicleDimensions_.wheel_radius = msg.wheel_radius;
vehicleDimensions_.wheel_width = msg.wheel_width; vehicleDimensions_.wheel_width = msg.wheel_width;
@ -137,23 +114,23 @@ namespace v2x
vehicleDimensions_.right_overhang = msg.right_overhang; vehicleDimensions_.right_overhang = msg.right_overhang;
vehicleDimensions_.height = msg.height; vehicleDimensions_.height = msg.height;
} }
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_) { if (updating_velocity_report_) {
RCLCPP_WARN(node_->get_logger(), "[CamApplication::updatevelocityReport_] already updating velocity report"); RCLCPP_WARN(node_->get_logger(), "[CamApplication::updateVelocityReport] already updating velocity report");
return; return;
} }
updating_velocity_report_ = true; 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);
float dt = (msg_stamp - velocityReport_.stamp).seconds(); float dt = (msg_stamp - 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;
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;
@ -163,49 +140,61 @@ namespace v2x
void CamApplication::updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) { void CamApplication::updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
if (updating_gear_report_) { if (updating_gear_report_) {
RCLCPP_WARN(node_->get_logger(), "[CamApplication::updatevelocityReport_] already updating gear report"); RCLCPP_WARN(node_->get_logger(), "[CamApplication::updateGearReport] already updating gear report");
return; return;
} }
updating_gear_report_ = true; updating_gear_report_ = true;
gearReport_ = msg->report; gearReport_.stamp = msg->stamp;
gearReport_.report = msg->report;
} }
void CamApplication::updateSteeringReport(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
if (updating_steering_report_) {
RCLCPP_WARN(node_->get_logger(), "[CamApplication::updateSteeringReport] already updating steering report");
return;
}
updating_steering_report_ = true;
steeringReport_.stamp = msg->stamp;
steeringReport_.steering_tire_angle = msg->steering_tire_angle;
}
void CamApplication::send() { void CamApplication::send() {
if (!is_sender_) return; if (!is_sender_) return;
if (sending_) { if (sending_) {
RCLCPP_WARN(node_->get_logger(), "[CamApplication::send] already sending"); RCLCPP_WARN(node_->get_logger(), "[CamApplication::send] already sending");
return; return;
} }
sending_ = true; sending_ = true;
//printObjectsList(cam_num_);
RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] cam_num: %d", cam_num_); RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] cam_num: %d", cam_num_);
// RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] sending CAM");
vanetza::asn1::Cam message; vanetza::asn1::Cam message;
ItsPduHeader_t &header = message->header; ItsPduHeader_t &header = message->header;
header.protocolVersion = 2; header.protocolVersion = 2;
header.messageID = ItsPduHeader__messageID_cam; header.messageID = ItsPduHeader__messageID_cam;
header.stationID = 0; header.stationID = cam_num_;
CoopAwareness_t &cam = message->cam; CoopAwareness_t &cam = message->cam;
//asn_long2INTEGER(&cam.generationDeltaTime, (long) gdt_timestamp_); cam.generationDeltaTime = std::chrono::duration_cast<std::chrono::milliseconds>(cam_interval_).count();
BasicContainer_t &basic_container = cam.camParameters.basicContainer; BasicContainer_t &basic_container = cam.camParameters.basicContainer;
basic_container.stationType = StationType_passengerCar; basic_container.stationType = StationType_passengerCar;
basic_container.referencePosition.latitude = ego_.latitude * 1e7; float latitude = ego_.latitude * 1e7;
basic_container.referencePosition.longitude = ego_.longitude * 1e7; float longitude = ego_.longitude * 1e7;
basic_container.referencePosition.altitude.altitudeValue = ego_.altitude; float altitude = ego_.altitude * 100;
basic_container.referencePosition.latitude = latitude >= -900000000 && latitude <= 900000000 ? latitude : Latitude_unavailable;
basic_container.referencePosition.longitude = longitude >= -1800000000 && longitude <= 1800000000 ? longitude : Longitude_unavailable;
basic_container.referencePosition.altitude.altitudeValue = altitude > -100000 && altitude < 800000 ? altitude : AltitudeValue_unavailable;
// UNAVAILABLE VALUES FOR TESTING // UNAVAILABLE VALUES FOR TESTING
basic_container.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable;
basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable; basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
@ -213,55 +202,48 @@ namespace v2x
BasicVehicleContainerHighFrequency_t &bvc = cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; BasicVehicleContainerHighFrequency_t &bvc = cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
int heading = std::lround(((-ego_.heading * 180.0 / M_PI) + 90.0) * 10.0); int heading = std::lround(((-ego_.heading * 180.0 / M_PI) + 90.0) * 10.0);
if (heading < 0) heading += 3600; if (heading < 0) heading += 3600;
bvc.heading.headingValue = heading; bvc.heading.headingValue = heading;
float heading_rate = velocityReport_.heading_rate; float heading_rate = velocityReport_.heading_rate;
float lateral_velocity = velocityReport_.lateral_velocity; float lateral_velocity = velocityReport_.lateral_velocity;
float longitudinal_velocity = velocityReport_.longitudinal_velocity; float longitudinal_velocity = velocityReport_.longitudinal_velocity;
float longitudinal_acceleration = std::lround(velocityReport_.longitudinal_acceleration * 100);
float speed = std::sqrt(std::pow(longitudinal_velocity, 2) + std::pow(lateral_velocity, 2)); uint8_t gearStatus = gearReport_.report;
bvc.speed.speedValue = std::lround(speed * 100); float steering_tire_angle = steeringReport_.steering_tire_angle;
if ((gearReport_ >= 2 && gearReport_ <= 19) || gearReport_ == 21 || gearReport_ == 22) float speed = std::lround(std::sqrt(std::pow(longitudinal_velocity, 2) + std::pow(lateral_velocity, 2)) * 100);
bvc.speed.speedValue = speed >= 0 && speed < 16382 ? speed : SpeedValue_unavailable;
if ((gearStatus >= 2 && gearStatus <= 19) || gearStatus == 21 || gearStatus == 22)
bvc.driveDirection = DriveDirection_forward; bvc.driveDirection = DriveDirection_forward;
else if (gearReport_ == 20 || gearReport_ == 21) else if (gearStatus == 20 || gearStatus == 21)
bvc.driveDirection = DriveDirection_backward; bvc.driveDirection = DriveDirection_backward;
else else
bvc.driveDirection = DriveDirection_unavailable; bvc.driveDirection = DriveDirection_unavailable;
float vehicleLength = vehicleDimensions_.front_overhang + vehicleDimensions_.wheel_base + vehicleDimensions_.rear_overhang;
if (vehicleLength <= 0 || vehicleLength >= 1023)
bvc.vehicleLength.vehicleLengthValue = VehicleLengthValue_unavailable;
else
bvc.vehicleLength.vehicleLengthValue = vehicleLength;
float vehicleWidth = vehicleDimensions_.left_overhang + vehicleDimensions_.wheel_tread + vehicleDimensions_.right_overhang;
if (vehicleWidth <= 0 || vehicleWidth >= 62)
bvc.vehicleWidth = VehicleWidth_unavailable;
else
bvc.vehicleWidth = vehicleWidth;
//if (longitudinal_acceleration > 160 || longitudinal_acceleration < -160) { float vehicleLength = std::lround(vehicleDimensions_.front_overhang + vehicleDimensions_.wheel_base + vehicleDimensions_.rear_overhang * 100);
// RCLCPP_WARN(node_->get_logger(), "[CamApplication::send] Longitudinal acceleration out of bounds: %d", longitudinal_acceleration); bvc.vehicleLength.vehicleLengthValue = vehicleLength >= 0 && vehicleLength <= 1022 ? vehicleLength : VehicleLengthValue_unavailable;
// bvc.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
//} else { float vehicleWidth = std::lround(vehicleDimensions_.left_overhang + vehicleDimensions_.wheel_tread + vehicleDimensions_.right_overhang * 100);
// bvc.longitudinalAcceleration.longitudinalAccelerationValue = longitudinal_acceleration; bvc.vehicleWidth = vehicleWidth >= 0 && vehicleWidth <= 61 ? vehicleWidth : VehicleWidth_unavailable;
//}
bvc.longitudinalAcceleration.longitudinalAccelerationValue = longitudinal_acceleration >= -160 && longitudinal_acceleration <= 160 ? longitudinal_acceleration : LongitudinalAccelerationValue_unavailable;
bvc.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
long curvature = longitudinal_velocity != 0 ? std::abs(std::lround(lateral_velocity / std::pow(longitudinal_velocity, 2) * 100)) * (steering_tire_angle < 0 ? -1 : 1)
if (longitudinal_velocity != 0) : std::numeric_limits<long>::infinity();
bvc.curvature.curvatureValue = std::lround(lateral_velocity / std::pow(longitudinal_velocity, 2) * 100); bvc.curvature.curvatureValue = curvature >= -1023 && curvature <= 1022 ? curvature : CurvatureValue_unavailable;
else
bvc.curvature.curvatureValue = std::numeric_limits<long>::infinity();
bvc.curvatureCalculationMode = CurvatureCalculationMode_yawRateNotUsed; bvc.curvatureCalculationMode = CurvatureCalculationMode_yawRateNotUsed;
bvc.yawRate.yawRateValue = std::lround(heading_rate * 100); long heading_rate_deg = std::abs(std::lround(heading_rate * (180.0 / M_PI))) * (steering_tire_angle < 0 ? -1 : 1);
bvc.yawRate.yawRateValue = heading_rate_deg >= -32766 && heading_rate_deg <= 32766 ? heading_rate_deg : YawRateValue_unavailable;
// UNAVAILABLE VALUES FOR TESTING // UNAVAILABLE VALUES FOR TESTING
basic_container.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable;
// ------------------------------
bvc.heading.headingConfidence = HeadingConfidence_unavailable; bvc.heading.headingConfidence = HeadingConfidence_unavailable;
bvc.speed.speedConfidence = SpeedConfidence_unavailable; bvc.speed.speedConfidence = SpeedConfidence_unavailable;
bvc.vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; bvc.vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable;
@ -269,19 +251,18 @@ namespace v2x
bvc.curvature.curvatureConfidence = CurvatureConfidence_unavailable; bvc.curvature.curvatureConfidence = CurvatureConfidence_unavailable;
bvc.yawRate.yawRateConfidence = YawRateConfidence_unavailable; bvc.yawRate.yawRateConfidence = YawRateConfidence_unavailable;
// ------------------------------ // ------------------------------
RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM"); RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM");
//insertCamToCamTable(message, (char *) "cam_sent");
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);
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;
request.communication_profile = geonet::CommunicationProfile::ITS_G5; request.communication_profile = geonet::CommunicationProfile::ITS_G5;
Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
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");
} }
@ -296,4 +277,4 @@ namespace v2x
++cam_num_; ++cam_num_;
} }
} }

13
src/v2x_app.cpp
View File

@ -58,7 +58,7 @@ namespace v2x
void V2XApp::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) { void V2XApp::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
if (!velocity_report_received_) { if (!velocity_report_received_) {
RCLCPP_WARN(node_->get_logger(), "[V2XApp::velocityReportCallback VelocityReport not received yet"); RCLCPP_WARN(node_->get_logger(), "[V2XApp::velocityReportCallback] VelocityReport not received yet");
} }
if (velocity_report_received_ && cam_started_) { if (velocity_report_received_ && cam_started_) {
cam->updateVelocityReport(msg); cam->updateVelocityReport(msg);
@ -67,13 +67,22 @@ namespace v2x
void V2XApp::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) { void V2XApp::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
if (!gear_report_received_) { if (!gear_report_received_) {
RCLCPP_WARN(node_->get_logger(), "[V2XApp::gearReportCallback GearReport not received yet"); RCLCPP_WARN(node_->get_logger(), "[V2XApp::gearReportCallback] GearReport not received yet");
} }
if (gear_report_received_ && cam_started_) { if (gear_report_received_ && cam_started_) {
cam->updateGearReport(msg); cam->updateGearReport(msg);
} }
} }
void V2XApp::steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
if (!steering_report_received_) {
RCLCPP_WARN(node_->get_logger(), "[V2XApp::gearReportCallback] SteeringReport not received yet");
}
if (steering_report_received_ && cam_started_) {
cam->updateSteeringReport(msg);
}
}
void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
tf_received_ = true; tf_received_ = true;

17
src/v2x_node.cpp
View File

@ -35,17 +35,18 @@ namespace v2x
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)); 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));
get_vehicle_dimensions_ = this->create_subscription<autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response>("/api/vehicle/dimensions", 10, std::bind(&V2XNode::getVehicleDimensionsCallback, this, _1)); get_vehicle_dimensions_ = this->create_subscription<autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response>("/api/vehicle/dimensions", 10, std::bind(&V2XNode::getVehicleDimensionsCallback, this, _1));
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});
// Declare Parameters // Declare Parameters
this->declare_parameter<std::string>("network_interface", "enx5414a7c7d3af"); this->declare_parameter<std::string>("network_interface", "v2x_testing");
this->declare_parameter<bool>("is_sender", true); this->declare_parameter<bool>("is_sender", true);
// Launch V2XApp in a new thread // Launch V2XApp in a new thread
@ -171,19 +172,23 @@ namespace v2x
void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
app->tfCallback(msg); app->tfCallback(msg);
} }
void V2XNode::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) { void V2XNode::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
app->velocityReportCallback(msg); app->velocityReportCallback(msg);
} }
void V2XNode::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) { void V2XNode::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
app->gearReportCallback(msg); app->gearReportCallback(msg);
} }
void V2XNode::steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
app->steeringReportCallback(msg);
}
void V2XNode::getVehicleDimensionsCallback(const autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response::ConstSharedPtr msg) { void V2XNode::getVehicleDimensionsCallback(const autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response::ConstSharedPtr msg) {
app->getVehicleDimensions(msg->dimensions); app->getVehicleDimensions(msg->dimensions);
} }
} }
#include "rclcpp_components/register_node_macro.hpp" #include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(v2x::V2XNode) RCLCPP_COMPONENTS_REGISTER_NODE(v2x::V2XNode)