Update topic /api/vehicle/dimensions to be correctly treated as a service

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

include/autoware_v2x/cam_application.hpp

@@ -22,14 +22,14 @@ public:
   PortType port() override;
   void indicate(const DataIndication &, UpPacketPtr) override;
   void set_interval(vanetza::Clock::duration);
-  bool 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 updateMGRS(double *, double *);
   void updateRP(double *, double *, double *);
   void updateGenerationTime(int *, long *);
   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 send();
 
 private:

include/autoware_v2x/v2x_app.hpp

@@ -7,6 +7,7 @@
 #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>
@@ -29,7 +30,7 @@ namespace v2x
     V2XApp(V2XNode *);
     void start();
     void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr);
-    void getVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions);
+    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);
@@ -58,4 +59,4 @@ namespace v2x
   };
 }
 
-#endif
+#endif

include/autoware_v2x/v2x_node.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/v2x_app.hpp"
@@ -31,7 +29,7 @@ namespace v2x
     V2XApp *app;
     void publishObjects(std::vector<CpmApplication::Object> *, int cpm_num);
     void publishCpmSenderObject(double, double, double);
-    
+
     std::ofstream latency_log_file;
 
   private:
@@ -39,15 +37,15 @@ 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 getVehicleDimensionsCallback(const autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response::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_auto_vehicle_msgs::msg::SteeringReport>::SharedPtr steering_report_sub_;
-    rclcpp::Subscription<autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response>::SharedPtr get_vehicle_dimensions_;
     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_;
     rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cpm_sender_pub_;
     rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cam_objects_pub_;
@@ -58,4 +56,4 @@ namespace v2x
   };
 }
 
-#endif
+#endif

src/cam_application.cpp

@@ -107,7 +107,7 @@ namespace v2x
     ego_.heading = *yaw;
   }
 
-  bool CamApplication::setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions msg) {
+  void CamApplication::setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &msg) {
     vehicleDimensions_.wheel_radius = msg.wheel_radius;
     vehicleDimensions_.wheel_width = msg.wheel_width;
     vehicleDimensions_.wheel_base = msg.wheel_base;
@@ -117,8 +117,6 @@ namespace v2x
     vehicleDimensions_.left_overhang = msg.left_overhang;
     vehicleDimensions_.right_overhang = msg.right_overhang;
     vehicleDimensions_.height = msg.height;
-
-    return vehicleDimensions_.height != 0;
   }
 
   void CamApplication::updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
@@ -129,7 +127,7 @@ namespace v2x
     updating_velocity_report_ = true;
 
     rclcpp::Time msg_stamp(msg->header.stamp.sec, msg->header.stamp.nanosec);
