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

bug fixes 

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

22
include/autoware_v2x/cam_application.hpp
View File

@ -24,7 +24,6 @@ public:
void set_interval(vanetza::Clock::duration);
void updateMGRS(double *, double *);
void updateRP(double *, double *, double *);
void updateGenerationDeltaTime(int *, long *);
void updateHeading(double *);
void setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &);
void updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr);
@ -41,15 +40,15 @@ private:
vanetza::Clock::duration cam_interval_{};
struct VehicleDimensions {
float wheel_radius;
float wheel_width;
float wheel_base;
float wheel_tread;
float front_overhang;
float rear_overhang;
float left_overhang;
float right_overhang;
float height;
float wheel_radius = 0.0;
float wheel_width = 0.0;
float wheel_base = 0.0;
float wheel_tread = 0.0;
float front_overhang = 0.0;
float rear_overhang = 0.0;
float left_overhang = 0.0;
float right_overhang = 0.0;
float height = 0.0;
};
VehicleDimensions vehicleDimensions_;
@ -132,9 +131,6 @@ private:
};
VehicleStatus vehicleStatus_;
int generationDeltaTime_;
long gdt_timestamp_{};
double objectConfidenceThreshold_;
bool updating_velocity_report_;

3
include/autoware_v2x/v2x_app.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/cpm_application.hpp"
@ -30,7 +28,6 @@ namespace v2x
V2XApp(V2XNode *);
void start();
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);

6
include/autoware_v2x/v2x_node.hpp
View File

@ -7,6 +7,8 @@
#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"
@ -32,6 +34,7 @@ namespace v2x
void publishObjects(std::vector<CpmApplication::Object> *, int cpm_num);
void publishCpmSenderObject(double, double, double);
void publishReceivedCam(etsi_its_cam_ts_msgs::msg::CAM &);
void getVehicleDimensions();
std::ofstream latency_log_file;
@ -40,7 +43,6 @@ 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 getVehicleDimensions();
void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg);
rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr objects_sub_;
@ -55,6 +57,8 @@ namespace v2x
double pos_lat_;
double pos_lon_;
bool vehicle_dimensions_available_;
};
}

19
src/cam_application.cpp
View File

@ -44,7 +44,6 @@ namespace v2x
positionConfidenceEllipse_(),
velocityReport_(),
vehicleStatus_(),
generationDeltaTime_(0),
objectConfidenceThreshold_(0.0),
sending_(false),
is_sender_(is_sender),
@ -62,6 +61,8 @@ namespace v2x
std::mt19937 gen(rd());
std::uniform_int_distribution<unsigned long> dis(0, 4294967295);
stationId_ = dis(gen);
node_->getVehicleDimensions();
}
void CamApplication::set_interval(Clock::duration interval) {
@ -157,16 +158,16 @@ namespace v2x
positionConfidenceEllipse_.y.insert(*lon);
}
void CamApplication::updateGenerationDeltaTime(int *gdt, long *gdt_timestamp) {
generationDeltaTime_ = *gdt;
gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
}
void CamApplication::updateHeading(double *yaw) {
ego_.heading = *yaw;
}
void CamApplication::setVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions &msg) {
if (msg == autoware_adapi_v1_msgs::msg::VehicleDimensions{}) {
RCLCPP_WARN(node_->get_logger(), "[CamApplication::getVehicleDimensions] Vehicle dimensions not available");
return;
}
vehicleDimensions_.wheel_radius = msg.wheel_radius;
vehicleDimensions_.wheel_width = msg.wheel_width;
vehicleDimensions_.wheel_base = msg.wheel_base;
@ -213,6 +214,8 @@ namespace v2x
sending_ = true;
std::chrono::milliseconds now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
vanetza::asn1::Cam message;
ItsPduHeader_t &header = message->header;
@ -222,7 +225,9 @@ namespace v2x
CoopAwareness_t &cam = message->cam;
cam.generationDeltaTime = generationDeltaTime_;
// Convert Unix timestamp to ETSI epoch (2004-01-01 00:00:00)
cam.generationDeltaTime = (now_ms.count() - 1072915200000) % 65536;
RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM with generationDeltaTime %ld", cam.generationDeltaTime);
BasicContainer_t &basic_container = cam.camParameters.basicContainer;
basic_container.stationType = StationType_passengerCar;

8
src/v2x_app.cpp
View File

@ -51,13 +51,6 @@ namespace v2x
}
}
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) {
if (cam_started_)
cam->updateVelocityReport(msg);
@ -127,7 +120,6 @@ namespace v2x
cam->updateMGRS(&x, &y);
cam->updateRP(&lat, &lon, &z);
cam->updateHeading(&yaw);
cam->updateGenerationDeltaTime(&gdt, &timestamp_msec);
}
}

48
src/v2x_node.cpp
View File

@ -39,10 +39,11 @@ namespace v2x
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();
RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] Service /api/vehicle/dimensions is now available.");
vehicle_dimensions_available_ = true;
} else {
RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is not available after waiting (timeout=%ds).", timeout);
RCLCPP_ERROR(get_logger(), "[V2XNode::V2XNode] Service /api/vehicle/dimensions is not available after waiting (timeout=%ds).", timeout);
vehicle_dimensions_available_ = false;
}
objects_sub_ = this->create_subscription<autoware_auto_perception_msgs::msg::PredictedObjects>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
@ -207,31 +208,28 @@ namespace v2x
}
void V2XNode::getVehicleDimensions() {
if (!vehicle_dimensions_available_) {
RCLCPP_ERROR(get_logger(), "[V2XNode::getVehicleDimensions] Service /api/vehicle/dimensions is not available.");
return;
}
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) {
RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received vehicle dimensions are zero. Trying again...");
this->getVehicleDimensions();
} 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) {
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());
}
});
try {
auto response = future.get();
RCLCPP_INFO(get_logger(), "[V2XNode::getVehicleDimensions] Received response from /api/vehicle/dimensions");
try {
auto dimensions = response->dimensions;
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->cam->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());
}
});
}
}