hotfix

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

include/autoware_v2x/cam_application.hpp

@@ -6,8 +6,9 @@
 #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>
 
@@ -27,7 +28,8 @@ public:
   void updateHeading(double *);
   void setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &);
   void updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr);
-  void updateVehicleStatus(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr);
+  void updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr);
+  void updateSteeringReport(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr);
   void send();
 
 private:

include/autoware_v2x/v2x_app.hpp

@@ -5,9 +5,10 @@
 #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"
@@ -31,7 +32,8 @@ 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 vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::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;
@@ -45,10 +47,6 @@ namespace v2x
     V2XNode* node_;
     bool tf_received_;
     int tf_interval_;
-    bool velocity_report_received_;
-    int velocity_report_interval_;
-    bool vehicle_status_received_;
-    int vehicle_status_interval_;
     bool vehicle_dimensions_set_;
     bool cp_started_;
     bool cam_started_;

include/autoware_v2x/v2x_node.hpp

@@ -5,7 +5,8 @@
 #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_adapi_v1_msgs/msg/vehicle_status.hpp"
+#include "autoware_auto_vehicle_msgs/msg/gear_report.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"
@@ -37,13 +38,15 @@ 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 vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::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 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_adapi_v1_msgs::msg::VehicleStatus>::SharedPtr vehicle_status_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<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

@@ -46,8 +46,6 @@ namespace v2x
     vehicleStatus_(),
     generationDeltaTime_(0),
     objectConfidenceThreshold_(0.0),
-    updating_velocity_report_(false),
-    updating_vehicle_status_(false),
     sending_(false),
     is_sender_(is_sender),
     reflect_packet_(false),
@@ -181,12 +179,6 @@ namespace v2x
   }
 
   void CamApplication::updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
-    if (updating_velocity_report_) {
-      return;
-    }
-
-    updating_velocity_report_ = true;
-
     rclcpp::Time msg_stamp(msg->header.stamp.sec, msg->header.stamp.nanosec);
     double dt = msg_stamp.seconds() - velocityReport_.stamp.seconds();
     if (dt == 0) {
@@ -200,21 +192,15 @@ namespace v2x
     velocityReport_.lateral_velocity = msg->lateral_velocity;
     velocityReport_.longitudinal_velocity = msg->longitudinal_velocity;
     velocityReport_.longitudinal_acceleration = longitudinal_acceleration;
-
-    updating_velocity_report_ = false;
   }
 
-  void CamApplication::updateVehicleStatus(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
+  void CamApplication::updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
-    if (updating_vehicle_status_) {
+    rclcpp::Time msg_stamp(msg->stamp.sec, msg->stamp.nanosec);
-      return;
+    vehicleStatus_.gear = msg->report;
-    }
+  }
 
-    updating_vehicle_status_ = true;
+  void CamApplication::updateSteeringReport(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
-
-    vehicleStatus_.gear = msg->gear.status;
     vehicleStatus_.steering_tire_angle = msg->steering_tire_angle;
-
-    updating_vehicle_status_ = false;
   }
 
   void CamApplication::send() {
@@ -315,9 +301,9 @@ namespace v2x
     if (0 <= speed && speed <= 16382) bvc.speed.speedValue = speed;
     else bvc.speed.speedValue = SpeedValue_unavailable;
 
-    if (gearStatus == 2 || gearStatus == 5)
+    if ((gearStatus >= 2 && gearStatus <= 19) || (gearStatus == 23 || gearStatus == 24))
       bvc.driveDirection = DriveDirection_forward;
-    else if (gearStatus == 3)
+    else if (gearStatus == 20 || gearStatus == 21)
       bvc.driveDirection = DriveDirection_backward;
     else
       bvc.driveDirection = DriveDirection_unavailable;

src/v2x_app.cpp

@@ -59,23 +59,21 @@ namespace v2x
   }
 
   void V2XApp::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
-    if (!velocity_report_received_) {
+    if (cam_started_)
-      RCLCPP_WARN(node_->get_logger(), "[V2XApp::velocityReportCallback] VelocityReport not received yet");
-    }
-    if (velocity_report_received_ && cam_started_) {
       cam->updateVelocityReport(msg);
-    }
   }
 
-  void V2XApp::vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
+  void V2XApp::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
-    if (!vehicle_status_received_) {
+    if (cam_started_)
-      RCLCPP_WARN(node_->get_logger(), "[V2XApp::vehicleStatusCallback] VehicleStatus not received yet");
+      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 (cam_started_)
+      cam->updateSteeringReport(msg);
+  }
+
+
   void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
 
     tf_received_ = true;
@@ -192,6 +190,8 @@ namespace v2x
     }
     auto security = create_security_entity(security_options, trigger.runtime(), *positioning);
 
+    RCLCPP_INFO(node_->get_logger(), "Security layer: %s", entity == "certs" ? "Certificates" : "None");
+
     RouterContext context(mib, trigger, *positioning, security.get());
 
     context.set_link_layer(link_layer.get());

src/v2x_node.cpp

@@ -36,19 +36,22 @@ namespace v2x
   V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) {
     using std::placeholders::_1;
 
+    int timeout = 60;
+    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(timeout))) {
+      RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is now available.");
+      this->getVehicleDimensions();
+    } else {
+      RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is not available after waiting (timeout=%ds).", timeout);
+    }
+
     objects_sub_ = this->create_subscription<autoware_auto_perception_msgs::msg::PredictedObjects>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
     tf_sub_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));
 
     // 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));
-    vehicle_status_sub_ = this->create_subscription<autoware_adapi_v1_msgs::msg::VehicleStatus>("/api/vehicle/status", 10, std::bind(&V2XNode::vehicleStatusCallback, 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));
-    get_vehicle_dimensions_ = this->create_client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>("/api/vehicle/dimensions");
+    steering_report_sub_ = this->create_subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>("/vehicle/status/steering_status", 10, std::bind(&V2XNode::steeringReportCallback, this, _1));
-    if (get_vehicle_dimensions_->wait_for_service(std::chrono::seconds(60))) {
-      RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is now available.");
-      this->getVehicleDimensions();
-    } else {
-      RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is not available after waiting (timeout=60s).");
-    }
 
     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});
@@ -195,8 +198,12 @@ namespace v2x
     app->velocityReportCallback(msg);
   }
 
-  void V2XNode::vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
+  void V2XNode::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
-    app->vehicleStatusCallback(msg);
+    app->gearReportCallback(msg);
+  }
+
+  void V2XNode::steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
+    app->steeringReportCallback(msg);
   }
 
   void V2XNode::getVehicleDimensions() {
@@ -209,9 +216,11 @@ namespace v2x
        RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received response from /api/vehicle/dimensions");
        try {
          auto dimensions = response->dimensions;
-         if (dimensions.height == 0 || dimensions.wheel_base == 0 || dimensions.wheel_tread == 0)
+         if (dimensions.height == 0 || dimensions.wheel_base == 0 || dimensions.wheel_tread == 0) {
+           RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received vehicle dimensions are zero. Trying again...");
            this->getVehicleDimensions();
-         else {
+         } else {
+           RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received vehicle dimensions: height=%f, wheel_base=%f, wheel_tread=%f", dimensions.height, dimensions.wheel_base, dimensions.wheel_tread);
            app->setVehicleDimensions(dimensions);
          }
        } catch (const std::exception &e) {