From a2a54ba37fbf4a43dc7f254f9c1267081418931f Mon Sep 17 00:00:00 2001 From: Tiago Garcia Date: Wed, 12 Jun 2024 23:51:59 +0100 Subject: [PATCH] Add CAMApplication prototype - Mandatory parameters filled through the use of 3 ros topics - Sending ITS packets with CAMs Signed-off-by: Tiago Garcia --- CMakeLists.txt | 1 + config/autoware_v2x.param.yaml | 2 +- include/autoware_v2x/cam_application.hpp | 148 +++++++++++ include/autoware_v2x/v2x_app.hpp | 15 ++ include/autoware_v2x/v2x_node.hpp | 13 + launch/v2x.launch.xml | 2 +- package.xml | 2 + src/cam_application.cpp | 299 +++++++++++++++++++++++ src/v2x_app.cpp | 58 ++++- src/v2x_node.cpp | 21 +- 10 files changed, 548 insertions(+), 13 deletions(-) create mode 100644 include/autoware_v2x/cam_application.hpp create mode 100644 src/cam_application.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 646cb6b..89987ad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,6 +35,7 @@ ament_auto_add_library(autoware_v2x SHARED src/v2x_app.cpp src/application.cpp src/cpm_application.cpp + src/cam_application.cpp src/ethernet_device.cpp src/link_layer.cpp src/raw_socket_link.cpp diff --git a/config/autoware_v2x.param.yaml b/config/autoware_v2x.param.yaml index 5b1bce5..c236935 100644 --- a/config/autoware_v2x.param.yaml +++ b/config/autoware_v2x.param.yaml @@ -1,4 +1,4 @@ /**: ros__parameters: - network_interface: "wlp4s0" + network_interface: "enx5414a7c7d3af" is_sender: true \ No newline at end of file diff --git a/include/autoware_v2x/cam_application.hpp b/include/autoware_v2x/cam_application.hpp new file mode 100644 index 0000000..4f3aca6 --- /dev/null +++ b/include/autoware_v2x/cam_application.hpp @@ -0,0 +1,148 @@ +#ifndef CAM_APPLICATION_HPP_EUIC2VFR +#define CAM_APPLICATION_HPP_EUIC2VFR + +#include "autoware_v2x/application.hpp" +#include "rclcpp/rclcpp.hpp" +#include +#include +#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" +#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp" +#include "autoware_v2x/positioning.hpp" +#include + +namespace v2x +{ +class V2XNode; +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 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); + + V2XNode *node_; + vanetza::Runtime &runtime_; + vanetza::Clock::duration cam_interval_; + + struct VehicleDimensions { + 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_; + + struct Ego_station { + double mgrs_x; + double mgrs_y; + double latitude; + double longitude; + double altitude; + double heading; + }; + Ego_station ego_; + + struct VelocityReport { + rclcpp::Time stamp; + float heading_rate; + float lateral_velocity; + float longitudinal_velocity; + float longitudinal_acceleration; + }; + VelocityReport velocityReport_; + + uint8_t gearReport_; + + int generationTime_; + long gdt_timestamp_; + + double objectConfidenceThreshold_; + + bool updating_velocity_report_; + bool updating_gear_report_; + bool sending_; + bool is_sender_; + bool reflect_packet_; + bool include_all_persons_and_animals_; + + int cam_num_; + int received_cam_num_; + + bool use_dynamic_generation_rules_; +}; +} + +#endif /* CAM_APPLICATION_HPP_EUIC2VFR */ diff --git a/include/autoware_v2x/v2x_app.hpp b/include/autoware_v2x/v2x_app.hpp index bdacc50..f1e3691 100644 --- a/include/autoware_v2x/v2x_app.hpp +++ b/include/autoware_v2x/v2x_app.hpp @@ -4,9 +4,13 @@ #include "rclcpp/rclcpp.hpp" #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_adapi_v1_msgs/msg/vehicle_dimensions.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include #include "autoware_v2x/cpm_application.hpp" +#include "autoware_v2x/cam_application.hpp" #include "autoware_v2x/time_trigger.hpp" #include "autoware_v2x/link_layer.hpp" #include "autoware_v2x/ethernet_device.hpp" @@ -24,18 +28,29 @@ namespace v2x V2XApp(V2XNode *); void start(); void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr); + 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 tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr); CpmApplication *cp; + CamApplication *cam; // V2XNode *v2x_node; private: friend class CpmApplication; + friend class CamApplication; friend class Application; V2XNode* node_; bool tf_received_; int tf_interval_; + bool velocity_report_received_; + int velocity_report_interval_; + bool gear_report_received_; + int gear_report_interval_; + bool vehicle_dimensions_set_; bool cp_started_; + bool cam_started_; }; } diff --git a/include/autoware_v2x/v2x_node.hpp b/include/autoware_v2x/v2x_node.hpp index 0ef79d9..be13d92 100644 --- a/include/autoware_v2x/v2x_node.hpp +++ b/include/autoware_v2x/v2x_node.hpp @@ -4,10 +4,15 @@ #include "rclcpp/rclcpp.hpp" #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_adapi_v1_msgs/srv/get_vehicle_dimensions.hpp" +#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include #include "autoware_v2x/v2x_app.hpp" #include "autoware_v2x/cpm_application.hpp" +#include "autoware_v2x/cam_application.hpp" #include "autoware_v2x/time_trigger.hpp" #include "autoware_v2x/link_layer.hpp" #include "autoware_v2x/ethernet_device.hpp" @@ -30,12 +35,20 @@ namespace v2x 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 getVehicleDimensionsCallback(const autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response::ConstSharedPtr msg); void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg); rclcpp::Subscription::SharedPtr objects_sub_; + rclcpp::Subscription::SharedPtr velocity_report_sub_; + rclcpp::Subscription::SharedPtr gear_report_sub_; + rclcpp::Subscription::SharedPtr get_vehicle_dimensions_; rclcpp::Subscription::SharedPtr tf_sub_; rclcpp::Publisher::SharedPtr cpm_objects_pub_; rclcpp::Publisher::SharedPtr cpm_sender_pub_; + rclcpp::Publisher::SharedPtr cam_objects_pub_; + rclcpp::Publisher::SharedPtr cam_sender_pub_; double pos_lat_; double pos_lon_; diff --git a/launch/v2x.launch.xml b/launch/v2x.launch.xml index 3e20eb1..11ae4ed 100644 --- a/launch/v2x.launch.xml +++ b/launch/v2x.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/package.xml b/package.xml index 4651ea5..94604d1 100644 --- a/package.xml +++ b/package.xml @@ -10,6 +10,8 @@ ament_cmake_auto autoware_auto_perception_msgs + autoware_auto_vehicle_msgs + autoware_adapi_v1_msgs tf2_msgs tf2 geometry_msgs diff --git a/src/cam_application.cpp b/src/cam_application.cpp new file mode 100644 index 0000000..75b77ae --- /dev/null +++ b/src/cam_application.cpp @@ -0,0 +1,299 @@ +#include "autoware_v2x/cam_application.hpp" +#include "autoware_v2x/positioning.hpp" +#include "autoware_v2x/security.hpp" +#include "autoware_v2x/link_layer.hpp" +#include "autoware_v2x/v2x_node.hpp" + +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Matrix3x3.h" + +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#define _USE_MATH_DEFINES +#include + +using namespace vanetza; +using namespace std::chrono; + +namespace v2x +{ + CamApplication::CamApplication(V2XNode * node, Runtime & rt, bool is_sender) + : node_(node), + runtime_(rt), + vehicleDimensions_(), + ego_(), + velocityReport_(), + gearReport_(), + generationTime_(0), + updating_velocity_report_(false), + updating_gear_report_(false), + sending_(false), + is_sender_(is_sender), + reflect_packet_(false), + objectConfidenceThreshold_(0.0), + include_all_persons_and_animals_(false), + 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(milliseconds(100)); + //createTables(); + } + + void CamApplication::set_interval(Clock::duration interval) { + cam_interval_ = interval; + runtime_.cancel(this); + schedule_timer(); + } + + 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) { + schedule_timer(); + send(); + } + + CamApplication::PortType CamApplication::port() { + 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 + } + + void CamApplication::updateMGRS(double *x, double *y) { + ego_.mgrs_x = *x; + ego_.mgrs_y = *y; + } + + void CamApplication::updateRP(double *lat, double *lon, double *altitude) { + ego_.latitude = *lat; + ego_.longitude = *lon; + ego_.altitude = *altitude; + } + + void CamApplication::updateGenerationTime(int *gdt, long *gdt_timestamp) { + generationTime_ = *gdt; + gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp + } + + void CamApplication::updateHeading(double *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) { + vehicleDimensions_.wheel_radius = msg.wheel_radius; + vehicleDimensions_.wheel_width = msg.wheel_width; + vehicleDimensions_.wheel_base = msg.wheel_base; + vehicleDimensions_.wheel_tread = msg.wheel_tread; + vehicleDimensions_.front_overhang = msg.front_overhang; + vehicleDimensions_.rear_overhang = msg.rear_overhang; + vehicleDimensions_.left_overhang = msg.left_overhang; + vehicleDimensions_.right_overhang = msg.right_overhang; + vehicleDimensions_.height = msg.height; + } + + 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"); + return; + } + + updating_velocity_report_ = true; + + 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"); + return; + } + float longitudinal_acceleration = (msg->longitudinal_velocity - velocityReport_.longitudinal_velocity) / dt; + + velocityReport_.stamp = msg->header.stamp; + velocityReport_.heading_rate = msg->heading_rate; + velocityReport_.lateral_velocity = msg->lateral_velocity; + velocityReport_.longitudinal_velocity = msg->longitudinal_velocity; + velocityReport_.longitudinal_acceleration = longitudinal_acceleration; + } + + 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"); + return; + } + + updating_gear_report_ = true; + + gearReport_ = msg->report; + } + + void CamApplication::send() { + if (!is_sender_) return; + + if (sending_) { + RCLCPP_WARN(node_->get_logger(), "[CamApplication::send] already sending"); + return; + } + + 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; + + CoopAwareness_t &cam = message->cam; + + //asn_long2INTEGER(&cam.generationDeltaTime, (long) gdt_timestamp_); + + 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; + + // 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; + // ------------------------------ + + BasicVehicleContainerHighFrequency_t &bvc = cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; + cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; + + int heading = std::lround(((-ego_.heading * 180.0 / M_PI) + 90.0) * 10.0); + if (heading < 0) heading += 3600; + bvc.heading.headingValue = heading; + + float heading_rate = velocityReport_.heading_rate; + float lateral_velocity = velocityReport_.lateral_velocity; + float longitudinal_velocity = velocityReport_.longitudinal_velocity; + + float speed = std::sqrt(std::pow(longitudinal_velocity, 2) + std::pow(lateral_velocity, 2)); + bvc.speed.speedValue = std::lround(speed * 100); + + if ((gearReport_ >= 2 && gearReport_ <= 19) || gearReport_ == 21 || gearReport_ == 22) + bvc.driveDirection = DriveDirection_forward; + else if (gearReport_ == 20 || gearReport_ == 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 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) { + // 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 = LongitudinalAccelerationValue_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::infinity(); + bvc.curvatureCalculationMode = CurvatureCalculationMode_yawRateNotUsed; + + bvc.yawRate.yawRateValue = std::lround(heading_rate * 100); + + // UNAVAILABLE VALUES FOR TESTING + bvc.heading.headingConfidence = HeadingConfidence_unavailable; + bvc.speed.speedConfidence = SpeedConfidence_unavailable; + bvc.vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; + bvc.longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; + bvc.curvature.curvatureConfidence = CurvatureConfidence_unavailable; + bvc.yawRate.yawRateConfidence = YawRateConfidence_unavailable; + // ------------------------------ + + RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM"); + //insertCamToCamTable(message, (char *) "cam_sent"); + std::unique_ptr payload{new geonet::DownPacket()}; + payload->layer(OsiLayer::Application) = std::move(message); + + Application::DataRequest request; + request.its_aid = aid::CP; + request.transport_type = geonet::TransportType::SHB; + request.communication_profile = geonet::CommunicationProfile::ITS_G5; + + Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); + + if (!confirm.accepted()) { + throw std::runtime_error("[CamApplication::send] CAM application data request failed"); + } + + sending_ = false; + + std::chrono::milliseconds ms = std::chrono::duration_cast ( + std::chrono::system_clock::now().time_since_epoch() + ); + node_->latency_log_file << "T_depart," << cam_num_ << "," << ms.count() << std::endl; + + ++cam_num_; + } + +} \ No newline at end of file diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index 18aa60d..1f4a268 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -6,6 +6,7 @@ #include "autoware_v2x/security.hpp" #include "autoware_v2x/link_layer.hpp" #include "autoware_v2x/cpm_application.hpp" +#include "autoware_v2x/cam_application.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" @@ -14,6 +15,7 @@ #include "tf2/LinearMath/Matrix3x3.h" #include +#include #include #include #include @@ -28,11 +30,13 @@ using namespace std::chrono; namespace v2x { - V2XApp::V2XApp(V2XNode *node) : + V2XApp::V2XApp(V2XNode *node) : node_(node), tf_received_(false), tf_interval_(0), - cp_started_(false) + vehicle_dimensions_set_(false), + cp_started_(false), + cam_started_(false) { } @@ -46,6 +50,30 @@ namespace v2x } } + void V2XApp::getVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions msg) { + if (vehicle_dimensions_set_) return; + cam->getVehicleDimensions(msg); + vehicle_dimensions_set_ = true; + } + + 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"); + } + if (velocity_report_received_ && cam_started_) { + cam->updateVelocityReport(msg); + } + } + + 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"); + } + if (gear_report_received_ && cam_started_) { + cam->updateGearReport(msg); + } + } + void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { tf_received_ = true; @@ -80,20 +108,27 @@ namespace v2x // Reverse projection from UTM to geographic. GeographicLib::UTMUPS::Reverse( - zone, - northp, - grid_num_x * grid_size + x, - grid_num_y * grid_size + y, - lat, + zone, + northp, + grid_num_x * grid_size + x, + grid_num_y * grid_size + y, + lat, lon ); - + if (cp && cp_started_) { cp->updateMGRS(&x, &y); cp->updateRP(&lat, &lon, &z); cp->updateHeading(&yaw); cp->updateGenerationTime(&gdt, ×tamp_msec); } + + if (cam && cp_started_) { + cam->updateMGRS(&x, &y); + cam->updateRP(&lat, &lon, &z); + cam->updateHeading(&yaw); + cam->updateGenerationTime(&gdt, ×tamp_msec); + } } void V2XApp::start() { @@ -132,11 +167,14 @@ namespace v2x bool is_sender; node_->get_parameter("is_sender", is_sender); cp = new CpmApplication(node_, trigger.runtime(), is_sender); - + cam = new CamApplication(node_, trigger.runtime(), is_sender); + context.enable(cp); + context.enable(cam); cp_started_ = true; + cam_started_ = true; io_service.run(); } -} \ No newline at end of file +} diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index f83fa54..eae5eed 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -6,11 +6,13 @@ #include "autoware_v2x/security.hpp" #include "autoware_v2x/link_layer.hpp" #include "autoware_v2x/cpm_application.hpp" +#include "autoware_v2x/cam_application.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" #include +#include #include #include #include @@ -33,12 +35,17 @@ namespace v2x objects_sub_ = this->create_subscription("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); tf_sub_ = this->create_subscription("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1)); + + // Topic subscriptions for CAMApplication + velocity_report_sub_ = this->create_subscription("/vehicle/status/velocity_status", 10, std::bind(&V2XNode::velocityReportCallback, this, _1)); + gear_report_sub_ = this->create_subscription("/vehicle/status/gear_status", 10, std::bind(&V2XNode::gearReportCallback, this, _1)); + get_vehicle_dimensions_ = this->create_subscription("/api/vehicle/dimensions", 10, std::bind(&V2XNode::getVehicleDimensionsCallback, this, _1)); cpm_objects_pub_ = create_publisher("/v2x/cpm/objects", rclcpp::QoS{10}); // cpm_sender_pub_ = create_publisher("/v2x/cpm/sender", rclcpp::QoS{10}); // Declare Parameters - this->declare_parameter("network_interface", "vmnet1"); + this->declare_parameter("network_interface", "enx5414a7c7d3af"); this->declare_parameter("is_sender", true); // Launch V2XApp in a new thread @@ -164,6 +171,18 @@ namespace v2x void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { app->tfCallback(msg); } + + void V2XNode::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) { + app->velocityReportCallback(msg); + } + + void V2XNode::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) { + app->gearReportCallback(msg); + } + + void V2XNode::getVehicleDimensionsCallback(const autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response::ConstSharedPtr msg) { + app->getVehicleDimensions(msg->dimensions); + } } #include "rclcpp_components/register_node_macro.hpp"