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

Update to autoware_auto_msgs

This commit is contained in:
Yu Asabe 2022-03-28 11:17:51 +09:00
parent f3733b8a8a
commit 56740cd436
6 changed files with 56 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/autoware_v2x/cpm_application.hpp
View File

@ -5,7 +5,7 @@
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include <boost/asio/io_service.hpp> #include <boost/asio/io_service.hpp>
#include <boost/asio/steady_timer.hpp> #include <boost/asio/steady_timer.hpp>
#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
#include "autoware_v2x/positioning.hpp" #include "autoware_v2x/positioning.hpp"
namespace v2x namespace v2x
@ -16,17 +16,17 @@ namespace v2x
public: public:
CpmApplication(V2XNode *node, vanetza::Runtime &, bool is_sender); CpmApplication(V2XNode *node, vanetza::Runtime &, bool is_sender);
PortType port() override; PortType port() override;
std::string uuidToHexString(const unique_identifier_msgs::msg::UUID&);
void indicate(const DataIndication &, UpPacketPtr) override; void indicate(const DataIndication &, UpPacketPtr) override;
void set_interval(vanetza::Clock::duration); void set_interval(vanetza::Clock::duration);
void updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr); void updateObjectsStack(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr);
void updateMGRS(double *, double *); void updateMGRS(double *, double *);
void updateRP(double *, double *, double *); void updateRP(double *, double *, double *);
void updateGenerationDeltaTime(int *, long long *); void updateGenerationDeltaTime(int *, long long *);
void updateHeading(double *); void updateHeading(double *);
void send(); void send();
struct Object struct Object {
{
int objectID; // 0-255 int objectID; // 0-255
rclcpp::Time timestamp; rclcpp::Time timestamp;
double position_x; double position_x;

4
include/autoware_v2x/v2x_app.hpp
View File

@ -3,7 +3,7 @@
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp" #include "std_msgs/msg/string.hpp"
#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
#include "tf2_msgs/msg/tf_message.hpp" #include "tf2_msgs/msg/tf_message.hpp"
#include <boost/asio/io_service.hpp> #include <boost/asio/io_service.hpp>
#include "autoware_v2x/cpm_application.hpp" #include "autoware_v2x/cpm_application.hpp"
@ -23,7 +23,7 @@ namespace v2x
public: public:
V2XApp(V2XNode *); V2XApp(V2XNode *);
void start(); void start();
void objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr); void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr);
void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr); void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr);
CpmApplication *cp; CpmApplication *cp;

9
include/autoware_v2x/v2x_node.hpp
View File

@ -3,7 +3,7 @@
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp" #include "std_msgs/msg/string.hpp"
#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
#include "tf2_msgs/msg/tf_message.hpp" #include "tf2_msgs/msg/tf_message.hpp"
#include <boost/asio/io_service.hpp> #include <boost/asio/io_service.hpp>
#include "autoware_v2x/v2x_app.hpp" #include "autoware_v2x/v2x_app.hpp"
@ -25,13 +25,12 @@ namespace v2x
void publishObjects(std::vector<CpmApplication::Object> *); void publishObjects(std::vector<CpmApplication::Object> *);
private: private:
void objectsCallback( void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg);
const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg);
void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg); void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg);
rclcpp::Subscription<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr subscription_; rclcpp::Subscription<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr subscription_;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_pos_; rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_pos_;
rclcpp::Publisher<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr publisher_; rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr publisher_;
double pos_lat_; double pos_lat_;
double pos_lon_; double pos_lon_;

42
src/cpm_application.cpp
View File

