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

fix sender bugs

This commit is contained in:
Yu Asabe 2021-11-17 17:45:25 +09:00
parent 3ca0e2154e
commit 13fde9edd9
2 changed files with 102 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/autoware_v2x/cpm_application.hpp
View File

@ -41,8 +41,8 @@ namespace v2x
int shape_z; int shape_z;
int xDistance; int xDistance;
int yDistance; int yDistance;
double xSpeed; int xSpeed;
double ySpeed; int ySpeed;
int yawAngle; int yawAngle;
vanetza::PositionFix position; vanetza::PositionFix position;
int timeOfMeasurement; int timeOfMeasurement;

79
src/cpm_application.cpp
View File

@ -44,7 +44,7 @@ namespace v2x
sending_(false) sending_(false)
{ {
RCLCPP_INFO(node_->get_logger(), "CpmApplication started..."); RCLCPP_INFO(node_->get_logger(), "CpmApplication started...");
set_interval(milliseconds(100)); set_interval(milliseconds(1000));
} }
void CpmApplication::set_interval(Clock::duration interval) void CpmApplication::set_interval(Clock::duration interval)
@ -171,20 +171,16 @@ namespace v2x
void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg)
{ {
// RCLCPP_INFO(node_->get_logger(), "Update ObjectsStack");
updating_objects_stack_ = true; updating_objects_stack_ = true;
if (sending_) if (sending_) {
{
RCLCPP_INFO(node_->get_logger(), "updateObjectsStack Skipped..."); RCLCPP_INFO(node_->get_logger(), "updateObjectsStack Skipped...");
return; return;
} else {
objectsStack.clear();
} }
if (msg->objects.size() > 0) if (msg->objects.size() > 0) {
{
objectsStack.clear();
// Create CpmApplication::Object per msg->object and add it to objectsStack
int i = 0; int i = 0;
for (auto obj : msg->objects) for (auto obj : msg->objects)
{ {
@ -198,37 +194,43 @@ namespace v2x
object.orientation_y = obj.state.pose_covariance.pose.orientation.y; object.orientation_y = obj.state.pose_covariance.pose.orientation.y;
object.orientation_z = obj.state.pose_covariance.pose.orientation.z; object.orientation_z = obj.state.pose_covariance.pose.orientation.z;
object.orientation_w = obj.state.pose_covariance.pose.orientation.w; object.orientation_w = obj.state.pose_covariance.pose.orientation.w;
object.shape_x = (int) obj.shape.dimensions.x * 10; object.shape_x = std::lround(obj.shape.dimensions.x * 10.0);
object.shape_y = (int) obj.shape.dimensions.y * 10; object.shape_y = std::lround(obj.shape.dimensions.y * 10.0);
object.shape_z = (int) obj.shape.dimensions.z * 10; object.shape_z = std::lround(obj.shape.dimensions.z * 10.0);
object.xDistance = (int)((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100; object.xDistance = std::lround(((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100.0);
object.yDistance = (int)((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100; object.yDistance = std::lround(((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100.0);
// RCLCPP_INFO(node_->get_logger(), "xDistance: %d, yDistance: %d", object.xDistance, object.yDistance); object.xSpeed = 0;
object.ySpeed = 0;
tf2::Quaternion quat(object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w); tf2::Quaternion quat(object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w);
tf2::Matrix3x3 matrix(quat); tf2::Matrix3x3 matrix(quat);
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); if (yaw < 0) {
object.yawAngle = (int) (yaw * 180 / M_PI) * 10 ; // 0 - 3600 object.yawAngle = std::lround(((yaw + 2*M_PI) * 180.0 / M_PI) * 10.0); // 0 - 3600
} else {
object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600
}
object.timeOfMeasurement = 100; object.timeOfMeasurement = 100;
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(), "%d objects added to objectsStack", objectsStack.size());
} }
RCLCPP_INFO(node_->get_logger(), "ObjectsStack: %d objects", objectsStack.size());
updating_objects_stack_ = false; updating_objects_stack_ = false;
} }
void CpmApplication::send() void CpmApplication::send()
{ {
sending_ = true; sending_ = true;
if (!updating_objects_stack_ && objectsStack.size() > 0)
{
RCLCPP_INFO(node_->get_logger(), "Sending CPM..."); RCLCPP_INFO(node_->get_logger(), "Sending CPM...");
// Send all objects in 1 CPM message
vanetza::asn1::Cpm message; vanetza::asn1::Cpm message;
// ITS PDU Header // ITS PDU Header
@ -276,16 +278,14 @@ namespace v2x
ovc.heading.headingValue = (int)std::fmod((1.5708 - ego_heading_) * 180 / M_PI, 360.0) * 10; ovc.heading.headingValue = (int)std::fmod((1.5708 - ego_heading_) * 180 / M_PI, 360.0) * 10;
ovc.heading.headingConfidence = 1; ovc.heading.headingConfidence = 1;
// RCLCPP_INFO(node_->get_logger(), "PerceviedObjectContainer..."); if (objectsStack.size() > 0) {
PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer; PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer;
poc = vanetza::asn1::allocate<PerceivedObjectContainer_t>(); poc = vanetza::asn1::allocate<PerceivedObjectContainer_t>();
for (CpmApplication::Object object : objectsStack) for (CpmApplication::Object object : objectsStack) {
{ // if (object.xDistance > 10000) continue;
if (object.xDistance > 10000) continue; // if (object.yDistance > 10000) continue;
if (object.yDistance > 10000) continue;
// RCLCPP_INFO(node_->get_logger(), "Adding Object...");
PerceivedObject *pObj = vanetza::asn1::allocate<PerceivedObject>(); PerceivedObject *pObj = vanetza::asn1::allocate<PerceivedObject>();
pObj->objectID = object.objectID; pObj->objectID = object.objectID;
pObj->timeOfMeasurement = object.timeOfMeasurement; pObj->timeOfMeasurement = object.timeOfMeasurement;
@ -302,8 +302,6 @@ namespace v2x
pObj->planarObjectDimension2 = vanetza::asn1::allocate<ObjectDimension_t>(); pObj->planarObjectDimension2 = vanetza::asn1::allocate<ObjectDimension_t>();
pObj->verticalObjectDimension = vanetza::asn1::allocate<ObjectDimension_t>(); pObj->verticalObjectDimension = vanetza::asn1::allocate<ObjectDimension_t>();
RCLCPP_INFO(node_->get_logger(), "%d %d %d", object.shape_y, object.shape_x, object.shape_z);
(*(pObj->planarObjectDimension1)).value = object.shape_y; (*(pObj->planarObjectDimension1)).value = object.shape_y;
(*(pObj->planarObjectDimension1)).confidence = 1; (*(pObj->planarObjectDimension1)).confidence = 1;
(*(pObj->planarObjectDimension2)).value = object.shape_x; (*(pObj->planarObjectDimension2)).value = object.shape_x;
@ -315,9 +313,15 @@ 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);
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);
} }
} else {
cpm.cpmParameters.perceivedObjectContainer = NULL;
RCLCPP_INFO(node_->get_logger(), "Empty POC");
}
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()};
// std::shared_ptr<asn1::Cpm> message_p = std::make_shared<asn1::Cpm>(message); // std::shared_ptr<asn1::Cpm> message_p = std::make_shared<asn1::Cpm>(message);
@ -330,15 +334,20 @@ namespace v2x
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;
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"); // 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; sending_ = false;
// RCLCPP_INFO(node->get_logger(), "Application::request END"); // RCLCPP_INFO(node->get_logger(), "Application::request END");
} }