From 5510a362dce8b8243a3faebaee39218fcdbc2d35 Mon Sep 17 00:00:00 2001 From: Tiago Garcia Date: Fri, 28 Jun 2024 14:11:04 +0100 Subject: [PATCH] Add formatted values - Missing confidence values Signed-off-by: Tiago Garcia --- config/autoware_v2x.param.yaml | 4 +- include/autoware_v2x/cam_application.hpp | 73 +++------- include/autoware_v2x/v2x_app.hpp | 4 + include/autoware_v2x/v2x_node.hpp | 3 + launch/v2x.launch.xml | 4 +- src/cam_application.cpp | 167 ++++++++++------------- src/v2x_app.cpp | 13 +- src/v2x_node.cpp | 17 ++- 8 files changed, 123 insertions(+), 162 deletions(-) diff --git a/config/autoware_v2x.param.yaml b/config/autoware_v2x.param.yaml index c236935..80a1e0f 100644 --- a/config/autoware_v2x.param.yaml +++ b/config/autoware_v2x.param.yaml @@ -1,4 +1,4 @@ /**: ros__parameters: - network_interface: "enx5414a7c7d3af" - is_sender: true \ No newline at end of file + network_interface: "v2x_testing" + is_sender: true diff --git a/include/autoware_v2x/cam_application.hpp b/include/autoware_v2x/cam_application.hpp index 4f3aca6..fb0c2cd 100644 --- a/include/autoware_v2x/cam_application.hpp +++ b/include/autoware_v2x/cam_application.hpp @@ -7,6 +7,7 @@ #include #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 @@ -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); @@ -123,9 +71,19 @@ private: float longitudinal_acceleration; }; 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_; diff --git a/include/autoware_v2x/v2x_app.hpp b/include/autoware_v2x/v2x_app.hpp index f1e3691..ea2f5cc 100644 --- a/include/autoware_v2x/v2x_app.hpp +++ b/include/autoware_v2x/v2x_app.hpp @@ -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 @@ -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_; diff --git a/include/autoware_v2x/v2x_node.hpp b/include/autoware_v2x/v2x_node.hpp index be13d92..f10e973 100644 --- a/include/autoware_v2x/v2x_node.hpp +++ b/include/autoware_v2x/v2x_node.hpp @@ -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::SharedPtr objects_sub_; rclcpp::Subscription::SharedPtr velocity_report_sub_; rclcpp::Subscription::SharedPtr gear_report_sub_; + rclcpp::Subscription::SharedPtr steering_report_sub_; rclcpp::Subscription::SharedPtr get_vehicle_dimensions_; rclcpp::Subscription::SharedPtr tf_sub_; rclcpp::Publisher::SharedPtr cpm_objects_pub_; diff --git a/launch/v2x.launch.xml b/launch/v2x.launch.xml index 11ae4ed..d15d223 100644 --- a/launch/v2x.launch.xml +++ b/launch/v2x.launch.xml @@ -1,7 +1,7 @@ - + - \ No newline at end of file + diff --git a/src/cam_application.cpp b/src/cam_application.cpp index 75b77ae..4490b5d 100644 --- a/src/cam_application.cpp +++ b/src/cam_application.cpp @@ -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), @@ -71,19 +73,11 @@ namespace v2x 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 @@ -93,13 +87,13 @@ namespace v2x 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 @@ -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; @@ -137,23 +114,23 @@ namespace v2x 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"); + 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"); + 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; @@ -163,49 +140,61 @@ 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() { 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; - + header.stationID = cam_num_; + CoopAwareness_t &cam = message->cam; - //asn_long2INTEGER(&cam.generationDeltaTime, (long) gdt_timestamp_); - + cam.generationDeltaTime = std::chrono::duration_cast(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; @@ -213,55 +202,48 @@ namespace v2x 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); + float longitudinal_acceleration = std::lround(velocityReport_.longitudinal_acceleration * 100); + uint8_t gearStatus = gearReport_.report; + 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; - 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 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(); + 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 = std::lround(vehicleDimensions_.left_overhang + vehicleDimensions_.wheel_tread + vehicleDimensions_.right_overhang * 100); + bvc.vehicleWidth = vehicleWidth >= 0 && vehicleWidth <= 61 ? vehicleWidth : VehicleWidth_unavailable; + + bvc.longitudinalAcceleration.longitudinalAccelerationValue = longitudinal_acceleration >= -160 && longitudinal_acceleration <= 160 ? longitudinal_acceleration : 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::infinity(); + bvc.curvature.curvatureValue = curvature >= -1023 && curvature <= 1022 ? curvature : CurvatureValue_unavailable; + 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; @@ -269,19 +251,18 @@ namespace v2x 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"); } @@ -296,4 +277,4 @@ namespace v2x ++cam_num_; } -} \ No newline at end of file +} diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index 1f4a268..e659192 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -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; diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index eae5eed..2e1b259 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -35,17 +35,18 @@ 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)); + steering_report_sub_ = this->create_subscription("/vehicle/status/steering_status", 10, std::bind(&V2XNode::steeringReportCallback, 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", "enx5414a7c7d3af"); + this->declare_parameter("network_interface", "v2x_testing"); this->declare_parameter("is_sender", true); // Launch V2XApp in a new thread @@ -171,19 +172,23 @@ 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::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); } } #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(v2x::V2XNode) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(v2x::V2XNode)