diff --git a/include/autoware_v2x/cam_application.hpp b/include/autoware_v2x/cam_application.hpp index 49b5bd6..d23c55f 100644 --- a/include/autoware_v2x/cam_application.hpp +++ b/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: diff --git a/include/autoware_v2x/v2x_app.hpp b/include/autoware_v2x/v2x_app.hpp index ea2f5cc..5e5e4f1 100644 --- a/include/autoware_v2x/v2x_app.hpp +++ b/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 @@ -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 \ No newline at end of file +#endif diff --git a/include/autoware_v2x/v2x_node.hpp b/include/autoware_v2x/v2x_node.hpp index f10e973..d4ee1ae 100644 --- a/include/autoware_v2x/v2x_node.hpp +++ b/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 #include "autoware_v2x/v2x_app.hpp" @@ -31,7 +29,7 @@ namespace v2x V2XApp *app; void publishObjects(std::vector *, 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::SharedPtr objects_sub_; rclcpp::Subscription::SharedPtr velocity_report_sub_; rclcpp::Subscription::SharedPtr gear_report_sub_; rclcpp::Subscription::SharedPtr steering_report_sub_; - rclcpp::Subscription::SharedPtr get_vehicle_dimensions_; rclcpp::Subscription::SharedPtr tf_sub_; + rclcpp::Client::SharedPtr get_vehicle_dimensions_; rclcpp::Publisher::SharedPtr cpm_objects_pub_; rclcpp::Publisher::SharedPtr cpm_sender_pub_; rclcpp::Publisher::SharedPtr cam_objects_pub_; @@ -58,4 +56,4 @@ namespace v2x }; } -#endif \ No newline at end of file +#endif diff --git a/src/cam_application.cpp b/src/cam_application.cpp index 93538b2..53a04b7 100644 --- a/src/cam_application.cpp +++ b/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 - ? majorConfidence - : SemiAxisLength_unavailable; - basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence = minorConfidence >= 0 && minorConfidence <= 4094 - ? minorConfidence - : SemiAxisLength_unavailable; - basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation = majorOrientation >= 0 && majorOrientation <= 3600 - ? majorOrientation - : HeadingValue_unavailable; + if (0 <= majorConfidence && majorConfidence <= 4094) basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence = majorConfidence; + else basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; + if (0 <= minorConfidence && minorConfidence <= 4094) basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence = minorConfidence; + else basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; + if (0 <= majorOrientation && majorOrientation <= 3600) basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation = majorOrientation; + else basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation = 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 - ? heading - : HeadingValue_unavailable; + if (0 <= heading && heading <= 3600) bvc.heading.headingValue = heading; + else bvc.heading.headingValue = 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); - bvc.speed.speedValue = speed >= 0 && speed < 16382 ? speed : SpeedValue_unavailable; + long speed = std::lround(std::sqrt(std::pow(longitudinal_velocity, 2) + std::pow(lateral_velocity, 2)) * 100); + 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); - bvc.vehicleLength.vehicleLengthValue = vehicleLength >= 1 && vehicleLength <= 1022 ? vehicleLength : VehicleLengthValue_unavailable; + long vehicleLength = std::lround((vehicleDimensions_.front_overhang + vehicleDimensions_.wheel_base + vehicleDimensions_.rear_overhang) * 10); + 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); - bvc.vehicleWidth = vehicleWidth >= 1 && vehicleWidth <= 61 ? vehicleWidth : VehicleWidth_unavailable; + long vehicleWidth = std::lround((vehicleDimensions_.left_overhang + vehicleDimensions_.wheel_tread + vehicleDimensions_.right_overhang) * 10); + 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::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; diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index e13237f..b5513e7 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -50,9 +50,11 @@ namespace v2x } } - void V2XApp::getVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions msg) { - if (vehicle_dimensions_set_) return; - vehicle_dimensions_set_ = cam->setVehicleDimensions(msg); + 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) { diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index 2e1b259..7f1af48 100644 --- a/src/v2x_node.cpp +++ b/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("/vehicle/status/velocity_status", 10, std::bind(&V2XNode::velocityReportCallback, this, _1)); 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)); - get_vehicle_dimensions_ = this->create_subscription("/api/vehicle/dimensions", 10, std::bind(&V2XNode::getVehicleDimensionsCallback, 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)."); + } cpm_objects_pub_ = create_publisher("/v2x/cpm/objects", rclcpp::QoS{10}); // cpm_sender_pub_ = create_publisher("/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) { - app->getVehicleDimensions(msg->dimensions); + void V2XNode::getVehicleDimensions() { + 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) + 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()); + } + }); } }