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

Merge pull request #3 from tlab-wide/fix/position 

Simple refactoring
This commit is contained in:
Yu Asabe 2022-02-24 00:54:15 +09:00 committed by GitHub
parent 7e43b2500a eb6a9b2040
commit d23ad2dc96
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 193 additions and 205 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
include/autoware_v2x/cpm_application.hpp
View File

@ -58,17 +58,24 @@ namespace v2x
vanetza::Runtime &runtime_; vanetza::Runtime &runtime_;
vanetza::Clock::duration cpm_interval_; vanetza::Clock::duration cpm_interval_;
double ego_x_; struct Ego_station {
double ego_y_; double mgrs_x;
double ego_lat_; double mgrs_y;
double ego_lon_; double latitude;
double ego_altitude_; double longitude;
double ego_heading_; double altitude;
double heading;
};
Ego_station ego_;
int generationDeltaTime_; int generationDeltaTime_;
long long gdt_timestamp_; long long gdt_timestamp_;
bool updating_objects_stack_; bool updating_objects_stack_;
bool sending_; bool sending_;
bool is_sender_;
bool reflect_packet_;
}; };
} }

3
include/autoware_v2x/v2x_node.hpp
View File

@ -33,9 +33,6 @@ namespace v2x
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_; rclcpp::Publisher<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr publisher_;
rclcpp::Time received_time;
bool cpm_received;
double pos_lat_; double pos_lat_;
double pos_lon_; double pos_lon_;
}; };

2
launch/v2x.launch.xml
View File

@ -1,6 +1,6 @@
<launch> <launch>
<arg name="network_interface" default="wlp5s0"/> <arg name="network_interface" default="wlp5s0"/>
<node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen" emulate_tty="true"> <node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen">
<param name="network_interface" value="$(var network_interface)"/> <param name="network_interface" value="$(var network_interface)"/>
</node> </node>
</launch> </launch>

314
src/cpm_application.cpp
View File

@ -28,54 +28,51 @@ using namespace vanetza;
using namespace vanetza::facilities; using namespace vanetza::facilities;
using namespace std::chrono; using namespace std::chrono;
namespace v2x namespace v2x {
{
CpmApplication::CpmApplication(V2XNode *node, Runtime &rt) : CpmApplication::CpmApplication(V2XNode *node, Runtime &rt) :
node_(node), node_(node),
runtime_(rt), runtime_(rt),
ego_x_(0), ego_(),
ego_y_(0),
ego_lat_(0),
ego_lon_(0),
ego_altitude_(0),
ego_heading_(0),
generationDeltaTime_(0), generationDeltaTime_(0),
updating_objects_stack_(false), updating_objects_stack_(false),
sending_(false) sending_(false),
is_sender_(true),
reflect_packet_(false)
{ {
RCLCPP_INFO(node_->get_logger(), "CpmApplication started..."); RCLCPP_INFO(node_->get_logger(), "CpmApplication started...");
set_interval(milliseconds(100)); set_interval(milliseconds(100));
} }
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) {
RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received decodable CPM content"); RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] Received decodable CPM content");
rclcpp::Time current_time = node_->now();
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r1 %ld", current_time.nanoseconds());
asn1::Cpm message = *cpm; asn1::Cpm message = *cpm;
ItsPduHeader_t &header = message->header; ItsPduHeader_t &header = message->header;
@ -84,7 +81,10 @@ namespace v2x
const auto time_now = duration_cast<milliseconds> (runtime_.now().time_since_epoch()); const auto time_now = duration_cast<milliseconds> (runtime_.now().time_since_epoch());
uint16_t gdt = time_now.count(); uint16_t gdt = time_now.count();
int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536; int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536;
RCLCPP_INFO(node_->get_logger(), "[indicate] gdt: %ld,%u,%d", gdt_cpm, gdt, gdt_diff); RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] GDT_CPM: %ld", gdt_cpm);
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] GDT: %u", gdt);
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_R1R2: %d", gdt_diff);
CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer; CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer;
double lat = management.referencePosition.latitude / 1.0e7; double lat = management.referencePosition.latitude / 1.0e7;
@ -149,41 +149,52 @@ namespace v2x
} else { } else {
RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC"); RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC");
} }
if (reflect_packet_) {
Application::DownPacketPtr packet{new DownPacket()};
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("[CpmApplication::indicate] Packet reflection failed");
}
}
} else { } else {
RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received broken content"); RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received broken content");
} }
} }
void CpmApplication::updateMGRS(double *x, double *y) void CpmApplication::updateMGRS(double *x, double *y) {
{ ego_.mgrs_x = *x;
// RCLCPP_INFO(node_->get_logger(), "Update MGRS"); ego_.mgrs_y = *y;
ego_x_ = *x;
ego_y_ = *y;
} }
void CpmApplication::updateRP(double *lat, double *lon, double *altitude) void CpmApplication::updateRP(double *lat, double *lon, double *altitude) {
{ ego_.latitude = *lat;
// RCLCPP_INFO(node_->get_logger(), "Update RP"); ego_.longitude = *lon;
ego_lat_ = *lat; ego_.altitude = *altitude;
ego_lon_ = *lon;
ego_altitude_ = *altitude;
} }
void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) {
{
generationDeltaTime_ = *gdt; generationDeltaTime_ = *gdt;
gdt_timestamp_ = *gdt_timestamp; gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
// RCLCPP_INFO(node_->get_logger(), "[updateGDT] %d %lu", generationDeltaTime_, gdt_timestamp_);
} }
void CpmApplication::updateHeading(double *yaw) void CpmApplication::updateHeading(double *yaw) {
{ ego_.heading = *yaw;
// RCLCPP_INFO(node_->get_logger(), "Update Heading : %f", *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) {
{
updating_objects_stack_ = true; updating_objects_stack_ = true;
if (sending_) { if (sending_) {
@ -209,8 +220,10 @@ namespace v2x
object.shape_x = std::lround(obj.shape.dimensions.x * 10.0); object.shape_x = std::lround(obj.shape.dimensions.x * 10.0);
object.shape_y = std::lround(obj.shape.dimensions.y * 10.0); object.shape_y = std::lround(obj.shape.dimensions.y * 10.0);
object.shape_z = std::lround(obj.shape.dimensions.z * 10.0); object.shape_z = std::lround(obj.shape.dimensions.z * 10.0);
object.xDistance = std::lround(((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100.0); // object.xDistance = std::lround(((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100.0);
object.yDistance = std::lround(((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100.0); // object.yDistance = std::lround(((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100.0);
object.xDistance = std::lround(((object.position_x - ego_.mgrs_x) * cos(-ego_.heading) - (object.position_y - ego_.mgrs_y) * sin(-ego_.heading)) * 100.0);
object.yDistance = std::lround(((object.position_x - ego_.mgrs_x) * sin(-ego_.heading) + (object.position_y - ego_.mgrs_y) * cos(-ego_.heading)) * 100.0);
if (object.xDistance < -132768 || object.xDistance > 132767) { if (object.xDistance < -132768 || object.xDistance > 132767) {
continue; continue;
} }
@ -230,152 +243,131 @@ namespace v2x
object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600 object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600
} }
long long timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec; long long msg_timestamp_sec = msg->header.stamp.sec;
object.timeOfMeasurement = (gdt_timestamp_ - timestamp) / 1e6; long long msg_timestamp_nsec = msg->header.stamp.nanosec;
msg_timestamp_sec -= 1072915200; // convert to etsi-epoch
long long msg_timestamp_msec = msg_timestamp_sec * 1000 + msg_timestamp_nsec / 1000000;
// long long timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec;
object.timeOfMeasurement = gdt_timestamp_ - msg_timestamp_msec;
// RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] %ld %ld %d", gdt_timestamp_, timestamp, object.timeOfMeasurement); // RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] %ld %ld %d", gdt_timestamp_, timestamp, object.timeOfMeasurement);
if (object.timeOfMeasurement < -1500 || object.timeOfMeasurement > 1500) { if (object.timeOfMeasurement < -1500 || object.timeOfMeasurement > 1500) {
RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] timeOfMeasurement out of bounds: %d", object.timeOfMeasurement);
continue; continue;
} }
objectsStack.push_back(object); objectsStack.push_back(object);
++i; ++i;
// RCLCPP_INFO(node_->get_logger(), "Added to stack: %f %f %f", obj.shape.dimensions.x, obj.shape.dimensions.y, obj.shape.dimensions.z);
// RCLCPP_INFO(node_->get_logger(), "Added to stack: %f %f %f", obj.shape.dimensions.x * 10.0, obj.shape.dimensions.y * 10.0, obj.shape.dimensions.z * 10.0);
// RCLCPP_INFO(node_->get_logger(), "Added to stack: %d %d %d", std::lround(obj.shape.dimensions.x * 10.0), std::lround(obj.shape.dimensions.y * 10.0), std::lround(obj.shape.dimensions.z * 10.0));
RCLCPP_INFO(node_->get_logger(), "Added to stack: #%d (%d, %d) (%d, %d) (%d, %d, %d) (%f: %d)", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_x, object.shape_y, object.shape_z, yaw, object.yawAngle); RCLCPP_INFO(node_->get_logger(), "Added to stack: #%d (%d, %d) (%d, %d) (%d, %d, %d) (%f: %d)", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_x, object.shape_y, object.shape_z, yaw, object.yawAngle);
} }
} }
RCLCPP_INFO(node_->get_logger(), "ObjectsStack: %d objects", objectsStack.size()); RCLCPP_INFO(node_->get_logger(), "ObjectsStack: %d objects", objectsStack.size());
rclcpp::Time current_time = node_->now();
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] [measure] T_objstack_updated %ld", current_time.nanoseconds());
updating_objects_stack_ = false; updating_objects_stack_ = false;
} }
void CpmApplication::send() void CpmApplication::send() {
{
sending_ = true;
RCLCPP_INFO(node_->get_logger(), "[SEND] Sending CPM..."); if (is_sender_) {
sending_ = true;
vanetza::asn1::Cpm message; RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM...");
// ITS PDU Header vanetza::asn1::Cpm message;
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()); // ITS PDU Header
// uint16_t gen_delta_time = time_now.count(); ItsPduHeader_t &header = message->header;
header.protocolVersion = 1;
header.messageID = 14;
header.stationID = 1;
CollectivePerceptionMessage_t &cpm = message->cpm; CollectivePerceptionMessage_t &cpm = message->cpm;
cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec;
// auto position = positioning->position_specify(pos_lat, pos_lon); // Set GenerationDeltaTime
cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec;
CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer; CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer;
management.stationType = StationType_passengerCar; management.stationType = StationType_passengerCar;
// management.referencePosition.latitude = pos_lat; PositionFix fix;
// management.referencePosition.longitude = pos_lon; fix.latitude = ego_.latitude * units::degree;
// management.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 1.0; fix.longitude = ego_.longitude * units::degree;
// management.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 1.0; fix.confidence.semi_major = 1.0 * units::si::meter;
PositionFix fix; fix.confidence.semi_minor = fix.confidence.semi_major;
// fix.timestamp = time_now; copy(fix, management.referencePosition);
fix.latitude = ego_lat_ * units::degree; cpm.cpmParameters.numberOfPerceivedObjects = objectsStack.size();
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; StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer;
// cpm.cpmParameters.perceivedObjectContainer = NULL; sdc = vanetza::asn1::allocate<StationDataContainer_t>();
cpm.cpmParameters.numberOfPerceivedObjects = objectsStack.size(); sdc->present = StationDataContainer_PR_originatingVehicleContainer;
OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer;
ovc.speed.speedValue = 0;
ovc.speed.speedConfidence = 1;
int heading = std::lround(((-ego_.heading * 180.0 / M_PI) + 90.0) * 10.0);
if (heading < 0) heading += 3600;
ovc.heading.headingValue = heading;
ovc.heading.headingConfidence = 1;
StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer; if (objectsStack.size() > 0) {
sdc = vanetza::asn1::allocate<StationDataContainer_t>(); PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer;
// RCLCPP_INFO(node->get_logger(), "Allocated sdc"); poc = vanetza::asn1::allocate<PerceivedObjectContainer_t>();
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;
// RCLCPP_INFO(node_->get_logger(), "headingValue...");
// ovc.heading.headingValue = (int)std::fmod((1.5708 - ego_heading_) * 180 / M_PI, 360.0) * 10;
int heading = std::lround(((-ego_heading_ * 180 / M_PI) + 90.0) * 10);
if (heading < 0) heading += 3600;
ovc.heading.headingValue = heading;
ovc.heading.headingConfidence = 1;
// RCLCPP_INFO(node_->get_logger(), "[SEND] headingValue: %f %d", ego_heading_, ovc.heading.headingValue);
if (objectsStack.size() > 0) { for (CpmApplication::Object object : objectsStack) {
PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer; PerceivedObject *pObj = vanetza::asn1::allocate<PerceivedObject>();
poc = vanetza::asn1::allocate<PerceivedObjectContainer_t>(); 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;
for (CpmApplication::Object object : objectsStack) { pObj->planarObjectDimension1 = vanetza::asn1::allocate<ObjectDimension_t>();
// if (object.xDistance > 10000) continue; pObj->planarObjectDimension2 = vanetza::asn1::allocate<ObjectDimension_t>();
// if (object.yDistance > 10000) continue; pObj->verticalObjectDimension = vanetza::asn1::allocate<ObjectDimension_t>();
PerceivedObject *pObj = vanetza::asn1::allocate<PerceivedObject>(); (*(pObj->planarObjectDimension1)).value = object.shape_y;
pObj->objectID = object.objectID; (*(pObj->planarObjectDimension1)).confidence = 1;
pObj->timeOfMeasurement = object.timeOfMeasurement; (*(pObj->planarObjectDimension2)).value = object.shape_x;
pObj->xDistance.value = object.xDistance; (*(pObj->planarObjectDimension2)).confidence = 1;
pObj->xDistance.confidence = 1; (*(pObj->verticalObjectDimension)).value = object.shape_z;
pObj->yDistance.value = object.yDistance; (*(pObj->verticalObjectDimension)).confidence = 1;
pObj->yDistance.confidence = 1;
pObj->xSpeed.value = object.xSpeed;
pObj->xSpeed.confidence = 1;
pObj->ySpeed.value = object.ySpeed;
pObj->ySpeed.confidence = 1;
pObj->planarObjectDimension1 = vanetza::asn1::allocate<ObjectDimension_t>(); pObj->yawAngle = vanetza::asn1::allocate<CartesianAngle>();
pObj->planarObjectDimension2 = vanetza::asn1::allocate<ObjectDimension_t>(); (*(pObj->yawAngle)).value = object.yawAngle;
pObj->verticalObjectDimension = vanetza::asn1::allocate<ObjectDimension_t>(); (*(pObj->yawAngle)).confidence = 1;
(*(pObj->planarObjectDimension1)).value = object.shape_y; RCLCPP_INFO(node_->get_logger(), "[SEND] Added: #%d (%d, %d) (%d, %d) (%d, %d, %d) %d", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_y, object.shape_x, object.shape_z, object.yawAngle);
(*(pObj->planarObjectDimension1)).confidence = 1;
(*(pObj->planarObjectDimension2)).value = object.shape_x;
(*(pObj->planarObjectDimension2)).confidence = 1;
(*(pObj->verticalObjectDimension)).value = object.shape_z;
(*(pObj->verticalObjectDimension)).confidence = 1;
pObj->yawAngle = vanetza::asn1::allocate<CartesianAngle>(); ASN_SEQUENCE_ADD(poc, pObj);
(*(pObj->yawAngle)).value = object.yawAngle; }
(*(pObj->yawAngle)).confidence = 1; } else {
cpm.cpmParameters.perceivedObjectContainer = NULL;
RCLCPP_INFO(node_->get_logger(), "[SEND] Added: #%d (%d, %d) (%d, %d) (%d, %d, %d) %d", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_y, object.shape_x, object.shape_z, object.yawAngle); RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Empty POC");
ASN_SEQUENCE_ADD(poc, pObj);
} }
} else {
cpm.cpmParameters.perceivedObjectContainer = NULL; Application::DownPacketPtr packet{new DownPacket()};
RCLCPP_INFO(node_->get_logger(), "[SEND] Empty POC"); 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("[CpmApplication::send] CPM application data request failed");
}
sending_ = false;
rclcpp::Time current_time = node_->now();
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds());
} }
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;
Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
if (!confirm.accepted()) {
throw std::runtime_error("[SEND] CPM application data request failed");
}
// try {
// Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
// if (!confirm.accepted()) {
// throw std::runtime_error("CPM application data request failed");
// }
// } catch (...) {
// RCLCPP_INFO(node_->get_logger(), "Request Failed");
// }
sending_ = false;
// RCLCPP_INFO(node->get_logger(), "Application::request END");
} }
} }

18
src/v2x_app.cpp
View File

@ -40,13 +40,16 @@ 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_) {
RCLCPP_WARN(node_->get_logger(), "[V2XApp::objectsCallback] tf not received yet");
}
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");
tf_received_ = true; tf_received_ = true;
double x = msg->transforms[0].transform.translation.x; double x = msg->transforms[0].transform.translation.x;
@ -57,12 +60,8 @@ namespace v2x
long long timestamp_nsec = msg->transforms[0].header.stamp.nanosec; // nanoseconds long long timestamp_nsec = msg->transforms[0].header.stamp.nanosec; // nanoseconds
timestamp_sec -= 1072915200; // convert to etsi-epoch timestamp_sec -= 1072915200; // convert to etsi-epoch
long long timestamp_msec = timestamp_sec * 1000 + timestamp_nsec / 1000000; long long timestamp_msec = timestamp_sec * 1000 + timestamp_nsec / 1000000;
// long long gdt_timestamp = gdt_timestamp_sec * 1e9 + gdt_timestamp_nsec; // nanoseconds
// long long gdt_timestamp_msec = gdt_timestamp / 1000000;
// long long gdt_timestamp_msec_etsi_epoch = gdt_timestamp_msec - 1072915200000;
// int gdt = gdt_timestamp_msec_etsi_epoch % 65536; // milliseconds
int gdt = timestamp_msec % 65536; int gdt = timestamp_msec % 65536;
RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec); // RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec);
double rot_x = msg->transforms[0].transform.rotation.x; double rot_x = msg->transforms[0].transform.rotation.x;
double rot_y = msg->transforms[0].transform.rotation.y; double rot_y = msg->transforms[0].transform.rotation.y;
@ -75,23 +74,16 @@ namespace v2x
double roll, pitch, yaw; double roll, pitch, yaw;
matrix.getRPY(roll, pitch, yaw); matrix.getRPY(roll, pitch, yaw);
char mgrs[20]; char mgrs[20];
int zone, prec; int zone, prec;
bool northp; bool northp;
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);
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 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_) { if (cp && cp_started_) {
cp->updateMGRS(&x, &y); cp->updateMGRS(&x, &y);
cp->updateRP(&lat, &lon, &z); cp->updateRP(&lat, &lon, &z);

38
src/v2x_node.cpp
View File

@ -25,14 +25,13 @@ using namespace std::chrono;
namespace v2x namespace v2x
{ {
V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options), received_time(this->now()), cpm_received(false) V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) {
{
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));
publisher_ = create_publisher<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/objects", rclcpp::QoS{10}); publisher_ = create_publisher<autoware_perception_msgs::msg::DynamicObjectArray>("/v2x/cpm/objects", rclcpp::QoS{10});
this->declare_parameter<std::string>("network_interface", "vmnet1"); this->declare_parameter<std::string>("network_interface", "vmnet1");
@ -40,13 +39,16 @@ namespace v2x
boost::thread v2xApp(boost::bind(&V2XApp::start, app)); boost::thread v2xApp(boost::bind(&V2XApp::start, app));
RCLCPP_INFO(get_logger(), "V2X Node Launched"); RCLCPP_INFO(get_logger(), "V2X Node Launched");
rclcpp::Time current_time = this->now();
RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] [measure] T_R1 %ld", current_time.nanoseconds());
} }
void V2XNode::publishObjects(std::vector<CpmApplication::Object> *objectsStack) { void V2XNode::publishObjects(std::vector<CpmApplication::Object> *objectsStack) {
autoware_perception_msgs::msg::DynamicObjectArray output_dynamic_object_msg; autoware_perception_msgs::msg::DynamicObjectArray output_dynamic_object_msg;
std_msgs::msg::Header header; std_msgs::msg::Header header;
rclcpp::Time current_time = this->now(); rclcpp::Time current_time = this->now();
received_time = current_time;
output_dynamic_object_msg.header.frame_id = "map"; output_dynamic_object_msg.header.frame_id = "map";
output_dynamic_object_msg.header.stamp = current_time; output_dynamic_object_msg.header.stamp = current_time;
@ -79,27 +81,25 @@ namespace v2x
output_dynamic_object_msg.objects.push_back(object); output_dynamic_object_msg.objects.push_back(object);
} }
cpm_received = true;
current_time = this->now();
RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds());
publisher_->publish(output_dynamic_object_msg); 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::Time current_time = this->now(); rclcpp::Time current_time = this->now();
// RCLCPP_INFO(get_logger(), "%ld", current_time.nanoseconds() - received_time.nanoseconds()); rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg.
if (cpm_received) { RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] %d objects", msg->objects.size());
// if time difference is more than 10ms
if (current_time - received_time > rclcpp::Duration(std::chrono::nanoseconds(10000000))) { // Measuring T_A1R1
app->objectsCallback(msg); RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj %ld", msg_time.nanoseconds());
} RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj_receive %ld", current_time.nanoseconds());
cpm_received = false; app->objectsCallback(msg);
} else {
app->objectsCallback(msg);
}
} }
void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
{
app->tfCallback(msg); app->tfCallback(msg);
} }
} }