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

Merge pull request #1 from yuasabe/separate_v2x_app 

Separate v2x app
This commit is contained in:
Yu Asabe 2021-11-10 17:18:02 +09:00 committed by GitHub
parent f2b667e44a ed2656a482
commit 62d7576f74
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 529 additions and 184 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -32,6 +32,7 @@ include_directories(
ament_auto_add_library(autoware_v2x SHARED
src/v2x_node.cpp
src/v2x_app.cpp
src/application.cpp
src/cpm_application.cpp
src/ethernet_device.cpp

44
include/autoware_v2x/cpm_application.hpp
View File

@ -11,13 +11,53 @@
class CpmApplication : public Application
{
public:
CpmApplication(rclcpp::Node *node);
CpmApplication(rclcpp::Node *node, vanetza::Runtime&);
PortType port() override;
void indicate(const DataIndication &, UpPacketPtr) override;
void send(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr, rclcpp::Node *, double, double);
void set_interval(vanetza::Clock::duration);
void updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr);
void updateMGRS(double *, double *);
void updateRP(double *, double *, double *);
void updateGenerationDeltaTime(int *);
void updateHeading(double *);
void send();
private:
void schedule_timer();
void on_timer(vanetza::Clock::time_point);
rclcpp::Node* node_;
vanetza::Runtime& runtime_;
vanetza::Clock::duration cpm_interval_;
struct Object {
int objectID; // 0-255
rclcpp::Time timestamp;
double position_x;
double position_y;
double position_z;
double orientation_x;
double orientation_y;
double orientation_z;
double orientation_w;
double xDistance;
double yDistance;
double xSpeed;
double ySpeed;
vanetza::PositionFix position;
int timeOfMeasurement;
};
std::vector<CpmApplication::Object> objectsStack;
double ego_x_;
double ego_y_;
double ego_lat_;
double ego_lon_;
double ego_altitude_;
double ego_heading_;
int generationDeltaTime_;
bool updating_objects_stack_;
bool sending_;
};
#endif /* CPM_APPLICATION_HPP_EUIC2VFR */

38
include/autoware_v2x/v2x_app.hpp Normal file
View File

@ -0,0 +1,38 @@
#ifndef V2X_APP_HPP_EUIC2VFR
#define V2X_APP_HPP_EUIC2VFR
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "autoware_perception_msgs/msg/dynamic_object_array.hpp"
#include "tf2_msgs/msg/tf_message.hpp"
#include <boost/asio/io_service.hpp>
#include "autoware_v2x/cpm_application.hpp"
#include "autoware_v2x/time_trigger.hpp"
#include "autoware_v2x/link_layer.hpp"
#include "autoware_v2x/ethernet_device.hpp"
#include "autoware_v2x/positioning.hpp"
#include "autoware_v2x/security.hpp"
#include "autoware_v2x/router_context.hpp"
namespace v2x
{
class V2XApp
{
public:
V2XApp(rclcpp::Node*);
void start();
void objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr);
void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr);
CpmApplication *cp;
private:
friend class CpmApplication;
friend class Application;
rclcpp::Node* node_;
bool tf_received_;
bool cp_started_;
};
}
#endif

32
include/autoware_v2x/v2x_node.hpp
View File

@ -3,6 +3,7 @@
#include "autoware_perception_msgs/msg/dynamic_object_array.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/time_trigger.hpp"
#include "autoware_v2x/link_layer.hpp"
@ -17,32 +18,35 @@ namespace v2x
{
public:
explicit V2XNode(const rclcpp::NodeOptions &node_options);
V2XApp *app;
private:
void objectsCallback(
const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg);
void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg);
friend class CpmApplication;
friend class Application;
// friend class CpmApplication;
// friend class Application;
rclcpp::Subscription<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr subscription_;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_pos_;
std::unique_ptr<CpmApplication> cp_;
std::unique_ptr<Application> app_;
boost::asio::io_service io_service_;
TimeTrigger trigger_;
const char *device_name_;
EthernetDevice device_;
vanetza::MacAddress mac_address_;
std::unique_ptr<LinkLayer> link_layer_;
vanetza::geonet::MIB mib_;
std::unique_ptr<vanetza::PositionProvider> positioning_;
std::unique_ptr<vanetza::security::SecurityEntity> security_;
RouterContext context_;
// std::unique_ptr<CpmApplication> cp_;
// std::unique_ptr<Application> app_;
// boost::asio::io_service io_service_;
// TimeTrigger trigger_;
// vanetza::Runtime& runtime_;
// const char *device_name_;
// EthernetDevice device_;
// vanetza::MacAddress mac_address_;
// std::unique_ptr<LinkLayer> link_layer_;
// vanetza::geonet::MIB mib_;
// std::unique_ptr<vanetza::PositionProvider> positioning_;
// std::unique_ptr<vanetza::security::SecurityEntity> security_;
// RouterContext context_;
double pos_lat_;
double pos_lon_;
// vanetza::PositionFix reference_position_;
};
}

