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

fix bug

This commit is contained in:
Yu Asabe 2021-11-17 17:58:41 +09:00
parent 13fde9edd9
commit 56a06b388e
1 changed files with 40 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/cpm_application.cpp
View File

@ -74,9 +74,8 @@ namespace v2x
{ {
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(), "Received decodable CPM content");
asn1::Cpm message = *cpm; asn1::Cpm message = *cpm;
ItsPduHeader_t &header = message->header; ItsPduHeader_t &header = message->header;
@ -104,10 +103,13 @@ namespace v2x
// Get PerceivedObjects // Get PerceivedObjects
receivedObjectsStack.clear(); receivedObjectsStack.clear();
PerceivedObjectContainer_t *&poc = message->cpm.cpmParameters.perceivedObjectContainer; PerceivedObjectContainer_t *&poc = message->cpm.cpmParameters.perceivedObjectContainer;
for (int i = 0; i < poc->list.count; ++i)
{ if (poc != NULL) {
// RCLCPP_INFO(node_->get_logger(), "cpm: %d", poc->list.array[i]->objectID); for (int i = 0; i < poc->list.count; ++i) {
RCLCPP_INFO(node_->get_logger(), "[INDICATE] Object: #%d", poc->list.array[i]->objectID);
CpmApplication::Object object; CpmApplication::Object object;
double x1 = poc->list.array[i]->xDistance.value; double x1 = poc->list.array[i]->xDistance.value;
double y1 = poc->list.array[i]->yDistance.value; double y1 = poc->list.array[i]->yDistance.value;
@ -135,10 +137,11 @@ namespace v2x
receivedObjectsStack.push_back(object); receivedObjectsStack.push_back(object);
} }
node_->publishObjects(&receivedObjectsStack); node_->publishObjects(&receivedObjectsStack);
} else {
RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC");
} }
else } else {
{ RCLCPP_INFO(node_->get_logger(), "[INDICATE] Received broken content");
RCLCPP_INFO(node_->get_logger(), "Received broken content");
} }
} }
@ -229,7 +232,7 @@ namespace v2x
{ {
sending_ = true; sending_ = true;
RCLCPP_INFO(node_->get_logger(), "Sending CPM..."); RCLCPP_INFO(node_->get_logger(), "[SEND] Sending CPM...");
vanetza::asn1::Cpm message; vanetza::asn1::Cpm message;
@ -313,13 +316,13 @@ namespace v2x
(*(pObj->yawAngle)).value = object.yawAngle; (*(pObj->yawAngle)).value = object.yawAngle;
(*(pObj->yawAngle)).confidence = 1; (*(pObj->yawAngle)).confidence = 1;
RCLCPP_INFO(node_->get_logger(), "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(), "[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);
ASN_SEQUENCE_ADD(poc, pObj); ASN_SEQUENCE_ADD(poc, pObj);
} }
} else { } else {
cpm.cpmParameters.perceivedObjectContainer = NULL; cpm.cpmParameters.perceivedObjectContainer = NULL;
RCLCPP_INFO(node_->get_logger(), "Empty POC"); RCLCPP_INFO(node_->get_logger(), "[SEND] Empty POC");
} }
Application::DownPacketPtr packet{new DownPacket()}; Application::DownPacketPtr packet{new DownPacket()};
@ -337,7 +340,7 @@ namespace v2x
Application::DataConfirm confirm = Application::request(request, std::move(payload), node_); Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
if (!confirm.accepted()) { if (!confirm.accepted()) {
throw std::runtime_error("CPM application data request failed"); throw std::runtime_error("[SEND] CPM application data request failed");
} }
// try { // try {