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