322
src/cpm_application.cpp
View File

@ -14,15 +14,47 @@
#include <sstream>
#include <exception>
#define _USE_MATH_DEFINES
#include <math.h>
// This is a very simple application that sends BTP-B messages with the content 0xc0ffee.
using namespace vanetza;
using namespace vanetza::facilities;
using namespace std::chrono;
CpmApplication::CpmApplication(rclcpp::Node *node) : node_(node)
CpmApplication::CpmApplication(rclcpp::Node *node, Runtime &rt) : node_(node),
runtime_(rt),
ego_x_(0),
ego_y_(0),
ego_lat_(0),
ego_lon_(0),
ego_altitude_(0),
ego_heading_(0),
generationDeltaTime_(0),
updating_objects_stack_(false),
sending_(false)
{
RCLCPP_INFO(node_->get_logger(), "CpmApplication started...");
set_interval(milliseconds(1000));
}
void CpmApplication::set_interval(Clock::duration interval)
{
cpm_interval_ = interval;
runtime_.cancel(this);
schedule_timer();
}
void CpmApplication::schedule_timer()
{
runtime_.schedule(cpm_interval_, std::bind(&CpmApplication::on_timer, this, std::placeholders::_1), this);
}
void CpmApplication::on_timer(Clock::time_point)
{
schedule_timer();
send();
}
CpmApplication::PortType CpmApplication::port()
@ -34,9 +66,12 @@ void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr pack
{
asn1::PacketVisitor<asn1::Cpm> visitor;
std::shared_ptr<const asn1::Cpm> cpm = boost::apply_visitor(visitor, *packet);
if (cpm) {
if (cpm)
{
RCLCPP_INFO(node_->get_logger(), "Received decodable CPM content");
} else {
}
else
{
RCLCPP_INFO(node_->get_logger(), "Received broken content");
}
asn1::Cpm message = *cpm;
@ -50,121 +85,174 @@ void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr pack
RCLCPP_INFO(node_->get_logger(), "cpm.(reference position) = %f, %f", lat, lon);
}
void CpmApplication::send(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg, rclcpp::Node *node, double pos_lat, double pos_lon)
{
RCLCPP_INFO(node->get_logger(), "Sending CPM...");
// Send all objects in 1 CPM message
vanetza::asn1::Cpm message;
// ITS PDU Header
ItsPduHeader_t &header = message->header;
header.protocolVersion = 1;
header.messageID = 14;
header.stationID = 1;
const auto time_now = duration_cast<milliseconds>(system_clock::now().time_since_epoch());
uint16_t gen_delta_time = time_now.count();
CollectivePerceptionMessage_t &cpm = message->cpm;
cpm.generationDeltaTime = gen_delta_time * GenerationDeltaTime_oneMilliSec;
// auto position = positioning->position_specify(pos_lat, pos_lon);
CpmManagementContainer_t& management = cpm.cpmParameters.managementContainer;
management.stationType = StationType_passengerCar;
// management.referencePosition.latitude = pos_lat;
// management.referencePosition.longitude = pos_lon;
// management.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 1.0;
// management.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 1.0;
PositionFix fix;
// fix.timestamp = time_now;
fix.latitude = pos_lat * units::degree;
fix.longitude = pos_lon * units::degree;
fix.confidence.semi_major = 1.0 * units::si::meter;
fix.confidence.semi_minor = fix.confidence.semi_major;
copy(fix, management.referencePosition);
// cpm.cpmParameters.stationDataContainer = NULL;
// cpm.cpmParameters.perceivedObjectContainer = NULL;
cpm.cpmParameters.numberOfPerceivedObjects = msg->objects.size();
StationDataContainer_t*& sdc = cpm.cpmParameters.stationDataContainer;
sdc = vanetza::asn1::allocate<StationDataContainer_t>();
// RCLCPP_INFO(node->get_logger(), "Allocated sdc");
sdc->present = StationDataContainer_PR_originatingVehicleContainer;
OriginatingVehicleContainer_t& ovc = sdc->choice.originatingVehicleContainer;
ovc.speed.speedValue = 0;
ovc.speed.speedConfidence = 1;
ovc.heading.headingValue = 0;
ovc.heading.headingConfidence = 1;
PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer;
poc = vanetza::asn1::allocate<PerceivedObjectContainer_t>();
// cpm.cpmParameters.perceivedObjectContainer = poc;
// PerceivedObjectContainer_t pocc = *poc;
// RCLCPP_INFO(node->get_logger(), "Allocated poc");
for (auto object : msg->objects) {
PerceivedObject *pObj = vanetza::asn1::allocate<PerceivedObject>();
pObj->objectID = 1;
pObj->timeOfMeasurement = 10;
pObj->xDistance.value = 100;
pObj->xDistance.confidence = 1;
pObj->yDistance.value = 200;
pObj->yDistance.confidence = 1;
pObj->xSpeed.value = 5;
pObj->xSpeed.confidence = 1;
pObj->ySpeed.value = 0;
pObj->ySpeed.confidence = 1;
ASN_SEQUENCE_ADD(poc, pObj);
// RCLCPP_INFO(node->get_logger(), "Added one object to poc->list");
}
Application::DownPacketPtr packet{new DownPacket()};
std::unique_ptr<geonet::DownPacket> payload { new geonet::DownPacket() };
// std::shared_ptr<asn1::Cpm> message_p = std::make_shared<asn1::Cpm>(message);
// std::unique_ptr<convertible::byte_buffer> buffer { new convertible::byte_buffer_impl<asn1::Cpm>(&message)};
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;
// RCLCPP_INFO(node->get_logger(), "Packet Size: %d", payload->size());
// RCLCPP_INFO(node->get_logger(), "Going to Application::request...");
Application::DataConfirm confirm = Application::request(request, std::move(payload), node);
if (!confirm.accepted())
{
throw std::runtime_error("CPM application data request failed");
}
// RCLCPP_INFO(node->get_logger(), "Application::request END");
void CpmApplication::updateMGRS(double *x, double *y) {
RCLCPP_INFO(node_->get_logger(), "Update MGRS");
ego_x_ = *x;
ego_y_ = *y;
}
// void HelloApplication::schedule_timer()
// {
// timer_.expires_from_now(interval_);
// timer_.async_wait(std::bind(&HelloApplication::on_timer, this, std::placeholders::_1));
// }
void CpmApplication::updateRP(double *lat, double *lon, double *altitude)
{
RCLCPP_INFO(node_->get_logger(), "Update RP");
ego_lat_ = *lat;
ego_lon_ = *lon;
ego_altitude_ = *altitude;
}
// void HelloApplication::on_timer(const boost::system::error_code& ec)
// {
// if (ec != boost::asio::error::operation_aborted) {
// DownPacketPtr packet { new DownPacket() };
// packet->layer(OsiLayer::Application) = ByteBuffer { 0xC0, 0xFF, 0xEE };
// DataRequest request;
// request.transport_type = geonet::TransportType::SHB;
// request.communication_profile = geonet::CommunicationProfile::ITS_G5;
// request.its_aid = aid::CA;
// auto confirm = Application::request(request, std::move(packet));
// if (!confirm.accepted()) {
// throw std::runtime_error("Hello application data request failed");
// }
void CpmApplication::updateGenerationDeltaTime(int *gdt)
{
RCLCPP_INFO(node_->get_logger(), "Update GDT");
generationDeltaTime_ = *gdt;
}
// schedule_timer();
// }
// }
void CpmApplication::updateHeading(double *yaw)
{
RCLCPP_INFO(node_->get_logger(), "Update Heading : %f", *yaw);
ego_heading_ = *yaw;
}
void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg)
{
RCLCPP_INFO(node_->get_logger(), "Update ObjectsStack");
updating_objects_stack_ = true;
if (sending_)
{
return;
}
if (msg->objects.size() > 0)
{
RCLCPP_INFO(node_->get_logger(), "At least 1 object detected");
// Initialize ObjectsStack
// std::vector<CpmApplication::Object> objectsStack;
objectsStack.clear();
// Create CpmApplication::Object per msg->object and add it to objectsStack
int i = 0;
for (auto obj : msg->objects)
{
CpmApplication::Object object;
object.objectID = i;
object.timestamp = msg->header.stamp;
object.position_x = obj.state.pose_covariance.pose.position.x; // MGRS
object.position_y = obj.state.pose_covariance.pose.position.y;
object.position_z = obj.state.pose_covariance.pose.position.z;
object.orientation_x = obj.state.pose_covariance.pose.orientation.x;
object.orientation_y = obj.state.pose_covariance.pose.orientation.y;
object.orientation_z = obj.state.pose_covariance.pose.orientation.z;
object.orientation_w = obj.state.pose_covariance.pose.orientation.w;
object.xDistance = ((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100;
object.yDistance = ((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100;
// RCLCPP_INFO(node_->get_logger(), "xDistance: %f, yDistance: %f", object.xDistance, object.yDistance);
objectsStack.push_back(object);
++i;
}
}
RCLCPP_INFO(node_->get_logger(), "%d objects added to objectsStack", objectsStack.size());
updating_objects_stack_ = false;
}
void CpmApplication::send()
{
sending_ = true;
if (!updating_objects_stack_)
{
RCLCPP_INFO(node_->get_logger(), "Sending CPM...");
// Send all objects in 1 CPM message
vanetza::asn1::Cpm message;
// ITS PDU Header
ItsPduHeader_t &header = message->header;
header.protocolVersion = 1;
header.messageID = 14;
header.stationID = 1;
// const auto time_now = duration_cast<milliseconds>(system_clock::now().time_since_epoch());
// uint16_t gen_delta_time = time_now.count();
CollectivePerceptionMessage_t &cpm = message->cpm;
cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec;
// auto position = positioning->position_specify(pos_lat, pos_lon);
CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer;
management.stationType = StationType_passengerCar;
// management.referencePosition.latitude = pos_lat;
// management.referencePosition.longitude = pos_lon;
// management.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 1.0;
// management.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 1.0;
PositionFix fix;
// fix.timestamp = time_now;
fix.latitude = ego_lat_ * units::degree;
fix.longitude = ego_lon_ * units::degree;
// fix.altitude = ego_altitude_;
fix.confidence.semi_major = 1.0 * units::si::meter;
fix.confidence.semi_minor = fix.confidence.semi_major;
copy(fix, management.referencePosition);
// cpm.cpmParameters.stationDataContainer = NULL;
// cpm.cpmParameters.perceivedObjectContainer = NULL;
cpm.cpmParameters.numberOfPerceivedObjects = objectsStack.size();
StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer;
sdc = vanetza::asn1::allocate<StationDataContainer_t>();
// RCLCPP_INFO(node->get_logger(), "Allocated sdc");
sdc->present = StationDataContainer_PR_originatingVehicleContainer;
OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer;
ovc.speed.speedValue = 0;
ovc.speed.speedConfidence = 1;
// ovc.heading.headingValue = (int) (1.5708 - ego_heading_) * M_PI / 180;
ovc.heading.headingValue = (int) std::fmod((1.5708-ego_heading_) * 180 / M_PI, 360.0) * 10;
ovc.heading.headingConfidence = 1;
PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer;
poc = vanetza::asn1::allocate<PerceivedObjectContainer_t>();
// cpm.cpmParameters.perceivedObjectContainer = poc;
// PerceivedObjectContainer_t pocc = *poc;
// RCLCPP_INFO(node->get_logger(), "Allocated poc");
for (CpmApplication::Object object : objectsStack)
{
PerceivedObject *pObj = vanetza::asn1::allocate<PerceivedObject>();
pObj->objectID = object.objectID;
pObj->timeOfMeasurement = object.timeOfMeasurement;
pObj->xDistance.value = object.xDistance;
pObj->xDistance.confidence = 1;
pObj->yDistance.value = object.yDistance;
pObj->yDistance.confidence = 1;
pObj->xSpeed.value = object.xSpeed;
pObj->xSpeed.confidence = 1;
pObj->ySpeed.value = object.ySpeed;
pObj->ySpeed.confidence = 1;
ASN_SEQUENCE_ADD(poc, pObj);
// RCLCPP_INFO(node->get_logger(), "Added one object to poc->list");
}
Application::DownPacketPtr packet{new DownPacket()};
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
// std::shared_ptr<asn1::Cpm> message_p = std::make_shared<asn1::Cpm>(message);
// std::unique_ptr<convertible::byte_buffer> buffer { new convertible::byte_buffer_impl<asn1::Cpm>(&message)};
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;
// RCLCPP_INFO(node->get_logger(), "Packet Size: %d", payload->size());
// RCLCPP_INFO(node->get_logger(), "Going to Application::request...");
Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
if (!confirm.accepted())
{
throw std::runtime_error("CPM application data request failed");
}
}
sending_ = false;
// RCLCPP_INFO(node->get_logger(), "Application::request END");
}

185
src/v2x_app.cpp Normal file
View File

@ -0,0 +1,185 @@
#include "autoware_v2x/v2x_app.hpp"
#include "autoware_v2x/time_trigger.hpp"
#include "autoware_v2x/router_context.hpp"
#include "autoware_v2x/positioning.hpp"
#include "autoware_v2x/security.hpp"
#include "autoware_v2x/link_layer.hpp"
#include "autoware_v2x/cpm_application.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include <vanetza/asn1/cpm.hpp>
#include <vanetza/facilities/cpm_functions.hpp>
#include <sstream>
#include <memory>
#include <GeographicLib/UTMUPS.hpp>
#include <GeographicLib/MGRS.hpp>
#include <boost/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
namespace gn = vanetza::geonet;
using namespace vanetza;
using namespace vanetza::facilities;
using namespace std::chrono;
namespace v2x
{
V2XApp::V2XApp(rclcpp::Node *node) :
node_(node),
tf_received_(false),
cp_started_(false)
// io_service_(),
// trigger_(io_service_),
// runtime_(new Runtime(Clock::at(boost::posix_time::microsec_clock::universal_time()))),
// device_name_("wlp4s0"),
// device_(device_name_),
// mac_address_(device_.address()),
// link_layer_(create_link_layer(io_service_, device_, "ethernet")),
// positioning_(create_position_provider(io_service_, trigger_.runtime())),
// security_(create_security_entity(trigger_.runtime(), *positioning_)),
// mib_(),
// cp_(new CpmApplication(node_, trigger_.runtime())),
// context_(mib_, trigger_, *positioning_, security_.get()),
// app_()
{
// device_name_ = "wlp4s0";
// device_(device_name_);
// mac_address_ = device_.address();
// std::stringstream sout;
// sout << mac_address_;
// RCLCPP_INFO(node_->get_logger(), "MAC Address: '%s'", sout.str().c_str());
// trigger_(io_service_);
// link_layer_ = create_link_layer(io_service_, device_, "ethernet");
// mib_.itsGnLocalGnAddr.mid(mac_address_);
// mib_.itsGnLocalGnAddr.is_manually_configured(true);
// mib_.itsGnLocalAddrConfMethod = geonet::AddrConfMethod::Managed;
// mib_.itsGnSecurity = false;
// mib_.itsGnProtocolVersion = 1;
// context_(mib_, trigger_, *positioning_, security_.get()),
// context_.router_.set_address(mib_.itsGnLocalGnAddr);
// context_.updateMIB(mib_);
// positioning_ = create_position_provider(io_service_, trigger_.runtime());
// security_ = create_security_entity(trigger_.runtime(), *positioning_);
// RouterContext context_(mib_, trigger_, *positioning_, security_.get());
// RouterContext context_(mib_, trigger_, *positioning_, security_.get());
// context_.set_link_layer(link_layer_.get());
// std::unique_ptr<CpmApplication> cp_ { new CpmApplication(this) };
// app_ = std::move(cp_);
// context_.enable(cp_.get());
// io_service_.run();
// boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_));
// // Print MAC Address to logger
// std::stringstream sout;
// sout << mac_address;
// RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str());
}
void V2XApp::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) {
RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received");
if (tf_received_) {
cp->updateObjectsStack(msg);
}
}
void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
RCLCPP_INFO(node_->get_logger(), "V2XApp: tf msg received");
tf_received_ = true;
double x = msg->transforms[0].transform.translation.x;
double y = msg->transforms[0].transform.translation.y;
double z = msg->transforms[0].transform.translation.z;
int timestamp = msg->transforms[0].header.stamp.sec;
int gdt = timestamp % 65536;
double rot_x = msg->transforms[0].transform.rotation.x;
double rot_y = msg->transforms[0].transform.rotation.y;
double rot_z = msg->transforms[0].transform.rotation.z;
double rot_w = msg->transforms[0].transform.rotation.w;
// Convert the quarternion to euler (yaw, pitch, roll)
tf2::Quaternion quat(rot_x, rot_y, rot_z, rot_w);
tf2::Matrix3x3 matrix(quat);
double roll, pitch, yaw;
matrix.getRPY(roll, pitch, yaw);
char mgrs[20];
int zone, prec;
bool northp;
double x_mgrs, y_mgrs;
double lat, lon;
sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y);
RCLCPP_INFO(node_->get_logger(), "MGRS: %s", mgrs);
GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec);
GeographicLib::UTMUPS::Reverse(zone, northp, x_mgrs, y_mgrs, lat, lon);
RCLCPP_INFO(node_->get_logger(), "Ego Position Lat/Lon: %f, %f", lat, lon);
RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f, %f, %f, %f", rot_x, rot_y, rot_z, rot_w);
RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f %f %f", roll, pitch, yaw);
RCLCPP_INFO(node_->get_logger(), "Timestamp: %d, GDT: %d", timestamp, gdt);
if (cp && cp_started_) {
cp->updateMGRS(&x, &y);
cp->updateRP(&lat, &lon, &z);
cp->updateHeading(&yaw);
cp->updateGenerationDeltaTime(&gdt);
}
}
void V2XApp::start() {
RCLCPP_INFO(node_->get_logger(), "V2X App Launched");
boost::asio::io_service io_service;
TimeTrigger trigger(io_service);
const char* device_name = "wlp4s0";
EthernetDevice device(device_name);
vanetza::MacAddress mac_address = device.address();
std::stringstream sout;
sout << mac_address;
RCLCPP_INFO(node_->get_logger(), "MAC Address: '%s'", sout.str().c_str());
gn::MIB mib;
mib.itsGnLocalGnAddr.mid(mac_address);
mib.itsGnLocalGnAddr.is_manually_configured(true);
mib.itsGnLocalAddrConfMethod = geonet::AddrConfMethod::Managed;
mib.itsGnSecurity = false;
mib.itsGnProtocolVersion = 1;
// context_.updateMIB(mib_);
auto link_layer = create_link_layer(io_service, device, "ethernet");
auto positioning = create_position_provider(io_service, trigger.runtime());
auto security = create_security_entity(trigger.runtime(), *positioning);
RouterContext context(mib, trigger, *positioning, security.get());
context.set_link_layer(link_layer.get());
cp = new CpmApplication(node_, trigger.runtime());
// context.enable(cp_.get());
context.enable(cp);
cp_started_ = true;
io_service.run();
}
}

91
src/v2x_node.cpp
View File

@ -1,4 +1,5 @@
#include "autoware_v2x/v2x_node.hpp"
#include "autoware_v2x/v2x_app.hpp"
#include "autoware_v2x/time_trigger.hpp"
#include "autoware_v2x/router_context.hpp"
#include "autoware_v2x/positioning.hpp"
@ -13,9 +14,8 @@
#include <vanetza/facilities/cpm_functions.hpp>
#include <sstream>
#include <memory>
#include <GeographicLib/UTMUPS.hpp>
#include <GeographicLib/MGRS.hpp>
#include <boost/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
namespace gn = vanetza::geonet;
@ -25,23 +25,23 @@ using namespace std::chrono;
namespace v2x
{
V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) :
rclcpp::Node("autoware_v2x_node", node_options),
io_service_(),
trigger_(io_service_),
device_name_("wlp4s0"),
device_(device_name_),
mac_address_(device_.address()),
link_layer_(create_link_layer(io_service_, device_, "ethernet")),
positioning_(create_position_provider(io_service_, trigger_.runtime())),
security_(create_security_entity(trigger_.runtime(), *positioning_)),
mib_(),
cp_(new CpmApplication(this)),
context_(mib_, trigger_, *positioning_, security_.get()),
app_()
V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options)
// io_service_(),
//trigger_(io_service_),
// runtime_(new Runtime(Clock::at(boost::posix_time::microsec_clock::universal_time()))),
// device_name_("wlp4s0"),
// device_(device_name_),
// mac_address_(device_.address()),
// link_layer_(create_link_layer(io_service_, device_, "ethernet")),
// positioning_(create_position_provider(io_service_, trigger_.runtime())),
// security_(create_security_entity(trigger_.runtime(), *positioning_)),
// mib_(),
// cp_(new CpmApplication(this, runtime_)),
// context_(mib_, trigger_, *positioning_, security_.get()),
// app_()
{
using std::placeholders::_1;
subscription_ = this->create_subscription<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/detection/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
subscription_ = this->create_subscription<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
subscription_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));
RCLCPP_INFO(get_logger(), "V2X Node Launched");
@ -50,38 +50,41 @@ namespace v2x
// device_(device_name_);
// mac_address_ = device_.address();
std::stringstream sout;
sout << mac_address_;
RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str());
// std::stringstream sout;
// sout << mac_address_;
// RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str());
// trigger_(io_service_);
// link_layer_ = create_link_layer(io_service_, device_, "ethernet");
mib_.itsGnLocalGnAddr.mid(mac_address_);
mib_.itsGnLocalGnAddr.is_manually_configured(true);
mib_.itsGnLocalAddrConfMethod = geonet::AddrConfMethod::Managed;
mib_.itsGnSecurity = false;
mib_.itsGnProtocolVersion = 1;
// mib_.itsGnLocalGnAddr.mid(mac_address_);
// mib_.itsGnLocalGnAddr.is_manually_configured(true);
// mib_.itsGnLocalAddrConfMethod = geonet::AddrConfMethod::Managed;
// mib_.itsGnSecurity = false;
// mib_.itsGnProtocolVersion = 1;
// context_(mib_, trigger_, *positioning_, security_.get()),
// context_.router_.set_address(mib_.itsGnLocalGnAddr);
context_.updateMIB(mib_);
// context_.updateMIB(mib_);
// positioning_ = create_position_provider(io_service_, trigger_.runtime());
// security_ = create_security_entity(trigger_.runtime(), *positioning_);
// RouterContext context_(mib_, trigger_, *positioning_, security_.get());
// RouterContext context_(mib_, trigger_, *positioning_, security_.get());
context_.set_link_layer(link_layer_.get());
// context_.set_link_layer(link_layer_.get());
// std::unique_ptr<CpmApplication> cp_ { new CpmApplication(this) };
// app_ = std::move(cp_);
context_.enable(cp_.get());
// context_.enable(cp_.get());
// io_service_.run();
boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_));
// boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_));
app = new V2XApp(this);
boost::thread v2xApp(boost::bind(&V2XApp::start, app));
// boost::thread v2xApp(V2XApp);
// boost::thread v2xApp(&V2XApp, this);
// // Print MAC Address to logger
// std::stringstream sout;
@ -94,31 +97,17 @@ namespace v2x
// RCLCPP_INFO(get_logger(), "I heard: '%s'", msg->data.c_str());
RCLCPP_INFO(get_logger(), "V2X: %d objects detected!", msg->objects.size());
// Send CPM
cp_->send(msg, this, pos_lat_, pos_lon_);
// cp_->send(msg, this, pos_lat_, pos_lon_);
// cp_->updateObjectsStack(msg, this);
app->objectsCallback(msg);
}
void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg)
{
// RCLCPP_INFO(get_logger(), "Ego Position: (%f, %f, %f)", msg->transforms[0].transform.translation.x, msg->transforms[0].transform.translation.y, msg->transforms[0].transform.translation.z);
float x = msg->transforms[0].transform.translation.x;
float y = msg->transforms[0].transform.translation.y;
float z = msg->transforms[0].transform.translation.z;
char mgrs[20];
int zone, prec;
bool northp;
double x_mgrs, y_mgrs;
double lat, lon;
sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y);
RCLCPP_INFO(get_logger(), "MGRS: %s", mgrs);
GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec);
GeographicLib::UTMUPS::Reverse(zone, northp, x_mgrs, y_mgrs, lat, lon);
pos_lat_ = lat;
pos_lon_ = lon;
RCLCPP_INFO(get_logger(), "Ego Position Lat/Lon: %f, %f", lat, lon);
app->tfCallback(msg);
}
}