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

Change topic subscriptions to use api 

Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>
This commit is contained in:
Tiago Garcia 2024-07-09 14:54:53 +01:00
parent 6207f27745
commit 26ad12c9e0
No known key found for this signature in database
GPG Key ID: A3B1096AD19E1688
6 changed files with 33 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
include/autoware_v2x/cam_application.hpp
View File

@ -6,9 +6,8 @@
#include <boost/asio/io_service.hpp>
#include <boost/asio/steady_timer.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/msg/vehicle_dimensions.hpp"
#include "autoware_adapi_v1_msgs/msg/vehicle_status.hpp"
#include "autoware_v2x/positioning.hpp"
#include <vanetza/asn1/cam.hpp>
@ -28,8 +27,7 @@ public:
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 updateVehicleStatus(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr);
void send();
private:
@ -126,17 +124,11 @@ private:
};
VelocityReport velocityReport_;
struct GearReport {
rclcpp::Time stamp;
uint8_t report;
};
GearReport gearReport_;
struct SteeringReport {
rclcpp::Time stamp;
struct VehicleStatus {
uint8_t gear;
float steering_tire_angle;
};
SteeringReport steeringReport_;
VehicleStatus vehicleStatus_;
int generationTime_;
long gdt_timestamp_;
@ -144,8 +136,7 @@ private:
double objectConfidenceThreshold_;
bool updating_velocity_report_;
bool updating_gear_report_;
bool updating_steering_report_;
bool updating_vehicle_status_;
bool sending_;
bool is_sender_;
bool reflect_packet_;

12
include/autoware_v2x/v2x_app.hpp
View File

@ -5,10 +5,9 @@
#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 <boost/asio/io_service.hpp>
#include "autoware_v2x/cpm_application.hpp"
@ -32,8 +31,7 @@ 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 gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr);
void steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr);
void vehicleStatusCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr);
void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr);
CpmApplication *cp;
@ -49,10 +47,8 @@ namespace v2x
int tf_interval_;
bool velocity_report_received_;
int velocity_report_interval_;
bool gear_report_received_;
int gear_report_interval_;
bool steering_report_received_;
int steering_report_interval_;
bool vehicle_status_received_;
int vehicle_status_interval_;
bool vehicle_dimensions_set_;
bool cp_started_;
bool cam_started_;

9
include/autoware_v2x/v2x_node.hpp
View File

@ -5,8 +5,7 @@
#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/msg/vehicle_status.hpp"
#include "tf2_msgs/msg/tf_message.hpp"
#include <boost/asio/io_service.hpp>
#include "autoware_v2x/v2x_app.hpp"
@ -35,15 +34,13 @@ 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 gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg);
void steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg);
void vehicleStatucCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::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::msg::VehicleStatus>::SharedPtr vehicle_status_sub_;
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_;

41
src/cam_application.cpp
View File

@ -40,12 +40,10 @@ namespace v2x
ego_(),
positionConfidenceEllipse_(),
velocityReport_(),
gearReport_(),
steeringReport_(),
vehicleStatus_(),
generationTime_(0),
updating_velocity_report_(false),
updating_gear_report_(false),
updating_steering_report_(false),
updating_vehicle_status_(false),
sending_(false),
is_sender_(is_sender),
reflect_packet_(false),
@ -143,30 +141,17 @@ namespace v2x
updating_velocity_report_ = false;
}
void CamApplication::updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
if (updating_gear_report_) {
void CamApplication::updateVehicleStatus(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
if (updating_vehicle_status_) {
return;
}
updating_gear_report_ = true;
updating_vehicle_status_ = true;
gearReport_.stamp = msg->stamp;
gearReport_.report = msg->report;
vehicleStatus_.gear = msg->gear.status;
vehicleStatus_.steering_tire_angle = msg->steering_tire_angle;
updating_gear_report_ = false;
}
void CamApplication::updateSteeringReport(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
if (updating_steering_report_) {
return;
}
updating_steering_report_ = true;
steeringReport_.stamp = msg->stamp;
steeringReport_.steering_tire_angle = msg->steering_tire_angle;
updating_steering_report_ = false;
updating_vehicle_status_ = false;
}
void CamApplication::send() {
@ -262,27 +247,25 @@ namespace v2x
float lateral_velocity = velocityReport_.lateral_velocity;
float longitudinal_velocity = velocityReport_.longitudinal_velocity;
float longitudinal_acceleration = std::lround(velocityReport_.longitudinal_acceleration * 100);
uint8_t gearStatus = gearReport_.report;
float steering_tire_angle = steeringReport_.steering_tire_angle;
uint8_t gearStatus = vehicleStatus_.gear;
float steering_tire_angle = vehicleStatus_.steering_tire_angle;
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)
if (gearStatus == 2 || gearStatus == 5)
bvc.driveDirection = DriveDirection_forward;
else if (gearStatus == 20 || gearStatus == 21)
else if (gearStatus == 3)
bvc.driveDirection = DriveDirection_backward;
else
bvc.driveDirection = DriveDirection_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;
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;

19
src/v2x_app.cpp
View File

@ -66,21 +66,12 @@ namespace v2x
}
}
void V2XApp::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
if (!gear_report_received_) {
RCLCPP_WARN(node_->get_logger(), "[V2XApp::gearReportCallback] GearReport not received yet");
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 (gear_report_received_ && cam_started_) {
cam->updateGearReport(msg);
}
}
void V2XApp::steeringReportCallback(const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg) {
if (!steering_report_received_) {
RCLCPP_WARN(node_->get_logger(), "[V2XApp::gearReportCallback] SteeringReport not received yet");
}
if (steering_report_received_ && cam_started_) {
cam->updateSteeringReport(msg);
if (vehicle_status_received_ && cam_started_) {
cam->updateVehicleStatus(msg);
}
}

11
src/v2x_node.cpp
View File

@ -41,8 +41,7 @@ namespace v2x
// Topic subscriptions for CAMApplication
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));
vehicle_status_sub_ = this->create_subscription<autoware_adapi_v1_msgs::msg::VehicleStatus>("/api/vehicle/status", 10, std::bind(&V2XNode::vehicleStatucCallback, 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.");
@ -186,12 +185,8 @@ namespace v2x
app->velocityReportCallback(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::vehicleStatucCallback(const autoware_adapi_v1_msgs::msg::VehicleStatus::ConstSharedPtr msg) {
app->vehicleStatusCallback(msg);
}
void V2XNode::getVehicleDimensions() {