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

hotfix 

Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>
This commit is contained in:
Tiago Garcia 2024-09-09 15:41:54 +01:00
parent 3b2e30d283
commit f83fbe197c
No known key found for this signature in database
GPG Key ID: A3B1096AD19E1688
6 changed files with 54 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/autoware_v2x/cam_application.hpp
View File

@ -6,8 +6,9 @@
#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>
@ -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:

10
include/autoware_v2x/v2x_app.hpp
View File

@ -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 <boost/asio/io_service.hpp>
#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_;

9
include/autoware_v2x/v2x_node.hpp
View File

@ -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 <boost/asio/io_service.hpp>
#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<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr objects_sub_;
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>::SharedPtr velocity_report_sub_;
rclcpp::Subscription<autoware_adapi_v1_msgs::msg::VehicleStatus>::SharedPtr vehicle_status_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<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_;

28
src/cam_application.cpp
View File

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

24
src/v2x_app.cpp
View File

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

33
src/v2x_node.cpp
View File

@ -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<autoware_adapi_v1_msgs::srv::GetVehicleDimensions>("/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<autoware_auto_perception_msgs::msg::PredictedObjects>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
tf_sub_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));
// 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));
vehicle_status_sub_ = this->create_subscription<autoware_adapi_v1_msgs::msg::VehicleStatus>("/api/vehicle/status", 10, std::bind(&V2XNode::vehicleStatusCallback, 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).");
}
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));
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});
@ -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) {