Add CAMApplication prototype
- Mandatory parameters filled through the use of 3 ros topics - Sending ITS packets with CAMs Signed-off-by: Tiago Garcia <tiago.rgarcia@av.it.pt>
This commit is contained in:
parent
5a2ff2f2d5
commit
a2a54ba37f
|
|
@ -35,6 +35,7 @@ ament_auto_add_library(autoware_v2x SHARED
|
|||
src/v2x_app.cpp
|
||||
src/application.cpp
|
||||
src/cpm_application.cpp
|
||||
src/cam_application.cpp
|
||||
src/ethernet_device.cpp
|
||||
src/link_layer.cpp
|
||||
src/raw_socket_link.cpp
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
/**:
|
||||
ros__parameters:
|
||||
network_interface: "wlp4s0"
|
||||
network_interface: "enx5414a7c7d3af"
|
||||
is_sender: true
|
||||
|
|
@ -0,0 +1,148 @@
|
|||
#ifndef CAM_APPLICATION_HPP_EUIC2VFR
|
||||
#define CAM_APPLICATION_HPP_EUIC2VFR
|
||||
|
||||
#include "autoware_v2x/application.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#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_adapi_v1_msgs/msg/vehicle_dimensions.hpp"
|
||||
#include "autoware_v2x/positioning.hpp"
|
||||
#include <vanetza/asn1/cam.hpp>
|
||||
|
||||
namespace v2x
|
||||
{
|
||||
class V2XNode;
|
||||
class CamApplication : public Application
|
||||
{
|
||||
public:
|
||||
CamApplication(V2XNode *node, vanetza::Runtime &, bool is_sender);
|
||||
PortType port() override;
|
||||
//std::string uuidToHexString(const unique_identifier_msgs::msg::UUID&);
|
||||
void indicate(const DataIndication &, UpPacketPtr) override;
|
||||
void set_interval(vanetza::Clock::duration);
|
||||
//void setAllObjectsOfPersonsAnimalsToSend(const autoware_auto_control_msgs::msg::LongitudinalCommand::ConstSharedPtr);
|
||||
void getVehicleDimensions(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 updateMGRS(double *, double *);
|
||||
void updateRP(double *, double *, double *);
|
||||
void updateGenerationTime(int *, long *);
|
||||
void updateHeading(double *);
|
||||
//void printObjectsList(int);
|
||||
void send();
|
||||
|
||||
/** Creates the database schema.
|
||||
* Creates tables for cam_sent, cam_received
|
||||
*/
|
||||
//void createTables();
|
||||
|
||||
/** Inserts CAM into the database.
|
||||
* Constructs and executes queries for inserting the specified CAM data.
|
||||
* @param cam The CAM to be inserted
|
||||
* @param table_name The table to insert the CAM into (cam_sent or cam_received)
|
||||
*/
|
||||
//void insertCamToCamTable(vanetza::asn1::Cam, char*);
|
||||
|
||||
struct Object {
|
||||
std::string uuid;
|
||||
int vehicleID;
|
||||
vanetza::Clock::time_point timestamp;
|
||||
rclcpp::Time timestamp_ros;
|
||||
|
||||
// BasicContainer
|
||||
long stationType;
|
||||
long latitude; // Obtained from tfMessage (same value as CPMs)
|
||||
long longitude; // Obtained from tfMessage (same value as CPMs)
|
||||
long semiMajorConfidence; // Confidence (?)
|
||||
long semiMinorConfidence; // Confidence (?)
|
||||
long semiMajorOrientation; // Confidence (?)
|
||||
long altitude; // Obtained from tfMessage (same value as CPMs)
|
||||
long altitudeConfidence; // Confidence (?)
|
||||
|
||||
// BasicVehicleContainerHighFrequency
|
||||
long heading; // Obtained from tfMessage (same value as CPMs)
|
||||
long headingConfidence; // Confidence (?)
|
||||
long speed; // Obtained from velocityReport after calculation
|
||||
long speedConfidence; // Confidence (?)
|
||||
long driveDirection; // Obtained from gearReport
|
||||
long vehicleLength;
|
||||
long vehicleLengthConfidence; // Confidence (?)
|
||||
long vehicleWidth;
|
||||
long longitudinalAcceleration; // Obtained from velocityReport after calculation over time
|
||||
long longitudinalAccelerationConfidence; // Confidence (?)
|
||||
long curvature; // Obtained from velocityReport after calculation
|
||||
long curvatureConfidence; // Confidence (?)
|
||||
long curvatureCalculationMode; // Manually set
|
||||
long yawRate; // Obtained from velocityReport
|
||||
long yawRateConfidence; // Confidence (?)
|
||||
|
||||
vanetza::PositionFix position;
|
||||
int timeOfMeasurement;
|
||||
bool to_send;
|
||||
int to_send_trigger;
|
||||
};
|
||||
|
||||
private:
|
||||
void schedule_timer();
|
||||
void on_timer(vanetza::Clock::time_point);
|
||||
|
||||
V2XNode *node_;
|
||||
vanetza::Runtime &runtime_;
|
||||
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;
|
||||
};
|
||||
VehicleDimensions vehicleDimensions_;
|
||||
|
||||
struct Ego_station {
|
||||
double mgrs_x;
|
||||
double mgrs_y;
|
||||
double latitude;
|
||||
double longitude;
|
||||
double altitude;
|
||||
double heading;
|
||||
};
|
||||
Ego_station ego_;
|
||||
|
||||
struct VelocityReport {
|
||||
rclcpp::Time stamp;
|
||||
float heading_rate;
|
||||
float lateral_velocity;
|
||||
float longitudinal_velocity;
|
||||
float longitudinal_acceleration;
|
||||
};
|
||||
VelocityReport velocityReport_;
|
||||
|
||||
uint8_t gearReport_;
|
||||
|
||||
int generationTime_;
|
||||
long gdt_timestamp_;
|
||||
|
||||
double objectConfidenceThreshold_;
|
||||
|
||||
bool updating_velocity_report_;
|
||||
bool updating_gear_report_;
|
||||
bool sending_;
|
||||
bool is_sender_;
|
||||
bool reflect_packet_;
|
||||
bool include_all_persons_and_animals_;
|
||||
|
||||
int cam_num_;
|
||||
int received_cam_num_;
|
||||
|
||||
bool use_dynamic_generation_rules_;
|
||||
};
|
||||
}
|
||||
|
||||
#endif /* CAM_APPLICATION_HPP_EUIC2VFR */
|
||||
|
|
@ -4,9 +4,13 @@
|
|||
#include "rclcpp/rclcpp.hpp"
|
||||
#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_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"
|
||||
#include "autoware_v2x/cam_application.hpp"
|
||||
#include "autoware_v2x/time_trigger.hpp"
|
||||
#include "autoware_v2x/link_layer.hpp"
|
||||
#include "autoware_v2x/ethernet_device.hpp"
|
||||
|
|
@ -24,18 +28,29 @@ 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 velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr);
|
||||
void gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr);
|
||||
void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr);
|
||||
|
||||
CpmApplication *cp;
|
||||
CamApplication *cam;
|
||||
// V2XNode *v2x_node;
|
||||
|
||||
private:
|
||||
friend class CpmApplication;
|
||||
friend class CamApplication;
|
||||
friend class Application;
|
||||
V2XNode* node_;
|
||||
bool tf_received_;
|
||||
int tf_interval_;
|
||||
bool velocity_report_received_;
|
||||
int velocity_report_interval_;
|
||||
bool gear_report_received_;
|
||||
int gear_report_interval_;
|
||||
bool vehicle_dimensions_set_;
|
||||
bool cp_started_;
|
||||
bool cam_started_;
|
||||
};
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -4,10 +4,15 @@
|
|||
#include "rclcpp/rclcpp.hpp"
|
||||
#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_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"
|
||||
#include "autoware_v2x/cpm_application.hpp"
|
||||
#include "autoware_v2x/cam_application.hpp"
|
||||
#include "autoware_v2x/time_trigger.hpp"
|
||||
#include "autoware_v2x/link_layer.hpp"
|
||||
#include "autoware_v2x/ethernet_device.hpp"
|
||||
|
|
@ -30,12 +35,20 @@ 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 getVehicleDimensionsCallback(const autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response::ConstSharedPtr msg);
|
||||
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_adapi_v1_msgs::srv::GetVehicleDimensions::Response>::SharedPtr get_vehicle_dimensions_;
|
||||
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_sub_;
|
||||
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_;
|
||||
rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr cam_sender_pub_;
|
||||
|
||||
double pos_lat_;
|
||||
double pos_lon_;
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
<launch>
|
||||
<arg name="network_interface" default="wlp5s0"/>
|
||||
<arg name="network_interface" default="enx5414a7c7d3af"/>
|
||||
<arg name="is_sender" default="true"/>
|
||||
<node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen">
|
||||
<param from="$(find-pkg-share autoware_v2x)/config/autoware_v2x.param.yaml"/>
|
||||
|
|
|
|||
|
|
@ -10,6 +10,8 @@
|
|||
<buildtool_depend>ament_cmake_auto</buildtool_depend>
|
||||
|
||||
<depend>autoware_auto_perception_msgs</depend>
|
||||
<depend>autoware_auto_vehicle_msgs</depend>
|
||||
<depend>autoware_adapi_v1_msgs</depend>
|
||||
<depend>tf2_msgs</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,299 @@
|
|||
#include "autoware_v2x/cam_application.hpp"
|
||||
#include "autoware_v2x/positioning.hpp"
|
||||
#include "autoware_v2x/security.hpp"
|
||||
#include "autoware_v2x/link_layer.hpp"
|
||||
#include "autoware_v2x/v2x_node.hpp"
|
||||
|
||||
#include "tf2/LinearMath/Quaternion.h"
|
||||
#include "tf2/LinearMath/Matrix3x3.h"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include <vanetza/btp/ports.hpp>
|
||||
#include <vanetza/asn1/cam.hpp>
|
||||
#include <vanetza/asn1/packet_visitor.hpp>
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <exception>
|
||||
#include <GeographicLib/UTMUPS.hpp>
|
||||
#include <GeographicLib/MGRS.hpp>
|
||||
#include <string>
|
||||
|
||||
#include <boost/units/cmath.hpp>
|
||||
#include <boost/units/systems/si/prefixes.hpp>
|
||||
|
||||
#include <sqlite3.h>
|
||||
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <math.h>
|
||||
|
||||
using namespace vanetza;
|
||||
using namespace std::chrono;
|
||||
|
||||
namespace v2x
|
||||
{
|
||||
CamApplication::CamApplication(V2XNode * node, Runtime & rt, bool is_sender)
|
||||
: node_(node),
|
||||
runtime_(rt),
|
||||
vehicleDimensions_(),
|
||||
ego_(),
|
||||
velocityReport_(),
|
||||
gearReport_(),
|
||||
generationTime_(0),
|
||||
updating_velocity_report_(false),
|
||||
updating_gear_report_(false),
|
||||
sending_(false),
|
||||
is_sender_(is_sender),
|
||||
reflect_packet_(false),
|
||||
objectConfidenceThreshold_(0.0),
|
||||
include_all_persons_and_animals_(false),
|
||||
cam_num_(0),
|
||||
received_cam_num_(0),
|
||||
use_dynamic_generation_rules_(false)
|
||||
{
|
||||
RCLCPP_INFO(node_->get_logger(), "CamApplication started. is_sender: %d", is_sender_);
|
||||
set_interval(milliseconds(100));
|
||||
//createTables();
|
||||
}
|
||||
|
||||
void CamApplication::set_interval(Clock::duration interval) {
|
||||
cam_interval_ = interval;
|
||||
runtime_.cancel(this);
|
||||
schedule_timer();
|
||||
}
|
||||
|
||||
void CamApplication::schedule_timer() {
|
||||
runtime_.schedule(cam_interval_, std::bind(&CamApplication::on_timer, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
void CamApplication::on_timer(vanetza::Clock::time_point) {
|
||||
schedule_timer();
|
||||
send();
|
||||
}
|
||||
|
||||
CamApplication::PortType CamApplication::port() {
|
||||
return btp::ports::CAM;
|
||||
}
|
||||
|
||||
//std::string CamApplication::uuidToHexString(const unique_identifier_msgs::msg::UUID &id) {
|
||||
// std::stringstream ss;
|
||||
// for (int i = 0; i < id.uuid.size(); i++) {
|
||||
// ss << std::hex << std::setfill('0') << std::setw(2) << (int)id.uuid[i];
|
||||
// }
|
||||
// return ss.str();
|
||||
//}
|
||||
|
||||
void CamApplication::indicate(const Application::DataIndication &, Application::UpPacketPtr)
|
||||
{
|
||||
// TODO: implement
|
||||
}
|
||||
|
||||
void CamApplication::updateMGRS(double *x, double *y) {
|
||||
ego_.mgrs_x = *x;
|
||||
ego_.mgrs_y = *y;
|
||||
}
|
||||
|
||||
void CamApplication::updateRP(double *lat, double *lon, double *altitude) {
|
||||
ego_.latitude = *lat;
|
||||
ego_.longitude = *lon;
|
||||
ego_.altitude = *altitude;
|
||||
}
|
||||
|
||||
void CamApplication::updateGenerationTime(int *gdt, long *gdt_timestamp) {
|
||||
generationTime_ = *gdt;
|
||||
gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
|
||||
}
|
||||
|
||||
void CamApplication::updateHeading(double *yaw) {
|
||||
ego_.heading = *yaw;
|
||||
}
|
||||
|
||||
//void CamApplication::setAllObjectsOfPersonsAnimalsToSend(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) {
|
||||
// if (msg->objects.size() > 0) {
|
||||
// for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) {
|
||||
// std::string object_uuid = uuidToHexString(obj.object_id);
|
||||
// auto found_object = std::find_if(objectsList.begin(), objectsList.end(), [&](auto const &e) {
|
||||
// return !strcmp(e.uuid.c_str(), object_uuid.c_str());
|
||||
// });
|
||||
//
|
||||
// if (found_object == objectsList.end()) {
|
||||
//
|
||||
// } else {
|
||||
// found_object->to_send = true;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
||||
void CamApplication::getVehicleDimensions(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;
|
||||
vehicleDimensions_.wheel_tread = msg.wheel_tread;
|
||||
vehicleDimensions_.front_overhang = msg.front_overhang;
|
||||
vehicleDimensions_.rear_overhang = msg.rear_overhang;
|
||||
vehicleDimensions_.left_overhang = msg.left_overhang;
|
||||
vehicleDimensions_.right_overhang = msg.right_overhang;
|
||||
vehicleDimensions_.height = msg.height;
|
||||
}
|
||||
|
||||
void CamApplication::updateVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
|
||||
if (updating_velocity_report_) {
|
||||
RCLCPP_WARN(node_->get_logger(), "[CamApplication::updatevelocityReport_] already updating velocity report");
|
||||
return;
|
||||
}
|
||||
|
||||
updating_velocity_report_ = true;
|
||||
|
||||
rclcpp::Time msg_stamp(msg->header.stamp.sec, msg->header.stamp.nanosec);
|
||||
float dt = (msg_stamp - velocityReport_.stamp).seconds();
|
||||
if (dt == 0) {
|
||||
RCLCPP_WARN(node_->get_logger(), "[CamApplication::updatevelocityReport_] deltaTime is 0");
|
||||
return;
|
||||
}
|
||||
float longitudinal_acceleration = (msg->longitudinal_velocity - velocityReport_.longitudinal_velocity) / dt;
|
||||
|
||||
velocityReport_.stamp = msg->header.stamp;
|
||||
velocityReport_.heading_rate = msg->heading_rate;
|
||||
velocityReport_.lateral_velocity = msg->lateral_velocity;
|
||||
velocityReport_.longitudinal_velocity = msg->longitudinal_velocity;
|
||||
velocityReport_.longitudinal_acceleration = longitudinal_acceleration;
|
||||
}
|
||||
|
||||
void CamApplication::updateGearReport(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
|
||||
if (updating_gear_report_) {
|
||||
RCLCPP_WARN(node_->get_logger(), "[CamApplication::updatevelocityReport_] already updating gear report");
|
||||
return;
|
||||
}
|
||||
|
||||
updating_gear_report_ = true;
|
||||
|
||||
gearReport_ = msg->report;
|
||||
}
|
||||
|
||||
void CamApplication::send() {
|
||||
if (!is_sender_) return;
|
||||
|
||||
if (sending_) {
|
||||
RCLCPP_WARN(node_->get_logger(), "[CamApplication::send] already sending");
|
||||
return;
|
||||
}
|
||||
|
||||
sending_ = true;
|
||||
|
||||
//printObjectsList(cam_num_);
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] cam_num: %d", cam_num_);
|
||||
// RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] sending CAM");
|
||||
|
||||
vanetza::asn1::Cam message;
|
||||
|
||||
ItsPduHeader_t &header = message->header;
|
||||
header.protocolVersion = 2;
|
||||
header.messageID = ItsPduHeader__messageID_cam;
|
||||
header.stationID = 0;
|
||||
|
||||
CoopAwareness_t &cam = message->cam;
|
||||
|
||||
//asn_long2INTEGER(&cam.generationDeltaTime, (long) gdt_timestamp_);
|
||||
|
||||
BasicContainer_t &basic_container = cam.camParameters.basicContainer;
|
||||
basic_container.stationType = StationType_passengerCar;
|
||||
basic_container.referencePosition.latitude = ego_.latitude * 1e7;
|
||||
basic_container.referencePosition.longitude = ego_.longitude * 1e7;
|
||||
basic_container.referencePosition.altitude.altitudeValue = ego_.altitude;
|
||||
|
||||
// UNAVAILABLE VALUES FOR TESTING
|
||||
basic_container.referencePosition.altitude.altitudeConfidence = AltitudeConfidence_unavailable;
|
||||
basic_container.referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable;
|
||||
basic_container.referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable;
|
||||
basic_container.referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable;
|
||||
// ------------------------------
|
||||
|
||||
BasicVehicleContainerHighFrequency_t &bvc = cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
|
||||
cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
|
||||
|
||||
int heading = std::lround(((-ego_.heading * 180.0 / M_PI) + 90.0) * 10.0);
|
||||
if (heading < 0) heading += 3600;
|
||||
bvc.heading.headingValue = heading;
|
||||
|
||||
float heading_rate = velocityReport_.heading_rate;
|
||||
float lateral_velocity = velocityReport_.lateral_velocity;
|
||||
float longitudinal_velocity = velocityReport_.longitudinal_velocity;
|
||||
|
||||
float speed = std::sqrt(std::pow(longitudinal_velocity, 2) + std::pow(lateral_velocity, 2));
|
||||
bvc.speed.speedValue = std::lround(speed * 100);
|
||||
|
||||
if ((gearReport_ >= 2 && gearReport_ <= 19) || gearReport_ == 21 || gearReport_ == 22)
|
||||
bvc.driveDirection = DriveDirection_forward;
|
||||
else if (gearReport_ == 20 || gearReport_ == 21)
|
||||
bvc.driveDirection = DriveDirection_backward;
|
||||
else
|
||||
bvc.driveDirection = DriveDirection_unavailable;
|
||||
|
||||
float vehicleLength = vehicleDimensions_.front_overhang + vehicleDimensions_.wheel_base + vehicleDimensions_.rear_overhang;
|
||||
if (vehicleLength <= 0 || vehicleLength >= 1023)
|
||||
bvc.vehicleLength.vehicleLengthValue = VehicleLengthValue_unavailable;
|
||||
else
|
||||
bvc.vehicleLength.vehicleLengthValue = vehicleLength;
|
||||
|
||||
float vehicleWidth = vehicleDimensions_.left_overhang + vehicleDimensions_.wheel_tread + vehicleDimensions_.right_overhang;
|
||||
if (vehicleWidth <= 0 || vehicleWidth >= 62)
|
||||
bvc.vehicleWidth = VehicleWidth_unavailable;
|
||||
else
|
||||
bvc.vehicleWidth = vehicleWidth;
|
||||
|
||||
//if (longitudinal_acceleration > 160 || longitudinal_acceleration < -160) {
|
||||
// RCLCPP_WARN(node_->get_logger(), "[CamApplication::send] Longitudinal acceleration out of bounds: %d", longitudinal_acceleration);
|
||||
// bvc.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
|
||||
//} else {
|
||||
// bvc.longitudinalAcceleration.longitudinalAccelerationValue = longitudinal_acceleration;
|
||||
//}
|
||||
|
||||
bvc.longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
|
||||
|
||||
if (longitudinal_velocity != 0)
|
||||
bvc.curvature.curvatureValue = std::lround(lateral_velocity / std::pow(longitudinal_velocity, 2) * 100);
|
||||
else
|
||||
bvc.curvature.curvatureValue = std::numeric_limits<long>::infinity();
|
||||
bvc.curvatureCalculationMode = CurvatureCalculationMode_yawRateNotUsed;
|
||||
|
||||
bvc.yawRate.yawRateValue = std::lround(heading_rate * 100);
|
||||
|
||||
// UNAVAILABLE VALUES FOR TESTING
|
||||
bvc.heading.headingConfidence = HeadingConfidence_unavailable;
|
||||
bvc.speed.speedConfidence = SpeedConfidence_unavailable;
|
||||
bvc.vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable;
|
||||
bvc.longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable;
|
||||
bvc.curvature.curvatureConfidence = CurvatureConfidence_unavailable;
|
||||
bvc.yawRate.yawRateConfidence = YawRateConfidence_unavailable;
|
||||
// ------------------------------
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "[CamApplication::send] Sending CAM");
|
||||
//insertCamToCamTable(message, (char *) "cam_sent");
|
||||
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
|
||||
payload->layer(OsiLayer::Application) = std::move(message);
|
||||
|
||||
Application::DataRequest request;
|
||||
request.its_aid = aid::CP;
|
||||
request.transport_type = geonet::TransportType::SHB;
|
||||
request.communication_profile = geonet::CommunicationProfile::ITS_G5;
|
||||
|
||||
Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
|
||||
|
||||
if (!confirm.accepted()) {
|
||||
throw std::runtime_error("[CamApplication::send] CAM application data request failed");
|
||||
}
|
||||
|
||||
sending_ = false;
|
||||
|
||||
std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds> (
|
||||
std::chrono::system_clock::now().time_since_epoch()
|
||||
);
|
||||
node_->latency_log_file << "T_depart," << cam_num_ << "," << ms.count() << std::endl;
|
||||
|
||||
++cam_num_;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -6,6 +6,7 @@
|
|||
#include "autoware_v2x/security.hpp"
|
||||
#include "autoware_v2x/link_layer.hpp"
|
||||
#include "autoware_v2x/cpm_application.hpp"
|
||||
#include "autoware_v2x/cam_application.hpp"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
|
@ -14,6 +15,7 @@
|
|||
#include "tf2/LinearMath/Matrix3x3.h"
|
||||
|
||||
#include <vanetza/asn1/cpm.hpp>
|
||||
#include <vanetza/asn1/cam.hpp>
|
||||
#include <sstream>
|
||||
#include <memory>
|
||||
#include <GeographicLib/UTMUPS.hpp>
|
||||
|
|
@ -28,11 +30,13 @@ using namespace std::chrono;
|
|||
|
||||
namespace v2x
|
||||
{
|
||||
V2XApp::V2XApp(V2XNode *node) :
|
||||
V2XApp::V2XApp(V2XNode *node) :
|
||||
node_(node),
|
||||
tf_received_(false),
|
||||
tf_interval_(0),
|
||||
cp_started_(false)
|
||||
vehicle_dimensions_set_(false),
|
||||
cp_started_(false),
|
||||
cam_started_(false)
|
||||
{
|
||||
}
|
||||
|
||||
|
|
@ -46,6 +50,30 @@ namespace v2x
|
|||
}
|
||||
}
|
||||
|
||||
void V2XApp::getVehicleDimensions(const autoware_adapi_v1_msgs::msg::VehicleDimensions msg) {
|
||||
if (vehicle_dimensions_set_) return;
|
||||
cam->getVehicleDimensions(msg);
|
||||
vehicle_dimensions_set_ = true;
|
||||
}
|
||||
|
||||
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_) {
|
||||
cam->updateVelocityReport(msg);
|
||||
}
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
if (gear_report_received_ && cam_started_) {
|
||||
cam->updateGearReport(msg);
|
||||
}
|
||||
}
|
||||
|
||||
void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
|
||||
|
||||
tf_received_ = true;
|
||||
|
|
@ -80,20 +108,27 @@ namespace v2x
|
|||
|
||||
// Reverse projection from UTM to geographic.
|
||||
GeographicLib::UTMUPS::Reverse(
|
||||
zone,
|
||||
northp,
|
||||
grid_num_x * grid_size + x,
|
||||
grid_num_y * grid_size + y,
|
||||
lat,
|
||||
zone,
|
||||
northp,
|
||||
grid_num_x * grid_size + x,
|
||||
grid_num_y * grid_size + y,
|
||||
lat,
|
||||
lon
|
||||
);
|
||||
|
||||
|
||||
if (cp && cp_started_) {
|
||||
cp->updateMGRS(&x, &y);
|
||||
cp->updateRP(&lat, &lon, &z);
|
||||
cp->updateHeading(&yaw);
|
||||
cp->updateGenerationTime(&gdt, ×tamp_msec);
|
||||
}
|
||||
|
||||
if (cam && cp_started_) {
|
||||
cam->updateMGRS(&x, &y);
|
||||
cam->updateRP(&lat, &lon, &z);
|
||||
cam->updateHeading(&yaw);
|
||||
cam->updateGenerationTime(&gdt, ×tamp_msec);
|
||||
}
|
||||
}
|
||||
|
||||
void V2XApp::start() {
|
||||
|
|
@ -132,11 +167,14 @@ namespace v2x
|
|||
bool is_sender;
|
||||
node_->get_parameter("is_sender", is_sender);
|
||||
cp = new CpmApplication(node_, trigger.runtime(), is_sender);
|
||||
|
||||
cam = new CamApplication(node_, trigger.runtime(), is_sender);
|
||||
|
||||
context.enable(cp);
|
||||
context.enable(cam);
|
||||
|
||||
cp_started_ = true;
|
||||
cam_started_ = true;
|
||||
|
||||
io_service.run();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -6,11 +6,13 @@
|
|||
#include "autoware_v2x/security.hpp"
|
||||
#include "autoware_v2x/link_layer.hpp"
|
||||
#include "autoware_v2x/cpm_application.hpp"
|
||||
#include "autoware_v2x/cam_application.hpp"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
#include <vanetza/asn1/cpm.hpp>
|
||||
#include <vanetza/asn1/cam.hpp>
|
||||
#include <sstream>
|
||||
#include <memory>
|
||||
#include <boost/thread.hpp>
|
||||
|
|
@ -33,12 +35,17 @@ namespace v2x
|
|||
|
||||
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));
|
||||
gear_report_sub_ = this->create_subscription<autoware_auto_vehicle_msgs::msg::GearReport>("/vehicle/status/gear_status", 10, std::bind(&V2XNode::gearReportCallback, 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));
|
||||
|
||||
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});
|
||||
|
||||
// Declare Parameters
|
||||
this->declare_parameter<std::string>("network_interface", "vmnet1");
|
||||
this->declare_parameter<std::string>("network_interface", "enx5414a7c7d3af");
|
||||
this->declare_parameter<bool>("is_sender", true);
|
||||
|
||||
// Launch V2XApp in a new thread
|
||||
|
|
@ -164,6 +171,18 @@ namespace v2x
|
|||
void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
|
||||
app->tfCallback(msg);
|
||||
}
|
||||
|
||||
void V2XNode::velocityReportCallback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {
|
||||
app->velocityReportCallback(msg);
|
||||
}
|
||||
|
||||
void V2XNode::gearReportCallback(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) {
|
||||
app->gearReportCallback(msg);
|
||||
}
|
||||
|
||||
void V2XNode::getVehicleDimensionsCallback(const autoware_adapi_v1_msgs::srv::GetVehicleDimensions::Response::ConstSharedPtr msg) {
|
||||
app->getVehicleDimensions(msg->dimensions);
|
||||
}
|
||||
}
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
|
|
|||
Loading…
Reference in New Issue