From 43a96d84dec544bd3b4de7a035dfe78c421568f7 Mon Sep 17 00:00:00 2001 From: Tiago Garcia Date: Tue, 10 Sep 2024 12:22:40 +0100 Subject: [PATCH] bug fixes Signed-off-by: Tiago Garcia --- include/autoware_v2x/cam_application.hpp | 22 +++++------ include/autoware_v2x/v2x_app.hpp | 3 -- include/autoware_v2x/v2x_node.hpp | 6 ++- src/cam_application.cpp | 19 ++++++---- src/v2x_app.cpp | 8 ---- src/v2x_node.cpp | 48 ++++++++++++------------ 6 files changed, 49 insertions(+), 57 deletions(-) diff --git a/include/autoware_v2x/cam_application.hpp b/include/autoware_v2x/cam_application.hpp index cb81cc0..3eb631e 100644 --- a/include/autoware_v2x/cam_application.hpp +++ b/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_width; - float wheel_base; - float wheel_tread; - float front_overhang; - float rear_overhang; - float left_overhang; - float right_overhang; - float height; + float wheel_radius = 0.0; + float wheel_width = 0.0; + float wheel_base = 0.0; + float wheel_tread = 0.0; + float front_overhang = 0.0; + float rear_overhang = 0.0; + float left_overhang = 0.0; + float right_overhang = 0.0; + float height = 0.0; }; VehicleDimensions vehicleDimensions_; @@ -132,9 +131,6 @@ private: }; VehicleStatus vehicleStatus_; - int generationDeltaTime_; - long gdt_timestamp_{}; - double objectConfidenceThreshold_; bool updating_velocity_report_; diff --git a/include/autoware_v2x/v2x_app.hpp b/include/autoware_v2x/v2x_app.hpp index 2a7d589..5f7595e 100644 --- a/include/autoware_v2x/v2x_app.hpp +++ b/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 #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); diff --git a/include/autoware_v2x/v2x_node.hpp b/include/autoware_v2x/v2x_node.hpp index 0d55fe9..7388cf3 100644 --- a/include/autoware_v2x/v2x_node.hpp +++ b/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 #include "autoware_v2x/v2x_app.hpp" @@ -32,6 +34,7 @@ namespace v2x void publishObjects(std::vector *, 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::SharedPtr objects_sub_; @@ -55,6 +57,8 @@ namespace v2x double pos_lat_; double pos_lon_; + + bool vehicle_dimensions_available_; }; } diff --git a/src/cam_application.cpp b/src/cam_application.cpp index 68fe585..b2ca927 100644 --- a/src/cam_application.cpp +++ b/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 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::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; diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index 599b574..f100a89 100644 --- a/src/v2x_app.cpp +++ b/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, ×tamp_msec); } } diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index 01cf526..abb889c 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -39,10 +39,11 @@ namespace v2x int timeout = 60; get_vehicle_dimensions_ = this->create_client("/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(); + RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] Service /api/vehicle/dimensions is now available."); + 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("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); @@ -207,31 +208,28 @@ 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(); auto future_result = get_vehicle_dimensions_->async_send_request(request, [this](rclcpp::Client::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) { - 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); - } - } 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()); - } - }); + try { + auto response = future.get(); + RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received response from /api/vehicle/dimensions"); + try { + auto dimensions = response->dimensions; + 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->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) { + RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service call of /api/vehicle/dimensions failed: %s", e.what()); + } + }); } }