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

add orientation and shape

This commit is contained in:
Yu Asabe 2021-11-14 13:26:18 +09:00
parent 1360510b2a
commit 72e7dc47c1
3 changed files with 71 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/autoware_v2x/cpm_application.hpp
View File

@ -36,10 +36,14 @@ namespace v2x
double orientation_y;
double orientation_z;
double orientation_w;
int shape_x;
int shape_y;
int shape_z;
int xDistance;
int yDistance;
double xSpeed;
double ySpeed;
int yawAngle;
vanetza::PositionFix position;
int timeOfMeasurement;
};

64
src/cpm_application.cpp
View File

@ -4,6 +4,9 @@
#include "autoware_v2x/link_layer.hpp"
#include "autoware_v2x/v2x_node.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "rclcpp/rclcpp.hpp"
#include <vanetza/btp/ports.hpp>
#include <vanetza/asn1/cpm.hpp>
@ -114,6 +117,20 @@ namespace v2x
object.position_x = x_mgrs + (cos(orientation) * x1 - sin(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);
object.shape_x = poc->list.array[i]->planarObjectDimension2->value / 100;
object.shape_y = poc->list.array[i]->planarObjectDimension1->value / 100;
object.shape_z = poc->list.array[i]->verticalObjectDimension->value / 100;
object.yawAngle = poc->list.array[i]->yawAngle->value / 10;
tf2::Quaternion quat;
quat.setRPY(0, 0, object.yawAngle);
object.orientation_x = quat.x();
object.orientation_y = quat.y();
object.orientation_z = quat.z();
object.orientation_w = quat.w();
receivedObjectsStack.push_back(object);
}
node_->publishObjects(&receivedObjectsStack);
@ -164,10 +181,6 @@ namespace v2x
if (msg->objects.size() > 0)
{
// RCLCPP_INFO(node_->get_logger(), "At least 1 object detected");
// Initialize ObjectsStack
// std::vector<CpmApplication::Object> objectsStack;
objectsStack.clear();
// Create CpmApplication::Object per msg->object and add it to objectsStack
@ -184,9 +197,20 @@ namespace v2x
object.orientation_y = obj.state.pose_covariance.pose.orientation.y;
object.orientation_z = obj.state.pose_covariance.pose.orientation.z;
object.orientation_w = obj.state.pose_covariance.pose.orientation.w;
object.shape_x = (int) obj.shape.dimensions.x * 10;
object.shape_y = (int) obj.shape.dimensions.y * 10;
object.shape_z = (int) obj.shape.dimensions.z * 10;
object.xDistance = (int)((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100;
object.yDistance = (int)((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100;
// RCLCPP_INFO(node_->get_logger(), "xDistance: %d, yDistance: %d", object.xDistance, object.yDistance);
tf2::Quaternion quat(object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w);
tf2::Matrix3x3 matrix(quat);
double roll, pitch, yaw;
matrix.getRPY(roll, pitch, yaw);
// RCLCPP_INFO(node_->get_logger(), "yaw: %f", yaw);
object.yawAngle = (int) (yaw * 180 / M_PI) * 10 ;
object.timeOfMeasurement = 100;
objectsStack.push_back(object);
++i;
@ -251,7 +275,7 @@ namespace v2x
ovc.heading.headingValue = (int)std::fmod((1.5708 - ego_heading_) * 180 / M_PI, 360.0) * 10;
ovc.heading.headingConfidence = 1;
// RCLCPP_INFO(node_->get_logger(), "PerceviedObjectContainer...");
RCLCPP_INFO(node_->get_logger(), "PerceviedObjectContainer...");
PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer;
poc = vanetza::asn1::allocate<PerceivedObjectContainer_t>();
@ -259,6 +283,8 @@ namespace v2x
{
if (object.xDistance > 10000) continue;
if (object.yDistance > 10000) continue;
RCLCPP_INFO(node_->get_logger(), "Adding Object...");
PerceivedObject *pObj = vanetza::asn1::allocate<PerceivedObject>();
pObj->objectID = object.objectID;
pObj->timeOfMeasurement = object.timeOfMeasurement;
@ -271,8 +297,34 @@ namespace v2x
pObj->ySpeed.value = object.ySpeed;
pObj->ySpeed.confidence = 1;
pObj->planarObjectDimension1 = vanetza::asn1::allocate<ObjectDimension_t>();
pObj->planarObjectDimension2 = 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)).confidence = 1;
(*(pObj->planarObjectDimension2)).value = object.shape_x;
(*(pObj->planarObjectDimension2)).confidence = 1;
(*(pObj->verticalObjectDimension)).value = object.shape_z;
(*(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)).value = object.yawAngle;
(*(pObj->yawAngle)).confidence = 1;
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()};
std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};

13
src/v2x_node.cpp
View File

@ -58,13 +58,18 @@ namespace v2x
semantic.confidence = 0.99;
shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
shape.dimensions.x = 2;
shape.dimensions.y = 5.0;
shape.dimensions.z = 2.5;
shape.dimensions.x = obj.shape_x / 10;
shape.dimensions.y = obj.shape_y / 10;
shape.dimensions.z = obj.shape_z / 10;
state.pose_covariance.pose.position.x = obj.position_x;
state.pose_covariance.pose.position.y = obj.position_y;
state.pose_covariance.pose.position.z = 0.2;
state.pose_covariance.pose.position.z = 0.1;
state.pose_covariance.pose.orientation.x = obj.orientation_x;
state.pose_covariance.pose.orientation.y = obj.orientation_y;
state.pose_covariance.pose.orientation.z = obj.orientation_z;
state.pose_covariance.pose.orientation.w = obj.orientation_w;
object.semantic = semantic;
object.shape = shape;