From f83fbe197c42064f8bb5d0e74bf8ad90d58df209 Mon Sep 17 00:00:00 2001 From: Tiago Garcia Date: Mon, 9 Sep 2024 15:41:54 +0100 Subject: [PATCH] hotfix Signed-off-by: Tiago Garcia --- include/autoware_v2x/cam_application.hpp | 6 +++-- include/autoware_v2x/v2x_app.hpp | 10 +++---- include/autoware_v2x/v2x_node.hpp | 9 ++++--- src/cam_application.cpp | 28 +++++--------------- src/v2x_app.cpp | 24 ++++++++--------- src/v2x_node.cpp | 33 +++++++++++++++--------- 6 files changed, 54 insertions(+), 56 deletions(-) diff --git a/include/autoware_v2x/cam_application.hpp b/include/autoware_v2x/cam_application.hpp index eccbb4e..cb81cc0 100644 --- a/include/autoware_v2x/cam_application.hpp +++ b/include/autoware_v2x/cam_application.hpp @@ -6,8 +6,9 @@ #include #include #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 @@ -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: diff --git a/include/autoware_v2x/v2x_app.hpp b/include/autoware_v2x/v2x_app.hpp index 6ec41b0..2a7d589 100644 --- a/include/autoware_v2x/v2x_app.hpp +++ b/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 #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_; diff --git a/include/autoware_v2x/v2x_node.hpp b/include/autoware_v2x/v2x_node.hpp index 665f13e..0d55fe9 100644 --- a/include/autoware_v2x/v2x_node.hpp +++ b/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 #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::SharedPtr objects_sub_; rclcpp::Subscription::SharedPtr velocity_report_sub_; - rclcpp::Subscription::SharedPtr vehicle_status_sub_; + rclcpp::Subscription::SharedPtr gear_report_sub_; + rclcpp::Subscription::SharedPtr steering_report_sub_; rclcpp::Subscription::SharedPtr tf_sub_; rclcpp::Client::SharedPtr get_vehicle_dimensions_; rclcpp::Publisher::SharedPtr cpm_objects_pub_; diff --git a/src/cam_application.cpp b/src/cam_application.cpp index 231e4cd..68fe585 100644 --- a/src/cam_application.cpp +++ b/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) { - if (updating_vehicle_status_) { - return; - } + void CamApplication::updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) { + rclcpp::Time msg_stamp(msg->stamp.sec, msg->stamp.nanosec); + vehicleStatus_.gear = msg->report; + } - updating_vehicle_status_ = true; - - vehicleStatus_.gear = msg->gear.status; + void CamApplication::updateSteeringReport(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) { 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; diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index 57f466c..599b574 100644 --- a/src/v2x_app.cpp +++ b/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_) { - RCLCPP_WARN(node_->get_logger(), "[V2XApp::velocityReportCallback] VelocityReport not received yet"); - } - if (velocity_report_received_ && cam_started_) { + if (cam_started_) cam->updateVelocityReport(msg); - } } - void V2XApp::vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) { - if (!vehicle_status_received_) { - RCLCPP_WARN(node_->get_logger(), "[V2XApp::vehicleStatusCallback] VehicleStatus not received yet"); - } - if (vehicle_status_received_ && cam_started_) { - cam->updateVehicleStatus(msg); - } + void V2XApp::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) { + if (cam_started_) + cam->updateGearReport(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()); diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index 8b3cb7a..01cf526 100644 --- a/src/v2x_node.cpp +++ b/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("/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("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); tf_sub_ = this->create_subscription("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1)); // Topic subscriptions for CAMApplication velocity_report_sub_ = this->create_subscription("/vehicle/status/velocity_status", 10, std::bind(&V2XNode::velocityReportCallback, this, _1)); - vehicle_status_sub_ = this->create_subscription("/api/vehicle/status", 10, std::bind(&V2XNode::vehicleStatusCallback, this, _1)); - get_vehicle_dimensions_ = this->create_client("/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)."); - } + gear_report_sub_ = this->create_subscription("/vehicle/status/gear_status", 10, std::bind(&V2XNode::gearReportCallback, this, _1)); + steering_report_sub_ = this->create_subscription("/vehicle/status/steering_status", 10, std::bind(&V2XNode::steeringReportCallback, this, _1)); cpm_objects_pub_ = create_publisher("/v2x/cpm/objects", rclcpp::QoS{10}); // cpm_sender_pub_ = create_publisher("/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) { - app->vehicleStatusCallback(msg); + void V2XNode::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr 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) {