-    float dt = (msg_stamp - velocityReport_.stamp).seconds();
+    float dt = msg_stamp.seconds() - velocityReport_.stamp.seconds();
     if (dt == 0) {
       RCLCPP_WARN(node_->get_logger(), "[CamApplication::updateVelocityReport] deltaTime is 0");
       return;
@@ -141,6 +139,8 @@ 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::updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
@@ -152,6 +152,8 @@ namespace v2x
 
     gearReport_.stamp = msg->stamp;
     gearReport_.report = msg->report;
+
+    updating_gear_report_ = false;
   }
 
   void CamApplication::updateSteeringReport(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
@@ -163,6 +165,8 @@ namespace v2x
 
     steeringReport_.stamp = msg->stamp;
     steeringReport_.steering_tire_angle = msg->steering_tire_angle;
+
+    updating_steering_report_ = false;
   }
 
   void CamApplication::send() {
@@ -193,10 +197,17 @@ namespace v2x
     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;
 
+    if (-900000000 <= latitude && latitude <= 900000000) basic_container.referencePosition.latitude = latitude;
+    else basic_container.referencePosition.latitude = Latitude_unavailable;
+    if (-1800000000 <= longitude && longitude <= 1800000000) basic_container.referencePosition.longitude = longitude;
+    else basic_container.referencePosition.longitude = Longitude_unavailable;
+    if (-100000 <= altitude && altitude <= 800000) basic_container.referencePosition.altitude.altitudeValue = altitude;
+    else basic_container.referencePosition.altitude.altitudeValue = AltitudeValue_unavailable;
+
+    // Articles consulted for the positionConficenceEllipse
+    // https://users.cs.utah.edu/~tch/CS4640F2019/resources/A%20geometric%20interpretation%20of%20the%20covariance%20matrix.pdf
+    // https://users.cs.utah.edu/~tch/CS6640F2020/resources/How%20to%20draw%20a%20covariance%20error%20ellipse.pdf
     if (positionConfidenceEllipse_.x.getSize() == positionConfidenceEllipse_.y.getSize()) {
       double xx_sum = 0;
       double yy_sum = 0;
@@ -227,15 +238,12 @@ namespace v2x
                                    : sigma_xx != 0 ? 0 : -90) * 10;
       if (majorOrientation < 0) majorOrientation += 3600;
 
-      basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence = majorConfidence >= 0 && majorConfidence <= 4094
+      if (0 <= majorConfidence && majorConfidence <= 4094) basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence = majorConfidence;
-                                                                                          ? majorConfidence
+      else basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
-                                                                                          : SemiAxisLength_unavailable;
+      if (0 <= minorConfidence && minorConfidence <= 4094) basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence = minorConfidence;
-      basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence = minorConfidence >= 0 && minorConfidence <= 4094
+      else basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
-                                                                                          ? minorConfidence
+      if (0 <= majorOrientation && majorOrientation <= 3600) basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation = majorOrientation;
-                                                                                          : SemiAxisLength_unavailable;
+      else basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
-      basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation = majorOrientation >= 0 && majorOrientation <= 3600
-                                                                                           ? majorOrientation
-                                                                                           : HeadingValue_unavailable;
     } else {
       basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
       basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
@@ -247,9 +255,8 @@ namespace v2x
 
     int heading = std::lround(((-ego_.heading * 180.0 / M_PI) + 90.0) * 10.0);
     if (heading < 0) heading += 3600;
-    bvc.heading.headingValue = heading >= 0 && heading <= 3600
+    if (0 <= heading && heading <= 3600) bvc.heading.headingValue = heading;
-                                 ? heading
+    else bvc.heading.headingValue = HeadingValue_unavailable;
-                                 : HeadingValue_unavailable;
 
     float heading_rate = velocityReport_.heading_rate;
     float lateral_velocity = velocityReport_.lateral_velocity;
@@ -258,8 +265,9 @@ namespace v2x
     uint8_t gearStatus = gearReport_.report;
     float steering_tire_angle = steeringReport_.steering_tire_angle;
 
-    float speed = std::lround(std::sqrt(std::pow(longitudinal_velocity, 2) + std::pow(lateral_velocity, 2)) * 100);
+    long 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 (0 <= speed && speed <= 16382) bvc.speed.speedValue = speed;
+    else bvc.speed.speedValue = SpeedValue_unavailable;
 
     if ((gearStatus >= 2 && gearStatus <= 19) || gearStatus == 23 || gearStatus == 24)
       bvc.driveDirection = DriveDirection_forward;
@@ -268,22 +276,28 @@ namespace v2x
     else
       bvc.driveDirection = DriveDirection_unavailable;
 
-    float vehicleLength = std::lround(vehicleDimensions_.front_overhang + vehicleDimensions_.wheel_base + vehicleDimensions_.rear_overhang * 100);
+    long vehicleLength = std::lround((vehicleDimensions_.front_overhang + vehicleDimensions_.wheel_base + vehicleDimensions_.rear_overhang) * 10);
-    bvc.vehicleLength.vehicleLengthValue = vehicleLength >= 1 && vehicleLength <= 1022 ? vehicleLength : VehicleLengthValue_unavailable;
+    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;
 
-    float vehicleWidth = std::lround(vehicleDimensions_.left_overhang + vehicleDimensions_.wheel_tread + vehicleDimensions_.right_overhang * 100);
+    long vehicleWidth = std::lround((vehicleDimensions_.left_overhang + vehicleDimensions_.wheel_tread + vehicleDimensions_.right_overhang) * 10);
-    bvc.vehicleWidth = vehicleWidth >= 1 && vehicleWidth <= 61 ? vehicleWidth : VehicleWidth_unavailable;
+    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;
 
-    bvc.longitudinalAcceleration.longitudinalAccelerationValue = longitudinal_acceleration >= -160 && longitudinal_acceleration <= 160 ? longitudinal_acceleration : LongitudinalAccelerationValue_unavailable;
+    if (-160 <= longitudinal_acceleration && longitudinal_acceleration <= 160) bvc.longitudinalAcceleration.longitudinalAccelerationValue = longitudinal_acceleration;
+    else bvc.longitudinalAcceleration.longitudinalAccelerationValue = 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<long>::infinity();
-    bvc.curvature.curvatureValue = curvature >= -1023 && curvature <= 1022 ? curvature : CurvatureValue_unavailable;
+    if (-1023 <= curvature && curvature <= 1022) bvc.curvature.curvatureValue = curvature;
-
+    else bvc.curvature.curvatureValue = CurvatureValue_unavailable;
     bvc.curvatureCalculationMode = CurvatureCalculationMode_yawRateNotUsed;
 
     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;
+    if (-32766 <= heading_rate_deg && heading_rate_deg <= 32766) bvc.yawRate.yawRateValue = heading_rate_deg;
+    else bvc.yawRate.yawRateValue = YawRateValue_unavailable;
 
     // UNAVAILABLE VALUES FOR TESTING
     basic_container.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable;

src/v2x_app.cpp

@@ -50,9 +50,11 @@ namespace v2x
     }
   }
 
-  void V2XApp::getVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions msg) {
+  void V2XApp::setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &msg) {
-    if (vehicle_dimensions_set_) return;
+    if (cam_started_)
-    vehicle_dimensions_set_ = cam->setVehicleDimensions(msg);
+      cam->setVehicleDimensions(msg);
+    else
+      this->setVehicleDimensions(msg);
   }
 
   void V2XApp::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {

src/v2x_node.cpp

@@ -8,6 +8,9 @@
 #include "autoware_v2x/cpm_application.hpp"
 #include "autoware_v2x/cam_application.hpp"
 
+#include "autoware_adapi_v1_msgs/srv/get_vehicle_dimensions.hpp"
+#include "autoware_adapi_v1_msgs/msg/vehicle_dimensions.hpp"
+
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 
@@ -40,7 +43,13 @@ namespace v2x
     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));
     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_subscription<autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response>("/api/vehicle/dimensions", 10, std::bind(&V2XNode::getVehicleDimensionsCallback, 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.");
+      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});
@@ -185,8 +194,29 @@ namespace v2x
     app->steeringReportCallback(msg);
   }
 
-  void V2XNode::getVehicleDimensionsCallback(const autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response::ConstSharedPtr msg) {
+  void V2XNode::getVehicleDimensions() {
-    app->getVehicleDimensions(msg->dimensions);
+    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
+     {
+       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)
+           this->getVehicleDimensions();
+         else
+           app->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)
+     {
+       RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service call of /api/vehicle/dimensions failed: %s", e.what());
+     }
+   });
   }
 }