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

fix bugs

This commit is contained in:
Yu Asabe 2021-11-14 14:07:08 +09:00
parent 72e7dc47c1
commit e7f8b7a210
1 changed files with 16 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/cpm_application.cpp
View File

@ -118,14 +118,15 @@ namespace v2x
object.position_y = y_mgrs + (sin(orientation) * x1 + cos(orientation) * y1); object.position_y = y_mgrs + (sin(orientation) * x1 + cos(orientation) * y1);
RCLCPP_INFO(node_->get_logger(), "cpm object: %f, %f, %f, %f", x1, y1, object.position_x, object.position_y); RCLCPP_INFO(node_->get_logger(), "cpm object: %f, %f, %f, %f", x1, y1, object.position_x, object.position_y);
object.shape_x = poc->list.array[i]->planarObjectDimension2->value / 100; object.shape_x = poc->list.array[i]->planarObjectDimension2->value;
object.shape_y = poc->list.array[i]->planarObjectDimension1->value / 100; object.shape_y = poc->list.array[i]->planarObjectDimension1->value;
object.shape_z = poc->list.array[i]->verticalObjectDimension->value / 100; object.shape_z = poc->list.array[i]->verticalObjectDimension->value;
object.yawAngle = poc->list.array[i]->yawAngle->value / 10; object.yawAngle = poc->list.array[i]->yawAngle->value;
double yaw_radian = (M_PI * object.yawAngle / 10) / 180;
tf2::Quaternion quat; tf2::Quaternion quat;
quat.setRPY(0, 0, object.yawAngle); quat.setRPY(0, 0, yaw_radian);
object.orientation_x = quat.x(); object.orientation_x = quat.x();
object.orientation_y = quat.y(); object.orientation_y = quat.y();
object.orientation_z = quat.z(); object.orientation_z = quat.z();
@ -209,7 +210,7 @@ namespace v2x
double roll, pitch, yaw; double roll, pitch, yaw;
matrix.getRPY(roll, pitch, yaw); matrix.getRPY(roll, pitch, yaw);
// RCLCPP_INFO(node_->get_logger(), "yaw: %f", yaw); // RCLCPP_INFO(node_->get_logger(), "yaw: %f", yaw);
object.yawAngle = (int) (yaw * 180 / M_PI) * 10 ; object.yawAngle = (int) (yaw * 180 / M_PI) * 10 ; // 0 - 3600
object.timeOfMeasurement = 100; object.timeOfMeasurement = 100;
objectsStack.push_back(object); objectsStack.push_back(object);
@ -309,22 +310,13 @@ namespace v2x
(*(pObj->planarObjectDimension2)).confidence = 1; (*(pObj->planarObjectDimension2)).confidence = 1;
(*(pObj->verticalObjectDimension)).value = object.shape_z; (*(pObj->verticalObjectDimension)).value = object.shape_z;
(*(pObj->verticalObjectDimension)).confidence = 1; (*(pObj->verticalObjectDimension)).confidence = 1;
// *((int*)pObj->planarObjectDimension1->confidence) = 1;
// *((int*)pObj->planarObjectDimension2->value) = object.shape_x;
// *((int*)pObj->planarObjectDimension2->confidence) = 1;
// *((int*)pObj->verticalObjectDimension->value) = object.shape_z;
// *((int*)pObj->verticalObjectDimension->confidence) = 1;
// RCLCPP_INFO(node_->get_logger(), "%d", *((int*)pObj->planarObjectDimension1->value));
// RCLCPP_INFO(node_->get_logger(), "%d", *((int*)pObj->planarObjectDimension2->value));
// RCLCPP_INFO(node_->get_logger(), "%d", *((int*)pObj->verticalObjectDimension->value));
pObj->yawAngle = vanetza::asn1::allocate<CartesianAngle>(); pObj->yawAngle = vanetza::asn1::allocate<CartesianAngle>();
(*(pObj->yawAngle)).value = object.yawAngle; (*(pObj->yawAngle)).value = object.yawAngle;
(*(pObj->yawAngle)).confidence = 1; (*(pObj->yawAngle)).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);
} }
Application::DownPacketPtr packet{new DownPacket()}; Application::DownPacketPtr packet{new DownPacket()};
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()}; std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
@ -337,13 +329,15 @@ namespace v2x
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(), "Going to Application::request..."); try {
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("CPM application data request failed");
} }
} catch (...) {
RCLCPP_INFO(node_->get_logger(), "Request Failed");
}
} }
sending_ = false; sending_ = false;
// RCLCPP_INFO(node->get_logger(), "Application::request END"); // RCLCPP_INFO(node->get_logger(), "Application::request END");