bug fixes

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

include/autoware_v2x/cam_application.hpp

@@ -24,7 +24,6 @@ public:
   void set_interval(vanetza::Clock::duration);
   void updateMGRS(double *, double *);
   void updateRP(double *, double *, double *);
-  void updateGenerationDeltaTime(int *, long *);
   void updateHeading(double *);
   void setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &);
   void updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr);
@@ -41,15 +40,15 @@ private:
   vanetza::Clock::duration cam_interval_{};
 
   struct VehicleDimensions {
-    float wheel_radius;
+    float wheel_radius = 0.0;
-    float wheel_width;
+    float wheel_width = 0.0;
-    float wheel_base;
+    float wheel_base = 0.0;
-    float wheel_tread;
+    float wheel_tread = 0.0;
-    float front_overhang;
+    float front_overhang = 0.0;
-    float rear_overhang;
+    float rear_overhang = 0.0;
-    float left_overhang;
+    float left_overhang = 0.0;
-    float right_overhang;
+    float right_overhang = 0.0;
-    float height;
+    float height = 0.0;
   };
   VehicleDimensions vehicleDimensions_;
 
@@ -132,9 +131,6 @@ private:
   };
   VehicleStatus vehicleStatus_;
 
-  int generationDeltaTime_;
-  long gdt_timestamp_{};
-
   double objectConfidenceThreshold_;
 
   bool updating_velocity_report_;

include/autoware_v2x/v2x_app.hpp

@@ -7,8 +7,6 @@
 #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"
 #include <boost/asio/io_service.hpp>
 #include "autoware_v2x/cpm_application.hpp"
@@ -30,7 +28,6 @@ namespace v2x
     V2XApp(V2XNode *);
     void start();
     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);

include/autoware_v2x/v2x_node.hpp

@@ -7,6 +7,8 @@
 #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"
 #include <boost/asio/io_service.hpp>
 #include "autoware_v2x/v2x_app.hpp"
@@ -32,6 +34,7 @@ namespace v2x
     void publishObjects(std::vector<CpmApplication::Object> *, int cpm_num);
     void publishCpmSenderObject(double, double, double);
     void publishReceivedCam(etsi_its_cam_ts_msgs::msg::CAM &);
+    void getVehicleDimensions();
 
     std::ofstream latency_log_file;
 
@@ -40,7 +43,6 @@ namespace v2x
     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 getVehicleDimensions();
     void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg);
 
     rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr objects_sub_;
@@ -55,6 +57,8 @@ namespace v2x
 
     double pos_lat_;
     double pos_lon_;
+
+    bool vehicle_dimensions_available_;
   };
 }
 

src/cam_application.cpp

@@ -44,7 +44,6 @@ namespace v2x
     positionConfidenceEllipse_(),
     velocityReport_(),
     vehicleStatus_(),
-    generationDeltaTime_(0),
     objectConfidenceThreshold_(0.0),
     sending_(false),
     is_sender_(is_sender),
@@ -62,6 +61,8 @@ namespace v2x
     std::mt19937 gen(rd());
     std::uniform_int_distribution<unsigned long> dis(0, 4294967295);
     stationId_ = dis(gen);
+
+    node_->getVehicleDimensions();
   }
 
   void CamApplication::set_interval(Clock::duration interval) {
@@ -157,16 +158,16 @@ namespace v2x
     positionConfidenceEllipse_.y.insert(*lon);
   }
 
-  void CamApplication::updateGenerationDeltaTime(int *gdt, long *gdt_timestamp) {
-    generationDeltaTime_ = *gdt;
-    gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
-  }
-
   void CamApplication::updateHeading(double *yaw) {
     ego_.heading = *yaw;
   }
 
   void CamApplication::setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &msg) {
+    if (msg == autoware_adapi_v1_msgs::msg::VehicleDimensions{}) {
+      RCLCPP_WARN(node_->get_logger(), "[CamApplication::getVehicleDimensions] Vehicle dimensions not available");
+      return;
+    }
+
     vehicleDimensions_.wheel_radius = msg.wheel_radius;
     vehicleDimensions_.wheel_width = msg.wheel_width;
     vehicleDimensions_.wheel_base = msg.wheel_base;
@@ -213,6 +214,8 @@ namespace v2x
 
     sending_ = true;
 
+    std::chrono::milliseconds now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
+
     vanetza::asn1::Cam message;
 
     ItsPduHeader_t &header = message->header;
@@ -222,7 +225,9 @@ namespace v2x
 
     CoopAwareness_t &cam = message->cam;
 
-    cam.generationDeltaTime = generationDeltaTime_;
+    // Convert Unix timestamp to ETSI epoch (2004-01-01 00:00:00)
+    cam.generationDeltaTime = (now_ms.count() - 1072915200000) % 65536;
+    RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM with generationDeltaTime %ld", cam.generationDeltaTime);
 
     BasicContainer_t &basic_container = cam.camParameters.basicContainer;
     basic_container.stationType = StationType_passengerCar;

src/v2x_app.cpp

@@ -51,13 +51,6 @@ namespace v2x
     }
   }
 
-  void V2XApp::setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &msg) {
-    if (cam_started_)
-      cam->setVehicleDimensions(msg);
-    else
-      this->setVehicleDimensions(msg);
-  }
-
   void V2XApp::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
     if (cam_started_)
       cam->updateVelocityReport(msg);
@@ -127,7 +120,6 @@ namespace v2x
       cam->updateMGRS(&x, &y);
       cam->updateRP(&lat, &lon, &z);
       cam->updateHeading(&yaw);
-      cam->updateGenerationDeltaTime(&gdt, &timestamp_msec);
     }
   }
 

src/v2x_node.cpp

@@ -39,10 +39,11 @@ namespace v2x
     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.");
+      RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] Service /api/vehicle/dimensions is now available.");
-      this->getVehicleDimensions();
+      vehicle_dimensions_available_ = true;
     } else {
-      RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is not available after waiting (timeout=%ds).", timeout);
+      RCLCPP_ERROR(get_logger(), "[V2XNode::V2XNode] Service /api/vehicle/dimensions is not available after waiting (timeout=%ds).", timeout);
+      vehicle_dimensions_available_ = false;
     }
 
     objects_sub_ = this->create_subscription<autoware_auto_perception_msgs::msg::PredictedObjects>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
@@ -207,28 +208,25 @@ namespace v2x
   }
 
   void V2XNode::getVehicleDimensions() {
+    if (!vehicle_dimensions_available_) {
+      RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is not available.");
+      return;
+    }
     RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Sending service request to /api/vehicle/dimensions");
     auto request = std::make_shared<autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Request>();
     auto future_result = get_vehicle_dimensions_->async_send_request(request, [this](rclcpp::Client<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>::SharedFuture future) {
-     try
+      try {
-     {
         auto response = future.get();
         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) {
-           RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received vehicle dimensions are zero. Trying again...");
-           this->getVehicleDimensions();
-         } 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);
+          app->cam->setVehicleDimensions(dimensions);
-         }
         } catch (const std::exception &e) {
           RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service response of /api/vehicle/dimensions failed: %s", e.what());
         }
       }
-     catch (const std::exception &e)
+      catch (const std::exception &e) {
-     {
         RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service call of /api/vehicle/dimensions failed: %s", e.what());
       }
     });