TiagoRG
/
AutowareV2X
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>
This commit is contained in:
Tiago Garcia 2024-07-08 11:08:49 +01:00
parent 67e214cda2
commit 6207f27745
No known key found for this signature in database
GPG Key ID: A3B1096AD19E1688
6 changed files with 92 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/autoware_v2x/cam_application.hpp
View File

@ -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:

3
include/autoware_v2x/v2x_app.hpp
View File

@ -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);

6
include/autoware_v2x/v2x_node.hpp
View File

@ -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"
@ -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_;

72
src/cam_application.cpp
View File

@ -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<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;

8
src/v2x_app.cpp
View File

@ -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) {

36
src/v2x_node.cpp
View File

@ -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) {
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<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());
}
});
}
}