@ -65,13 +65,21 @@ namespace v2x {
return btp::ports::CPM; return btp::ports::CPM;
} }
std::string CpmApplication::uuidToHexString(const unique_identifier_msgs::msg::UUID &id) {
std::stringstream ss;
for (auto i = 0; i < 16; ++i) {
ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i];
}
return ss.str();
}
void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet) { void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet) {
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(), "[CpmApplication::indicate] Received decodable CPM content"); RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] Received CPM");
rclcpp::Time current_time = node_->now(); rclcpp::Time current_time = node_->now();
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r1 %ld", current_time.nanoseconds()); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r1 %ld", current_time.nanoseconds());
@ -112,8 +120,8 @@ namespace v2x {
double orientation = (90.0 - (double) heading / 10.0) * M_PI / 180.0; double orientation = (90.0 - (double) heading / 10.0) * M_PI / 180.0;
if (orientation < 0.0) orientation += (2.0 * M_PI); if (orientation < 0.0) orientation += (2.0 * M_PI);
// double orientation = heading / 10.0; // double orientation = heading / 10.0;
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] heading: %d", heading); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] heading: %d", heading);
RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] orientation: %f", orientation); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] orientation: %f", orientation);
// Get PerceivedObjects // Get PerceivedObjects
receivedObjectsStack.clear(); receivedObjectsStack.clear();
@ -150,7 +158,7 @@ namespace v2x {
} }
node_->publishObjects(&receivedObjectsStack); node_->publishObjects(&receivedObjectsStack);
} else { } else {
RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC"); // RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC");
} }
if (reflect_packet_) { if (reflect_packet_) {
@ -198,7 +206,7 @@ namespace v2x {
ego_.heading = *yaw; ego_.heading = *yaw;
} }
void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { void CpmApplication::updateObjectsStack(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) {
updating_objects_stack_ = true; updating_objects_stack_ = true;
if (sending_) { if (sending_) {
@ -210,17 +218,20 @@ namespace v2x {
if (msg->objects.size() > 0) { if (msg->objects.size() > 0) {
int i = 0; int i = 0;
for (auto obj : msg->objects) { for (autoware_auto_perception_msgs::msg::PredictedObject obj : msg->objects) {
// RCLCPP_INFO(node_->get_logger(), "%d", obj.classification.front().label);
CpmApplication::Object object; CpmApplication::Object object;
object.objectID = i; object.objectID = i;
object.timestamp = msg->header.stamp; object.timestamp = msg->header.stamp;
object.position_x = obj.state.pose_covariance.pose.position.x; // MGRS object.position_x = obj.kinematics.initial_pose_with_covariance.pose.position.x; // MGRS
object.position_y = obj.state.pose_covariance.pose.position.y; object.position_y = obj.kinematics.initial_pose_with_covariance.pose.position.y;
object.position_z = obj.state.pose_covariance.pose.position.z; object.position_z = obj.kinematics.initial_pose_with_covariance.pose.position.z;
object.orientation_x = obj.state.pose_covariance.pose.orientation.x; object.orientation_x = obj.kinematics.initial_pose_with_covariance.pose.orientation.x;
object.orientation_y = obj.state.pose_covariance.pose.orientation.y; object.orientation_y = obj.kinematics.initial_pose_with_covariance.pose.orientation.y;
object.orientation_z = obj.state.pose_covariance.pose.orientation.z; object.orientation_z = obj.kinematics.initial_pose_with_covariance.pose.orientation.z;
object.orientation_w = obj.state.pose_covariance.pose.orientation.w; object.orientation_w = obj.kinematics.initial_pose_with_covariance.pose.orientation.w;
object.shape_x = std::lround(obj.shape.dimensions.x * 10.0); object.shape_x = std::lround(obj.shape.dimensions.x * 10.0);
object.shape_y = std::lround(obj.shape.dimensions.y * 10.0); object.shape_y = std::lround(obj.shape.dimensions.y * 10.0);
object.shape_z = std::lround(obj.shape.dimensions.z * 10.0); object.shape_z = std::lround(obj.shape.dimensions.z * 10.0);
@ -251,8 +262,6 @@ namespace v2x {
} else { } else {
object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600 object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600
} }
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] object.yawAngle: %d", object.yawAngle);
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] object.quat: %f, %f, %f, %f", object.orientation_x, object.orientation_y, object.orientation_z, object.orientation_w);
long long msg_timestamp_sec = msg->header.stamp.sec; long long msg_timestamp_sec = msg->header.stamp.sec;
long long msg_timestamp_nsec = msg->header.stamp.nanosec; long long msg_timestamp_nsec = msg->header.stamp.nanosec;
@ -267,10 +276,9 @@ namespace v2x {
} }
objectsStack.push_back(object); objectsStack.push_back(object);
++i; ++i;
// 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(), "ObjectsStack: %d objects", objectsStack.size()); // RCLCPP_INFO(node_->get_logger(), "ObjectsStack: %d objects", objectsStack.size());
rclcpp::Time current_time = node_->now(); rclcpp::Time current_time = node_->now();
// RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] [measure] T_objstack_updated %ld", current_time.nanoseconds()); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] [measure] T_objstack_updated %ld", current_time.nanoseconds());

2
src/v2x_app.cpp
View File

@ -38,7 +38,7 @@ namespace v2x
{ {
} }
void V2XApp::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { void V2XApp::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) {
// RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received"); // RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received");
if (!tf_received_) { if (!tf_received_) {
RCLCPP_WARN(node_->get_logger(), "[V2XApp::objectsCallback] tf not received yet"); RCLCPP_WARN(node_->get_logger(), "[V2XApp::objectsCallback] tf not received yet");

40
src/v2x_node.cpp
View File

@ -27,11 +27,11 @@ namespace v2x
{ {
V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) { V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) {
using std::placeholders::_1; using std::placeholders::_1;
subscription_ = this->create_subscription<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); subscription_ = this->create_subscription<autoware_auto_perception_msgs::msg::PredictedObjects>("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1));
subscription_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1)); subscription_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));
publisher_ = create_publisher<autoware_perception_msgs::msg::DynamicObjectArray>("/v2x/cpm/objects", rclcpp::QoS{10}); publisher_ = create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>("/v2x/cpm/objects", rclcpp::QoS{10});
this->declare_parameter<std::string>("network_interface", "vmnet1"); this->declare_parameter<std::string>("network_interface", "vmnet1");
this->declare_parameter<bool>("is_sender", true); this->declare_parameter<bool>("is_sender", true);
@ -47,38 +47,38 @@ namespace v2x
} }
void V2XNode::publishObjects(std::vector<CpmApplication::Object> *objectsStack) { void V2XNode::publishObjects(std::vector<CpmApplication::Object> *objectsStack) {
autoware_perception_msgs::msg::DynamicObjectArray output_dynamic_object_msg; autoware_auto_perception_msgs::msg::PredictedObjects output_dynamic_object_msg;
std_msgs::msg::Header header; std_msgs::msg::Header header;
rclcpp::Time current_time = this->now(); rclcpp::Time current_time = this->now();
output_dynamic_object_msg.header.frame_id = "map"; output_dynamic_object_msg.header.frame_id = "map";
output_dynamic_object_msg.header.stamp = current_time; output_dynamic_object_msg.header.stamp = current_time;
for (CpmApplication::Object obj : *objectsStack) { for (CpmApplication::Object obj : *objectsStack) {
autoware_perception_msgs::msg::DynamicObject object; autoware_auto_perception_msgs::msg::PredictedObject object;
autoware_perception_msgs::msg::Semantic semantic; autoware_auto_perception_msgs::msg::ObjectClassification classification;
autoware_perception_msgs::msg::Shape shape; autoware_auto_perception_msgs::msg::Shape shape;
autoware_perception_msgs::msg::State state; autoware_auto_perception_msgs::msg::PredictedObjectKinematics kinematics;
semantic.type = autoware_perception_msgs::msg::Semantic::CAR; classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR;
semantic.confidence = 0.99; classification.probability = 0.99;
shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX;
shape.dimensions.x = obj.shape_x / 10.0; shape.dimensions.x = obj.shape_x / 10.0;
shape.dimensions.y = obj.shape_y / 10.0; shape.dimensions.y = obj.shape_y / 10.0;
shape.dimensions.z = obj.shape_z / 10.0; shape.dimensions.z = obj.shape_z / 10.0;
state.pose_covariance.pose.position.x = obj.position_x; kinematics.initial_pose_with_covariance.pose.position.x = obj.position_x;
state.pose_covariance.pose.position.y = obj.position_y; kinematics.initial_pose_with_covariance.pose.position.y = obj.position_y;
state.pose_covariance.pose.position.z = 0.1; kinematics.initial_pose_with_covariance.pose.position.z = 0.1;
state.pose_covariance.pose.orientation.x = obj.orientation_x; kinematics.initial_pose_with_covariance.pose.orientation.x = obj.orientation_x;
state.pose_covariance.pose.orientation.y = obj.orientation_y; kinematics.initial_pose_with_covariance.pose.orientation.y = obj.orientation_y;
state.pose_covariance.pose.orientation.z = obj.orientation_z; kinematics.initial_pose_with_covariance.pose.orientation.z = obj.orientation_z;
state.pose_covariance.pose.orientation.w = obj.orientation_w; kinematics.initial_pose_with_covariance.pose.orientation.w = obj.orientation_w;
object.semantic = semantic; object.classification.emplace_back(classification);
object.shape = shape; object.shape = shape;
object.state = state; object.kinematics = kinematics;
output_dynamic_object_msg.objects.push_back(object); output_dynamic_object_msg.objects.push_back(object);
} }
@ -89,7 +89,7 @@ namespace v2x
publisher_->publish(output_dynamic_object_msg); publisher_->publish(output_dynamic_object_msg);
} }
void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) { void V2XNode::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) {
rclcpp::Time current_time = this->now(); rclcpp::Time current_time = this->now();
rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg. rclcpp::Time msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg.
// RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] %d objects", msg->objects.size()); // RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] %d objects", msg->objects.size());