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

Major updates

This commit is contained in:
Yu Asabe 2021-11-11 00:21:32 +09:00
parent cc438fcb8c
commit e8238701ae
6 changed files with 387 additions and 370 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
include/autoware_v2x/cpm_application.hpp
View File

@ -8,10 +8,13 @@
#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" #include "autoware_perception_msgs/msg/dynamic_object_array.hpp"
#include "autoware_v2x/positioning.hpp" #include "autoware_v2x/positioning.hpp"
class CpmApplication : public Application namespace v2x
{ {
public: class V2XNode;
CpmApplication(rclcpp::Node *node, vanetza::Runtime&); class CpmApplication : public Application
{
public:
CpmApplication(V2XNode *node, vanetza::Runtime &);
PortType port() override; PortType port() override;
void indicate(const DataIndication &, UpPacketPtr) override; void indicate(const DataIndication &, UpPacketPtr) override;
void set_interval(vanetza::Clock::duration); void set_interval(vanetza::Clock::duration);
@ -22,14 +25,8 @@ public:
void updateHeading(double *); void updateHeading(double *);
void send(); void send();
private: struct Object
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 int objectID; // 0-255
rclcpp::Time timestamp; rclcpp::Time timestamp;
double position_x; double position_x;
@ -47,6 +44,15 @@ private:
int timeOfMeasurement; int timeOfMeasurement;
}; };
std::vector<CpmApplication::Object> objectsStack; std::vector<CpmApplication::Object> objectsStack;
std::vector<CpmApplication::Object> receivedObjectsStack;
private:
void schedule_timer();
void on_timer(vanetza::Clock::time_point);
V2XNode *node_;
vanetza::Runtime &runtime_;
vanetza::Clock::duration cpm_interval_;
double ego_x_; double ego_x_;
double ego_y_; double ego_y_;
@ -58,6 +64,7 @@ private:
bool updating_objects_stack_; bool updating_objects_stack_;
bool sending_; bool sending_;
}; };
}
#endif /* CPM_APPLICATION_HPP_EUIC2VFR */ #endif /* CPM_APPLICATION_HPP_EUIC2VFR */

7
include/autoware_v2x/v2x_app.hpp
View File

@ -13,23 +13,26 @@
#include "autoware_v2x/positioning.hpp" #include "autoware_v2x/positioning.hpp"
#include "autoware_v2x/security.hpp" #include "autoware_v2x/security.hpp"
#include "autoware_v2x/router_context.hpp" #include "autoware_v2x/router_context.hpp"
// #include "autoware_v2x/v2x_node.hpp"
namespace v2x namespace v2x
{ {
class V2XNode;
class V2XApp class V2XApp
{ {
public: public:
V2XApp(rclcpp::Node*); V2XApp(V2XNode *);
void start(); void start();
void objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr); void objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr);
void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr); void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr);
CpmApplication *cp; CpmApplication *cp;
// V2XNode *v2x_node;
private: private:
friend class CpmApplication; friend class CpmApplication;
friend class Application; friend class Application;
rclcpp::Node* node_; V2XNode* node_;
bool tf_received_; bool tf_received_;
bool cp_started_; bool cp_started_;
}; };

26
include/autoware_v2x/v2x_node.hpp
View File

@ -1,3 +1,6 @@
#ifndef V2X_NODE_HPP_EUIC2VFR
#define V2X_NODE_HPP_EUIC2VFR
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp" #include "std_msgs/msg/string.hpp"
#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" #include "autoware_perception_msgs/msg/dynamic_object_array.hpp"
@ -19,34 +22,23 @@ namespace v2x
public: public:
explicit V2XNode(const rclcpp::NodeOptions &node_options); explicit V2XNode(const rclcpp::NodeOptions &node_options);
V2XApp *app; V2XApp *app;
void publishObjects(std::vector<CpmApplication::Object> *);
private: private:
void objectsCallback( void objectsCallback(
const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg); const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg);
void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg); void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg);
// friend class CpmApplication;
// friend class Application;
rclcpp::Subscription<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr subscription_; rclcpp::Subscription<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr subscription_;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_pos_; rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_pos_;
rclcpp::Publisher<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr publisher_;
// std::unique_ptr<CpmApplication> cp_; rclcpp::Time received_time;
// std::unique_ptr<Application> app_; bool cpm_received;
// 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_lat_;
double pos_lon_; double pos_lon_;
// vanetza::PositionFix reference_position_;
}; };
} }
#endif

148
src/cpm_application.cpp
View File

@ -2,6 +2,7 @@
#include "autoware_v2x/positioning.hpp" #include "autoware_v2x/positioning.hpp"
#include "autoware_v2x/security.hpp" #include "autoware_v2x/security.hpp"
#include "autoware_v2x/link_layer.hpp" #include "autoware_v2x/link_layer.hpp"
#include "autoware_v2x/v2x_node.hpp"
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include <vanetza/btp/ports.hpp> #include <vanetza/btp/ports.hpp>
@ -13,6 +14,9 @@
#include <iostream> #include <iostream>
#include <sstream> #include <sstream>
#include <exception> #include <exception>
#include <GeographicLib/UTMUPS.hpp>
#include <GeographicLib/MGRS.hpp>
#include <string>
#define _USE_MATH_DEFINES #define _USE_MATH_DEFINES
#include <math.h> #include <math.h>
@ -21,7 +25,9 @@ using namespace vanetza;
using namespace vanetza::facilities; using namespace vanetza::facilities;
using namespace std::chrono; using namespace std::chrono;
CpmApplication::CpmApplication(rclcpp::Node *node, Runtime &rt) : namespace v2x
{
CpmApplication::CpmApplication(V2XNode *node, Runtime &rt) :
node_(node), node_(node),
runtime_(rt), runtime_(rt),
ego_x_(0), ego_x_(0),
@ -33,36 +39,36 @@ CpmApplication::CpmApplication(rclcpp::Node *node, Runtime &rt) :
generationDeltaTime_(0), generationDeltaTime_(0),
updating_objects_stack_(false), updating_objects_stack_(false),
sending_(false) sending_(false)
{ {
RCLCPP_INFO(node_->get_logger(), "CpmApplication started..."); RCLCPP_INFO(node_->get_logger(), "CpmApplication started...");
set_interval(milliseconds(1000)); set_interval(milliseconds(1000));
} }
void CpmApplication::set_interval(Clock::duration interval) void CpmApplication::set_interval(Clock::duration interval)
{ {
cpm_interval_ = interval; cpm_interval_ = interval;
runtime_.cancel(this); runtime_.cancel(this);
schedule_timer(); schedule_timer();
} }
void CpmApplication::schedule_timer() void CpmApplication::schedule_timer()
{ {
runtime_.schedule(cpm_interval_, std::bind(&CpmApplication::on_timer, this, std::placeholders::_1), this); runtime_.schedule(cpm_interval_, std::bind(&CpmApplication::on_timer, this, std::placeholders::_1), this);
} }
void CpmApplication::on_timer(Clock::time_point) void CpmApplication::on_timer(Clock::time_point)
{ {
schedule_timer(); schedule_timer();
send(); send();
} }
CpmApplication::PortType CpmApplication::port() CpmApplication::PortType CpmApplication::port()
{ {
return btp::ports::CPM; return btp::ports::CPM;
} }
void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet) void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet)
{ {
asn1::PacketVisitor<asn1::Cpm> visitor; asn1::PacketVisitor<asn1::Cpm> visitor;
std::shared_ptr<const asn1::Cpm> cpm = boost::apply_visitor(visitor, *packet); std::shared_ptr<const asn1::Cpm> cpm = boost::apply_visitor(visitor, *packet);
if (cpm) if (cpm)
@ -72,55 +78,89 @@ void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr pack
ItsPduHeader_t &header = message->header; ItsPduHeader_t &header = message->header;
CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer; CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer;
double lat = management.referencePosition.latitude; double lat = management.referencePosition.latitude / 1.0e7;
double lon = management.referencePosition.longitude; double lon = management.referencePosition.longitude / 1.0e7;
RCLCPP_INFO(node_->get_logger(), "cpm.(reference position) = %f, %f", lat, lon); RCLCPP_INFO(node_->get_logger(), "cpm.(reference position) = %f, %f", lat, lon);
std::string mgrs;
int zone;
bool northp;
double x, y;
GeographicLib::UTMUPS::Forward(lat, lon, zone, northp, x, y);
GeographicLib::MGRS::Forward(zone, northp, x, y, lat, 5, mgrs);
int x_mgrs = std::stoi(mgrs.substr(5, 5));
int y_mgrs = std::stoi(mgrs.substr(10, 5));
RCLCPP_INFO(node_->get_logger(), "cpm.(RP).mgrs = %s, %d, %d", mgrs.c_str(), x_mgrs, y_mgrs);
// Calculate orientation from Heading
OriginatingVehicleContainer_t &ovc = message->cpm.cpmParameters.stationDataContainer->choice.originatingVehicleContainer;
int heading = ovc.heading.headingValue;
double orientation = 1.5708 - (M_PI * heading / 10) / 180;
RCLCPP_INFO(node_->get_logger(), "cpm: heading = %d, orientation = %f", heading, orientation);
// Get PerceivedObjects
receivedObjectsStack.clear();
PerceivedObjectContainer_t *&poc = message->cpm.cpmParameters.perceivedObjectContainer;
for (int i = 0; i < poc->list.count; ++i)
{
// RCLCPP_INFO(node_->get_logger(), "cpm: %d", poc->list.array[i]->objectID);
CpmApplication::Object object;
double x1 = poc->list.array[i]->xDistance.value;
double y1 = poc->list.array[i]->yDistance.value;
object.position_x = x_mgrs - (cos(orientation) * x1 - sin(orientation) * y1);
object.position_y = y_mgrs - (sin(orientation) * x1 + cos(orientation) * y1);
receivedObjectsStack.push_back(object);
}
node_->publishObjects(&receivedObjectsStack);
} }
else else
{ {
RCLCPP_INFO(node_->get_logger(), "Received broken content"); RCLCPP_INFO(node_->get_logger(), "Received broken content");
} }
} }
void CpmApplication::updateMGRS(double *x, double *y) { void CpmApplication::updateMGRS(double *x, double *y)
RCLCPP_INFO(node_->get_logger(), "Update MGRS"); {
// RCLCPP_INFO(node_->get_logger(), "Update MGRS");
ego_x_ = *x; ego_x_ = *x;
ego_y_ = *y; ego_y_ = *y;
} }
void CpmApplication::updateRP(double *lat, double *lon, double *altitude) void CpmApplication::updateRP(double *lat, double *lon, double *altitude)
{ {
RCLCPP_INFO(node_->get_logger(), "Update RP"); // RCLCPP_INFO(node_->get_logger(), "Update RP");
ego_lat_ = *lat; ego_lat_ = *lat;
ego_lon_ = *lon; ego_lon_ = *lon;
ego_altitude_ = *altitude; ego_altitude_ = *altitude;
} }
void CpmApplication::updateGenerationDeltaTime(int *gdt) void CpmApplication::updateGenerationDeltaTime(int *gdt)
{ {
RCLCPP_INFO(node_->get_logger(), "Update GDT"); // RCLCPP_INFO(node_->get_logger(), "Update GDT");
generationDeltaTime_ = *gdt; generationDeltaTime_ = *gdt;
} }
void CpmApplication::updateHeading(double *yaw) void CpmApplication::updateHeading(double *yaw)
{ {
RCLCPP_INFO(node_->get_logger(), "Update Heading : %f", *yaw); // RCLCPP_INFO(node_->get_logger(), "Update Heading : %f", *yaw);
ego_heading_ = *yaw; ego_heading_ = *yaw;
} }
void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg)
{ {
RCLCPP_INFO(node_->get_logger(), "Update ObjectsStack"); RCLCPP_INFO(node_->get_logger(), "Update ObjectsStack");
updating_objects_stack_ = true; updating_objects_stack_ = true;
if (sending_) if (sending_)
{ {
RCLCPP_INFO(node_->get_logger(), "updateObjectsStack Skipped...");
return; return;
} }
if (msg->objects.size() > 0) if (msg->objects.size() > 0)
{ {
RCLCPP_INFO(node_->get_logger(), "At least 1 object detected"); // RCLCPP_INFO(node_->get_logger(), "At least 1 object detected");
// Initialize ObjectsStack // Initialize ObjectsStack
// std::vector<CpmApplication::Object> objectsStack; // std::vector<CpmApplication::Object> objectsStack;
@ -142,7 +182,7 @@ void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::Dyn
object.orientation_w = obj.state.pose_covariance.pose.orientation.w; object.orientation_w = obj.state.pose_covariance.pose.orientation.w;
object.xDistance = (int)((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100; object.xDistance = (int)((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100;
object.yDistance = (int)((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100; object.yDistance = (int)((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100;
RCLCPP_INFO(node_->get_logger(), "xDistance: %d, yDistance: %d", object.xDistance, object.yDistance); // RCLCPP_INFO(node_->get_logger(), "xDistance: %d, yDistance: %d", object.xDistance, object.yDistance);
object.timeOfMeasurement = 100; object.timeOfMeasurement = 100;
objectsStack.push_back(object); objectsStack.push_back(object);
++i; ++i;
@ -150,12 +190,12 @@ void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::Dyn
} }
RCLCPP_INFO(node_->get_logger(), "%d objects added to objectsStack", objectsStack.size()); RCLCPP_INFO(node_->get_logger(), "%d objects added to objectsStack", objectsStack.size());
updating_objects_stack_ = false; updating_objects_stack_ = false;
} }
void CpmApplication::send() void CpmApplication::send()
{ {
sending_ = true; sending_ = true;
if (!updating_objects_stack_) if (!updating_objects_stack_ && objectsStack.size() > 0)
{ {
RCLCPP_INFO(node_->get_logger(), "Sending CPM..."); RCLCPP_INFO(node_->get_logger(), "Sending CPM...");
@ -203,16 +243,13 @@ void CpmApplication::send()
ovc.speed.speedValue = 0; ovc.speed.speedValue = 0;
ovc.speed.speedConfidence = 1; ovc.speed.speedConfidence = 1;
// ovc.heading.headingValue = (int) (1.5708 - ego_heading_) * M_PI / 180; // ovc.heading.headingValue = (int) (1.5708 - ego_heading_) * M_PI / 180;
RCLCPP_INFO(node_->get_logger(), "headingValue..."); // RCLCPP_INFO(node_->get_logger(), "headingValue...");
ovc.heading.headingValue = (int) std::fmod((1.5708-ego_heading_) * 180 / M_PI, 360.0) * 10; ovc.heading.headingValue = (int)std::fmod((1.5708 - ego_heading_) * 180 / M_PI, 360.0) * 10;
ovc.heading.headingConfidence = 1; ovc.heading.headingConfidence = 1;
RCLCPP_INFO(node_->get_logger(), "PerceviedObjectContainer..."); // RCLCPP_INFO(node_->get_logger(), "PerceviedObjectContainer...");
PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer; PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer;
poc = vanetza::asn1::allocate<PerceivedObjectContainer_t>(); 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) for (CpmApplication::Object object : objectsStack)
{ {
@ -229,32 +266,22 @@ void CpmApplication::send()
pObj->ySpeed.confidence = 1; pObj->ySpeed.confidence = 1;
ASN_SEQUENCE_ADD(poc, pObj); ASN_SEQUENCE_ADD(poc, pObj);
RCLCPP_INFO(node_->get_logger(), "ADD: %d, %d, %d, %d, %d, %d", pObj->objectID, pObj->timeOfMeasurement, pObj->xDistance.value, pObj->yDistance.value, pObj->xSpeed.value, pObj->ySpeed.value); // RCLCPP_INFO(node_->get_logger(), "ADD: %d, %d, %d, %d, %d, %d", pObj->objectID, pObj->timeOfMeasurement, pObj->xDistance.value, pObj->yDistance.value, pObj->xSpeed.value, pObj->ySpeed.value);
} }
RCLCPP_INFO(node_->get_logger(), "1");
Application::DownPacketPtr packet{new DownPacket()}; Application::DownPacketPtr packet{new DownPacket()};
RCLCPP_INFO(node_->get_logger(), "2");
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()}; std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
RCLCPP_INFO(node_->get_logger(), "3");
// std::shared_ptr<asn1::Cpm> message_p = std::make_shared<asn1::Cpm>(message); // 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)}; // std::unique_ptr<convertible::byte_buffer> buffer { new convertible::byte_buffer_impl<asn1::Cpm>(&message)};
payload->layer(OsiLayer::Application) = std::move(message); payload->layer(OsiLayer::Application) = std::move(message);
RCLCPP_INFO(node_->get_logger(), "4");
Application::DataRequest request; Application::DataRequest request;
request.its_aid = aid::CP; request.its_aid = aid::CP;
request.transport_type = geonet::TransportType::SHB; request.transport_type = geonet::TransportType::SHB;
request.communication_profile = geonet::CommunicationProfile::ITS_G5; request.communication_profile = geonet::CommunicationProfile::ITS_G5;
// RCLCPP_INFO(node->get_logger(), "Packet Size: %d", payload->size()); // RCLCPP_INFO(node->get_logger(), "Packet Size: %d", payload->size());
// RCLCPP_INFO(node->get_logger(), "Going to Application::request..."); // RCLCPP_INFO(node->get_logger(), "Going to Application::request...");
RCLCPP_INFO(node_->get_logger(), "5");
Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
RCLCPP_INFO(node_->get_logger(), "6");
if (!confirm.accepted()) if (!confirm.accepted())
{ {
throw std::runtime_error("CPM application data request failed"); throw std::runtime_error("CPM application data request failed");
@ -262,4 +289,5 @@ void CpmApplication::send()
} }
sending_ = false; sending_ = false;
// RCLCPP_INFO(node->get_logger(), "Application::request END"); // RCLCPP_INFO(node->get_logger(), "Application::request END");
}
} }

19
src/v2x_app.cpp
View File

@ -1,4 +1,5 @@
#include "autoware_v2x/v2x_app.hpp" #include "autoware_v2x/v2x_app.hpp"
#include "autoware_v2x/v2x_node.hpp"
#include "autoware_v2x/time_trigger.hpp" #include "autoware_v2x/time_trigger.hpp"
#include "autoware_v2x/router_context.hpp" #include "autoware_v2x/router_context.hpp"
#include "autoware_v2x/positioning.hpp" #include "autoware_v2x/positioning.hpp"
@ -29,7 +30,7 @@ using namespace std::chrono;
namespace v2x namespace v2x
{ {
V2XApp::V2XApp(rclcpp::Node *node) : V2XApp::V2XApp(V2XNode *node) :
node_(node), node_(node),
tf_received_(false), tf_received_(false),
cp_started_(false) cp_started_(false)
@ -37,14 +38,14 @@ namespace v2x
} }
void V2XApp::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { void V2XApp::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) {
RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received"); // RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received");
if (tf_received_ && cp_started_) { if (tf_received_ && cp_started_) {
cp->updateObjectsStack(msg); cp->updateObjectsStack(msg);
} }
} }
void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) { void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
RCLCPP_INFO(node_->get_logger(), "V2XApp: tf msg received"); // RCLCPP_INFO(node_->get_logger(), "V2XApp: tf msg received");
tf_received_ = true; tf_received_ = true;
double x = msg->transforms[0].transform.translation.x; double x = msg->transforms[0].transform.translation.x;
@ -71,15 +72,15 @@ namespace v2x
double x_mgrs, y_mgrs; double x_mgrs, y_mgrs;
double lat, lon; double lat, lon;
sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y); sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y);
RCLCPP_INFO(node_->get_logger(), "MGRS: %s", mgrs); // RCLCPP_INFO(node_->get_logger(), "MGRS: %s", mgrs);
GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec); GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec);
GeographicLib::UTMUPS::Reverse(zone, northp, x_mgrs, y_mgrs, lat, lon); 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 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, %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(), "Ego Orientation: %f %f %f", roll, pitch, yaw);
RCLCPP_INFO(node_->get_logger(), "Timestamp: %d, GDT: %d", timestamp, gdt); // RCLCPP_INFO(node_->get_logger(), "Timestamp: %d, GDT: %d", timestamp, gdt);
if (cp && cp_started_) { if (cp && cp_started_) {
cp->updateMGRS(&x, &y); cp->updateMGRS(&x, &y);
@ -95,7 +96,7 @@ namespace v2x
boost::asio::io_service io_service; boost::asio::io_service io_service;
TimeTrigger trigger(io_service); TimeTrigger trigger(io_service);
const char* device_name = "wlp4s0"; const char* device_name = "vmnet1";
EthernetDevice device(device_name); EthernetDevice device(device_name);
vanetza::MacAddress mac_address = device.address(); vanetza::MacAddress mac_address = device.address();

112
src/v2x_node.cpp
View File

@ -25,88 +25,74 @@ using namespace std::chrono;
namespace v2x namespace v2x
{ {
V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options), received_time(this->now()), cpm_received(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(this, runtime_)),
// context_(mib_, trigger_, *positioning_, security_.get()),
// app_()
{ {
using std::placeholders::_1; using std::placeholders::_1;
subscription_ = this->create_subscription<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/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)); subscription_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));
RCLCPP_INFO(get_logger(), "V2X Node Launched");
// device_name_ = "wlp4s0"; publisher_ = create_publisher<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/objects", rclcpp::QoS{10});
// device_(device_name_);
// mac_address_ = device_.address();
// 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;
// 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_));
app = new V2XApp(this); app = new V2XApp(this);
boost::thread v2xApp(boost::bind(&V2XApp::start, app)); boost::thread v2xApp(boost::bind(&V2XApp::start, app));
// boost::thread v2xApp(V2XApp);
// boost::thread v2xApp(&V2XApp, this);
// // Print MAC Address to logger RCLCPP_INFO(get_logger(), "V2X Node Launched");
// std::stringstream sout; }
// sout << mac_address;
// RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str()); void V2XNode::publishObjects(std::vector<CpmApplication::Object> *objectsStack) {
autoware_perception_msgs::msg::DynamicObjectArray output_dynamic_object_msg;
std_msgs::msg::Header header;
rclcpp::Time current_time = this->now();
received_time = current_time;
output_dynamic_object_msg.header.frame_id = "map";
output_dynamic_object_msg.header.stamp = current_time;
for (CpmApplication::Object obj : *objectsStack) {
autoware_perception_msgs::msg::DynamicObject object;
autoware_perception_msgs::msg::Semantic semantic;
autoware_perception_msgs::msg::Shape shape;
autoware_perception_msgs::msg::State state;
semantic.type = autoware_perception_msgs::msg::Semantic::CAR;
semantic.confidence = 0.99;
shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
shape.dimensions.x = 2;
shape.dimensions.y = 5.0;
shape.dimensions.z = 2.5;
state.pose_covariance.pose.position.x = obj.position_x;
state.pose_covariance.pose.position.y = obj.position_y;
state.pose_covariance.pose.position.z = 0.2;
object.semantic = semantic;
object.shape = shape;
object.state = state;
output_dynamic_object_msg.objects.push_back(object);
}
cpm_received = true;
publisher_->publish(output_dynamic_object_msg);
} }
void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg)
{ {
// RCLCPP_INFO(get_logger(), "I heard: '%s'", msg->data.c_str()); rclcpp::Time current_time = this->now();
RCLCPP_INFO(get_logger(), "V2X: %d objects detected!", msg->objects.size()); // RCLCPP_INFO(get_logger(), "%ld", current_time.nanoseconds() - received_time.nanoseconds());
// Send CPM if (cpm_received) {
// cp_->send(msg, this, pos_lat_, pos_lon_); // if time difference is more than 10ms
// cp_->updateObjectsStack(msg, this); if (current_time - received_time > rclcpp::Duration(std::chrono::nanoseconds(10000000))) {
app->objectsCallback(msg); app->objectsCallback(msg);
}
cpm_received = false;
} else {
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);
app->tfCallback(msg); app->tfCallback(msg);
} }
} }