From 26ad12c9e073917eba80de070f6a4859c1a25996 Mon Sep 17 00:00:00 2001 From: Tiago Garcia Date: Tue, 9 Jul 2024 14:54:53 +0100 Subject: [PATCH] Change topic subscriptions to use api Signed-off-by: Tiago Garcia --- include/autoware_v2x/cam_application.hpp | 21 ++++-------- include/autoware_v2x/v2x_app.hpp | 12 +++---- include/autoware_v2x/v2x_node.hpp | 9 ++---- src/cam_application.cpp | 41 +++++++----------------- src/v2x_app.cpp | 19 +++-------- src/v2x_node.cpp | 11 ++----- 6 files changed, 33 insertions(+), 80 deletions(-) diff --git a/include/autoware_v2x/cam_application.hpp b/include/autoware_v2x/cam_application.hpp index d23c55f..bf9f39a 100644 --- a/include/autoware_v2x/cam_application.hpp +++ b/include/autoware_v2x/cam_application.hpp @@ -6,9 +6,8 @@ #include #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_adapi_v1_msgs/msg/vehicle_status.hpp" #include "autoware_v2x/positioning.hpp" #include @@ -28,8 +27,7 @@ public: void updateHeading(double *); void setVehicleDimensions(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 updateVehicleStatus(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr); void send(); private: @@ -126,17 +124,11 @@ private: }; VelocityReport velocityReport_; - struct GearReport { - rclcpp::Time stamp; - uint8_t report; - }; - GearReport gearReport_; - - struct SteeringReport { - rclcpp::Time stamp; + struct VehicleStatus { + uint8_t gear; float steering_tire_angle; }; - SteeringReport steeringReport_; + VehicleStatus vehicleStatus_; int generationTime_; long gdt_timestamp_; @@ -144,8 +136,7 @@ private: double objectConfidenceThreshold_; bool updating_velocity_report_; - bool updating_gear_report_; - bool updating_steering_report_; + bool updating_vehicle_status_; 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 5e5e4f1..6ec41b0 100644 --- a/include/autoware_v2x/v2x_app.hpp +++ b/include/autoware_v2x/v2x_app.hpp @@ -5,10 +5,9 @@ #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_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 "autoware_adapi_v1_msgs/msg/vehicle_status.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include #include "autoware_v2x/cpm_application.hpp" @@ -32,8 +31,7 @@ namespace v2x void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr); void setVehicleDimensions(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 vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr); void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr); CpmApplication *cp; @@ -49,10 +47,8 @@ namespace v2x int tf_interval_; bool velocity_report_received_; int velocity_report_interval_; - bool gear_report_received_; - int gear_report_interval_; - bool steering_report_received_; - int steering_report_interval_; + bool vehicle_status_received_; + int vehicle_status_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 d4ee1ae..94ae5ed 100644 --- a/include/autoware_v2x/v2x_node.hpp +++ b/include/autoware_v2x/v2x_node.hpp @@ -5,8 +5,7 @@ #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_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_adapi_v1_msgs/msg/vehicle_status.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include #include "autoware_v2x/v2x_app.hpp" @@ -35,15 +34,13 @@ 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 steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg); + void vehicleStatucCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg); void getVehicleDimensions(); 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 vehicle_status_sub_; rclcpp::Subscription::SharedPtr tf_sub_; rclcpp::Client::SharedPtr get_vehicle_dimensions_; rclcpp::Publisher::SharedPtr cpm_objects_pub_; diff --git a/src/cam_application.cpp b/src/cam_application.cpp index 53a04b7..015efe0 100644 --- a/src/cam_application.cpp +++ b/src/cam_application.cpp @@ -40,12 +40,10 @@ namespace v2x ego_(), positionConfidenceEllipse_(), velocityReport_(), - gearReport_(), - steeringReport_(), + vehicleStatus_(), generationTime_(0), updating_velocity_report_(false), - updating_gear_report_(false), - updating_steering_report_(false), + updating_vehicle_status_(false), sending_(false), is_sender_(is_sender), reflect_packet_(false), @@ -143,30 +141,17 @@ namespace v2x updating_velocity_report_ = false; } - void CamApplication::updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) { - if (updating_gear_report_) { + void CamApplication::updateVehicleStatus(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) { + if (updating_vehicle_status_) { return; } - updating_gear_report_ = true; + updating_vehicle_status_ = true; - gearReport_.stamp = msg->stamp; - gearReport_.report = msg->report; + vehicleStatus_.gear = msg->gear.status; + vehicleStatus_.steering_tire_angle = msg->steering_tire_angle; - updating_gear_report_ = false; - } - - void CamApplication::updateSteeringReport(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) { - if (updating_steering_report_) { - return; - } - - updating_steering_report_ = true; - - steeringReport_.stamp = msg->stamp; - steeringReport_.steering_tire_angle = msg->steering_tire_angle; - - updating_steering_report_ = false; + updating_vehicle_status_ = false; } void CamApplication::send() { @@ -262,27 +247,25 @@ namespace v2x 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; + uint8_t gearStatus = vehicleStatus_.gear; + float steering_tire_angle = vehicleStatus_.steering_tire_angle; long speed = std::lround(std::sqrt(std::pow(longitudinal_velocity, 2) + std::pow(lateral_velocity, 2)) * 100); if (0 <= speed && speed <= 16382) bvc.speed.speedValue = speed; else bvc.speed.speedValue = SpeedValue_unavailable; - if ((gearStatus >= 2 && gearStatus <= 19) || gearStatus == 23 || gearStatus == 24) + if (gearStatus == 2 || gearStatus == 5) bvc.driveDirection = DriveDirection_forward; - else if (gearStatus == 20 || gearStatus == 21) + else if (gearStatus == 3) bvc.driveDirection = DriveDirection_backward; else bvc.driveDirection = DriveDirection_unavailable; long vehicleLength = std::lround((vehicleDimensions_.front_overhang + vehicleDimensions_.wheel_base + vehicleDimensions_.rear_overhang) * 10); - RCLCPP_INFO(node_->get_logger(), "LENGTH: front_overhang: %f, wheel_base: %f, rear_overhang: %f, total: %ld", vehicleDimensions_.front_overhang, vehicleDimensions_.wheel_base, vehicleDimensions_.rear_overhang, vehicleLength); if (1 <= vehicleLength && vehicleLength <= 1022) bvc.vehicleLength.vehicleLengthValue = vehicleLength; else bvc.vehicleLength.vehicleLengthValue = VehicleLengthValue_unavailable; long vehicleWidth = std::lround((vehicleDimensions_.left_overhang + vehicleDimensions_.wheel_tread + vehicleDimensions_.right_overhang) * 10); - RCLCPP_INFO(node_->get_logger(), "WIDTH: left_overhang: %f, wheel_tread: %f, right_overhang: %f, total: %ld", vehicleDimensions_.left_overhang, vehicleDimensions_.wheel_tread, vehicleDimensions_.right_overhang, vehicleWidth); if (1 <= vehicleWidth && vehicleWidth <= 61) bvc.vehicleWidth = vehicleWidth; else bvc.vehicleWidth = VehicleWidth_unavailable; diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index b5513e7..90cddf0 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -66,24 +66,15 @@ 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"); + void V2XApp::vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) { + if (!vehicle_status_received_) { + RCLCPP_WARN(node_->get_logger(), "[V2XApp::vehicleStatusCallback] VehicleStatus not received yet"); } - if (gear_report_received_ && cam_started_) { - cam->updateGearReport(msg); + if (vehicle_status_received_ && cam_started_) { + cam->updateVehicleStatus(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 7f1af48..420a625 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -41,8 +41,7 @@ namespace v2x // 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)); + vehicle_status_sub_ = this->create_subscription("/api/vehicle/status", 10, std::bind(&V2XNode::vehicleStatucCallback, this, _1)); get_vehicle_dimensions_ = this->create_client("/api/vehicle/dimensions"); if (get_vehicle_dimensions_->wait_for_service(std::chrono::seconds(60))) { RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is now available."); @@ -186,12 +185,8 @@ namespace v2x 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::vehicleStatucCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) { + app->vehicleStatusCallback(msg); } void V2XNode::getVehicleDimensions() {