Change topic subscriptions to use api

Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>

include/autoware_v2x/cam_application.hpp

@@ -6,9 +6,8 @@
 #include <boost/asio/io_service.hpp>
 #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_adapi_v1_msgs/msg/vehicle_status.hpp"
 #include "autoware_v2x/positioning.hpp"
 #include <vanetza/asn1/cam.hpp>
 
@@ -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 updateVehicleStatus(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr);
-  void updateSteeringReport(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr);
   void send();
 
 private:
@@ -126,17 +124,11 @@ private:
   };
   VelocityReport velocityReport_;
 
-  struct GearReport {
+  struct VehicleStatus {
-    rclcpp::Time stamp;
+    uint8_t gear;
-    uint8_t report;
-  };
-  GearReport gearReport_;
-
-  struct SteeringReport {
-    rclcpp::Time stamp;
     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_vehicle_status_;
-  bool updating_steering_report_;
   bool sending_;
   bool is_sender_;
   bool reflect_packet_;

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 <boost/asio/io_service.hpp>
 #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 vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr);
-    void steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::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_;
+    bool vehicle_status_received_;
-    int gear_report_interval_;
+    int vehicle_status_interval_;
-    bool steering_report_received_;
-    int  steering_report_interval_;
     bool vehicle_dimensions_set_;
     bool cp_started_;
     bool cam_started_;

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_adapi_v1_msgs/msg/vehicle_status.hpp"
-#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 #include <boost/asio/io_service.hpp>
 #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 vehicleStatucCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg);
-    void steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg);
     void getVehicleDimensions();
     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_adapi_v1_msgs::msg::VehicleStatus>::SharedPtr vehicle_status_sub_;
-    rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>::SharedPtr steering_report_sub_;
     rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_sub_;
     rclcpp::Client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>::SharedPtr get_vehicle_dimensions_;
     rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cpm_objects_pub_;

src/cam_application.cpp

@@ -40,12 +40,10 @@ namespace v2x
     ego_(),
     positionConfidenceEllipse_(),
     velocityReport_(),
-    gearReport_(),
+    vehicleStatus_(),
-    steeringReport_(),
     generationTime_(0),
     updating_velocity_report_(false),
-    updating_gear_report_(false),
+    updating_vehicle_status_(false),
-    updating_steering_report_(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) {
+  void CamApplication::updateVehicleStatus(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
-    if (updating_gear_report_) {
+    if (updating_vehicle_status_) {
       return;
     }
 
-    updating_gear_report_ = true;
+    updating_vehicle_status_ = true;
 
-    gearReport_.stamp = msg->stamp;
+    vehicleStatus_.gear = msg->gear.status;
-    gearReport_.report = msg->report;
+    vehicleStatus_.steering_tire_angle = msg->steering_tire_angle;
 
-    updating_gear_report_ = false;
+    updating_vehicle_status_ = 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;
   }
 
   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;
+    uint8_t gearStatus = vehicleStatus_.gear;
-    float steering_tire_angle = steeringReport_.steering_tire_angle;
+    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;
 

src/v2x_app.cpp

@@ -66,21 +66,12 @@ namespace v2x
     }
   }
 
-  void V2XApp::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
+  void V2XApp::vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
-    if (!gear_report_received_) {
+    if (!vehicle_status_received_) {
-      RCLCPP_WARN(node_->get_logger(), "[V2XApp::gearReportCallback] GearReport not received yet");
+      RCLCPP_WARN(node_->get_logger(), "[V2XApp::vehicleStatusCallback] VehicleStatus not received yet");
     }
-    if (gear_report_received_ && cam_started_) {
+    if (vehicle_status_received_ && cam_started_) {
-      cam->updateGearReport(msg);
+      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);
     }
   }
 

src/v2x_node.cpp

@@ -41,8 +41,7 @@ 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));
+    vehicle_status_sub_ = this->create_subscription<autoware_adapi_v1_msgs::msg::VehicleStatus>("/api/vehicle/status", 10, std::bind(&V2XNode::vehicleStatucCallback, 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_client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>("/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) {
+  void V2XNode::vehicleStatucCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
-    app->gearReportCallback(msg);
+    app->vehicleStatusCallback(msg);
-  }
-
-  void V2XNode::steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
-    app->steeringReportCallback(msg);
   }
 
   void V2XNode::getVehicleDimensions() {