Add formatted values
- Missing confidence values Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>
This commit is contained in:
parent
a2a54ba37f
commit
5510a362dc
|
|
@ -1,4 +1,4 @@
|
|||
/**:
|
||||
ros__parameters:
|
||||
network_interface: "enx5414a7c7d3af"
|
||||
network_interface: "v2x_testing"
|
||||
is_sender: true
|
||||
|
|
@ -7,6 +7,7 @@
|
|||
#include <boost/asio/steady_timer.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/msg/vehicle_dimensions.hpp"
|
||||
#include "autoware_v2x/positioning.hpp"
|
||||
#include <vanetza/asn1/cam.hpp>
|
||||
|
|
@ -19,71 +20,18 @@ class CamApplication : public Application
|
|||
public:
|
||||
CamApplication(V2XNode *node, vanetza::Runtime &, bool is_sender);
|
||||
PortType port() override;
|
||||
//std::string uuidToHexString(const unique_identifier_msgs::msg::UUID&);
|
||||
void indicate(const DataIndication &, UpPacketPtr) override;
|
||||
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 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 updateMGRS(double *, double *);
|
||||
void updateRP(double *, double *, double *);
|
||||
void updateGenerationTime(int *, long *);
|
||||
void updateHeading(double *);
|
||||
//void printObjectsList(int);
|
||||
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:
|
||||
void schedule_timer();
|
||||
void on_timer(vanetza::Clock::time_point);
|
||||
|
|
@ -124,7 +72,17 @@ private:
|
|||
};
|
||||
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_;
|
||||
long gdt_timestamp_;
|
||||
|
|
@ -133,6 +91,7 @@ private:
|
|||
|
||||
bool updating_velocity_report_;
|
||||
bool updating_gear_report_;
|
||||
bool updating_steering_report_;
|
||||
bool sending_;
|
||||
bool is_sender_;
|
||||
bool reflect_packet_;
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@
|
|||
#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/msg/vehicle_dimensions.hpp"
|
||||
#include "tf2_msgs/msg/tf_message.hpp"
|
||||
#include <boost/asio/io_service.hpp>
|
||||
|
|
@ -31,6 +32,7 @@ namespace v2x
|
|||
void getVehicleDimensions(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 tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr);
|
||||
|
||||
CpmApplication *cp;
|
||||
|
|
@ -48,6 +50,8 @@ namespace v2x
|
|||
int velocity_report_interval_;
|
||||
bool gear_report_received_;
|
||||
int gear_report_interval_;
|
||||
bool steering_report_received_;
|
||||
int steering_report_interval_;
|
||||
bool vehicle_dimensions_set_;
|
||||
bool cp_started_;
|
||||
bool cam_started_;
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@
|
|||
#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 "tf2_msgs/msg/tf_message.hpp"
|
||||
|
|
@ -37,12 +38,14 @@ namespace v2x
|
|||
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 getVehicleDimensionsCallback(const autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response::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_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::srv::GetVehicleDimensions::Response>::SharedPtr get_vehicle_dimensions_;
|
||||
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_sub_;
|
||||
rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cpm_objects_pub_;
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
<launch>
|
||||
<arg name="network_interface" default="enx5414a7c7d3af"/>
|
||||
<arg name="network_interface" default="v2x_testing"/>
|
||||
<arg name="is_sender" default="true"/>
|
||||
<node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen">
|
||||
<param from="$(find-pkg-share autoware_v2x)/config/autoware_v2x.param.yaml"/>
|
||||
|
|
|
|||
|
|
@ -40,9 +40,11 @@ namespace v2x
|
|||
ego_(),
|
||||
velocityReport_(),
|
||||
gearReport_(),
|
||||
steeringReport_(),
|
||||
generationTime_(0),
|
||||
updating_velocity_report_(false),
|
||||
updating_gear_report_(false),
|
||||
updating_steering_report_(false),
|
||||
sending_(false),
|
||||
is_sender_(is_sender),
|
||||
reflect_packet_(false),
|
||||
|
|
@ -76,14 +78,6 @@ namespace v2x
|
|||
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)
|
||||
{
|
||||
// TODO: implement
|
||||
|
|
@ -109,23 +103,6 @@ namespace v2x
|
|||
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) {
|
||||
vehicleDimensions_.wheel_radius = msg.wheel_radius;
|
||||
vehicleDimensions_.wheel_width = msg.wheel_width;
|
||||
|
|
@ -140,7 +117,7 @@ namespace v2x
|
|||
|
||||
void CamApplication::updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -149,7 +126,7 @@ namespace v2x
|
|||
rclcpp::Time msg_stamp(msg->header.stamp.sec, msg->header.stamp.nanosec);
|
||||
float dt = (msg_stamp - velocityReport_.stamp).seconds();
|
||||
if (dt == 0) {
|
||||
RCLCPP_WARN(node_->get_logger(), "[CamApplication::updatevelocityReport_] deltaTime is 0");
|
||||
RCLCPP_WARN(node_->get_logger(), "[CamApplication::updateVelocityReport] deltaTime is 0");
|
||||
return;
|
||||
}
|
||||
float longitudinal_acceleration = (msg->longitudinal_velocity - velocityReport_.longitudinal_velocity) / dt;
|
||||
|
|
@ -163,13 +140,26 @@ namespace v2x
|
|||
|
||||
void CamApplication::updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
|
||||
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;
|
||||
}
|
||||
|
||||
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() {
|
||||
|
|
@ -182,30 +172,29 @@ namespace v2x
|
|||
|
||||
sending_ = true;
|
||||
|
||||
//printObjectsList(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;
|
||||
|
||||
ItsPduHeader_t &header = message->header;
|
||||
header.protocolVersion = 2;
|
||||
header.messageID = ItsPduHeader__messageID_cam;
|
||||
header.stationID = 0;
|
||||
header.stationID = cam_num_;
|
||||
|
||||
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;
|
||||
basic_container.stationType = StationType_passengerCar;
|
||||
basic_container.referencePosition.latitude = ego_.latitude * 1e7;
|
||||
basic_container.referencePosition.longitude = ego_.longitude * 1e7;
|
||||
basic_container.referencePosition.altitude.altitudeValue = ego_.altitude;
|
||||
float latitude = ego_.latitude * 1e7;
|
||||
float longitude = ego_.longitude * 1e7;
|
||||
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
|
||||
basic_container.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable;
|
||||
basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
|
||||
basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
|
||||
basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
|
||||
|
|
@ -221,47 +210,40 @@ namespace v2x
|
|||
float heading_rate = velocityReport_.heading_rate;
|
||||
float lateral_velocity = velocityReport_.lateral_velocity;
|
||||
float longitudinal_velocity = velocityReport_.longitudinal_velocity;
|
||||
float longitudinal_acceleration = std::lround(velocityReport_.longitudinal_acceleration * 100);
|
||||
uint8_t gearStatus = gearReport_.report;
|
||||
float steering_tire_angle = steeringReport_.steering_tire_angle;
|
||||
|
||||
float speed = std::sqrt(std::pow(longitudinal_velocity, 2) + std::pow(lateral_velocity, 2));
|
||||
bvc.speed.speedValue = std::lround(speed * 100);
|
||||
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 ((gearReport_ >= 2 && gearReport_ <= 19) || gearReport_ == 21 || gearReport_ == 22)
|
||||
if ((gearStatus >= 2 && gearStatus <= 19) || gearStatus == 21 || gearStatus == 22)
|
||||
bvc.driveDirection = DriveDirection_forward;
|
||||
else if (gearReport_ == 20 || gearReport_ == 21)
|
||||
else if (gearStatus == 20 || gearStatus == 21)
|
||||
bvc.driveDirection = DriveDirection_backward;
|
||||
else
|
||||
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 vehicleLength = std::lround(vehicleDimensions_.front_overhang + vehicleDimensions_.wheel_base + vehicleDimensions_.rear_overhang * 100);
|
||||
bvc.vehicleLength.vehicleLengthValue = vehicleLength >= 0 && vehicleLength <= 1022 ? vehicleLength : VehicleLengthValue_unavailable;
|
||||
|
||||
float vehicleWidth = vehicleDimensions_.left_overhang + vehicleDimensions_.wheel_tread + vehicleDimensions_.right_overhang;
|
||||
if (vehicleWidth <= 0 || vehicleWidth >= 62)
|
||||
bvc.vehicleWidth = VehicleWidth_unavailable;
|
||||
else
|
||||
bvc.vehicleWidth = vehicleWidth;
|
||||
float vehicleWidth = std::lround(vehicleDimensions_.left_overhang + vehicleDimensions_.wheel_tread + vehicleDimensions_.right_overhang * 100);
|
||||
bvc.vehicleWidth = vehicleWidth >= 0 && vehicleWidth <= 61 ? vehicleWidth : VehicleWidth_unavailable;
|
||||
|
||||
//if (longitudinal_acceleration > 160 || longitudinal_acceleration < -160) {
|
||||
// RCLCPP_WARN(node_->get_logger(), "[CamApplication::send] Longitudinal acceleration out of bounds: %d", longitudinal_acceleration);
|
||||
// bvc.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
|
||||
//} else {
|
||||
// bvc.longitudinalAcceleration.longitudinalAccelerationValue = longitudinal_acceleration;
|
||||
//}
|
||||
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)
|
||||
: std::numeric_limits<long>::infinity();
|
||||
bvc.curvature.curvatureValue = curvature >= -1023 && curvature <= 1022 ? curvature : CurvatureValue_unavailable;
|
||||
|
||||
if (longitudinal_velocity != 0)
|
||||
bvc.curvature.curvatureValue = std::lround(lateral_velocity / std::pow(longitudinal_velocity, 2) * 100);
|
||||
else
|
||||
bvc.curvature.curvatureValue = std::numeric_limits<long>::infinity();
|
||||
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
|
||||
basic_container.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable;
|
||||
// ------------------------------
|
||||
bvc.heading.headingConfidence = HeadingConfidence_unavailable;
|
||||
bvc.speed.speedConfidence = SpeedConfidence_unavailable;
|
||||
bvc.vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable;
|
||||
|
|
@ -271,7 +253,6 @@ namespace v2x
|
|||
// ------------------------------
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM");
|
||||
//insertCamToCamTable(message, (char *) "cam_sent");
|
||||
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
|
||||
payload->layer(OsiLayer::Application) = std::move(message);
|
||||
|
||||
|
|
|
|||
|
|
@ -58,7 +58,7 @@ namespace v2x
|
|||
|
||||
void V2XApp::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
|
||||
if (!velocity_report_received_) {
|
||||
RCLCPP_WARN(node_->get_logger(), "[V2XApp::velocityReportCallback VelocityReport not received yet");
|
||||
RCLCPP_WARN(node_->get_logger(), "[V2XApp::velocityReportCallback] VelocityReport not received yet");
|
||||
}
|
||||
if (velocity_report_received_ && cam_started_) {
|
||||
cam->updateVelocityReport(msg);
|
||||
|
|
@ -67,13 +67,22 @@ namespace v2x
|
|||
|
||||
void V2XApp::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
|
||||
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_) {
|
||||
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) {
|
||||
|
||||
tf_received_ = true;
|
||||
|
|
|
|||
|
|
@ -39,13 +39,14 @@ namespace v2x
|
|||
// 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));
|
||||
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_sender_pub_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/sender", rclcpp::QoS{10});
|
||||
|
||||
// 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);
|
||||
|
||||
// Launch V2XApp in a new thread
|
||||
|
|
@ -180,6 +181,10 @@ namespace v2x
|
|||
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) {
|
||||
app->getVehicleDimensions(msg->dimensions);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue