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

Update to include packet reflection

This commit is contained in:
yuasabe 2022-01-21 22:07:33 +09:00
parent c91415eba9
commit e674134bba
3 changed files with 125 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/autoware_v2x/cpm_application.hpp
View File

@ -69,6 +69,8 @@ namespace v2x
bool updating_objects_stack_; bool updating_objects_stack_;
bool sending_; bool sending_;
bool is_sender_;
bool reflect_packet_;
}; };
} }

257
src/cpm_application.cpp
View File

@ -41,43 +41,43 @@ namespace v2x
ego_heading_(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::Time current_time = node_->now();
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r2 %ld", current_time.nanoseconds()); 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;
@ -155,40 +155,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) {
{
// 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");
ego_lat_ = *lat; ego_lat_ = *lat;
ego_lon_ = *lon; ego_lon_ = *lon;
ego_altitude_ = *altitude; 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; // ETSI-epoch milliseconds timestamp gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
} }
void CpmApplication::updateHeading(double *yaw) void CpmApplication::updateHeading(double *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) {
{
updating_objects_stack_ = true; updating_objects_stack_ = true;
if (sending_) { if (sending_) {
@ -259,134 +271,107 @@ namespace v2x
} }
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_lat_ * units::degree;
// management.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 1.0; fix.longitude = ego_lon_ * 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 / M_PI) + 90.0) * 10);
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");
rclcpp::Time current_time = node_->now();
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds());
} }
} }