diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp index 4f64a76..ce85419 100644 --- a/include/autoware_v2x/cpm_application.hpp +++ b/include/autoware_v2x/cpm_application.hpp @@ -5,7 +5,7 @@ #include "rclcpp/rclcpp.hpp" #include #include -#include "autoware_perception_msgs/msg/dynamic_object_array.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" #include "autoware_v2x/positioning.hpp" namespace v2x @@ -16,17 +16,17 @@ namespace v2x public: CpmApplication(V2XNode *node, vanetza::Runtime &, bool is_sender); PortType port() override; + std::string uuidToHexString(const unique_identifier_msgs::msg::UUID&); void indicate(const DataIndication &, UpPacketPtr) override; 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 updateRP(double *, double *, double *); void updateGenerationDeltaTime(int *, long long *); void updateHeading(double *); void send(); - struct Object - { + struct Object { int objectID; // 0-255 rclcpp::Time timestamp; double position_x; diff --git a/include/autoware_v2x/v2x_app.hpp b/include/autoware_v2x/v2x_app.hpp index 7526af4..bdacc50 100644 --- a/include/autoware_v2x/v2x_app.hpp +++ b/include/autoware_v2x/v2x_app.hpp @@ -3,7 +3,7 @@ #include "rclcpp/rclcpp.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 #include "autoware_v2x/cpm_application.hpp" @@ -23,7 +23,7 @@ namespace v2x public: V2XApp(V2XNode *); 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); CpmApplication *cp; diff --git a/include/autoware_v2x/v2x_node.hpp b/include/autoware_v2x/v2x_node.hpp index 0427084..4c5440a 100644 --- a/include/autoware_v2x/v2x_node.hpp +++ b/include/autoware_v2x/v2x_node.hpp @@ -3,7 +3,7 @@ #include "rclcpp/rclcpp.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 #include "autoware_v2x/v2x_app.hpp" @@ -25,13 +25,12 @@ namespace v2x void publishObjects(std::vector *); private: - void objectsCallback( - const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg); + void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg); - rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Subscription::SharedPtr subscription_; rclcpp::Subscription::SharedPtr subscription_pos_; - rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_; double pos_lat_; double pos_lon_; diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp index 4d1cd7c..e1df7d4 100644 --- a/src/cpm_application.cpp +++ b/src/cpm_application.cpp @@ -65,13 +65,21 @@ namespace v2x { 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) { asn1::PacketVisitor visitor; std::shared_ptr cpm = boost::apply_visitor(visitor, *packet); 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_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; if (orientation < 0.0) orientation += (2.0 * M_PI); // double orientation = heading / 10.0; - 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] heading: %d", heading); + // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] orientation: %f", orientation); // Get PerceivedObjects receivedObjectsStack.clear(); @@ -150,7 +158,7 @@ namespace v2x { } node_->publishObjects(&receivedObjectsStack); } else { - RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC"); + // RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC"); } if (reflect_packet_) { @@ -198,7 +206,7 @@ namespace v2x { 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; if (sending_) { @@ -210,17 +218,20 @@ namespace v2x { if (msg->objects.size() > 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; object.objectID = i; object.timestamp = msg->header.stamp; - object.position_x = obj.state.pose_covariance.pose.position.x; // MGRS - object.position_y = obj.state.pose_covariance.pose.position.y; - object.position_z = obj.state.pose_covariance.pose.position.z; - object.orientation_x = obj.state.pose_covariance.pose.orientation.x; - 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.position_x = obj.kinematics.initial_pose_with_covariance.pose.position.x; // MGRS + object.position_y = obj.kinematics.initial_pose_with_covariance.pose.position.y; + object.position_z = obj.kinematics.initial_pose_with_covariance.pose.position.z; + object.orientation_x = obj.kinematics.initial_pose_with_covariance.pose.orientation.x; + object.orientation_y = obj.kinematics.initial_pose_with_covariance.pose.orientation.y; + object.orientation_z = obj.kinematics.initial_pose_with_covariance.pose.orientation.z; + 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_y = std::lround(obj.shape.dimensions.y * 10.0); object.shape_z = std::lround(obj.shape.dimensions.z * 10.0); @@ -251,8 +262,6 @@ namespace v2x { } else { 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_nsec = msg->header.stamp.nanosec; @@ -267,10 +276,9 @@ namespace v2x { } objectsStack.push_back(object); ++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::Time current_time = node_->now(); // RCLCPP_INFO(node_->get_logger(), "[CpmApplication::updateObjectsStack] [measure] T_objstack_updated %ld", current_time.nanoseconds()); diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp index a56db42..89fd3fc 100644 --- a/src/v2x_app.cpp +++ b/src/v2x_app.cpp @@ -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"); if (!tf_received_) { RCLCPP_WARN(node_->get_logger(), "[V2XApp::objectsCallback] tf not received yet"); diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp index 7e4dd0e..c4644fd 100644 --- a/src/v2x_node.cpp +++ b/src/v2x_node.cpp @@ -27,11 +27,11 @@ namespace v2x { V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options) { using std::placeholders::_1; - subscription_ = this->create_subscription("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); + subscription_ = this->create_subscription("/perception/object_recognition/objects", 10, std::bind(&V2XNode::objectsCallback, this, _1)); subscription_pos_ = this->create_subscription("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1)); - publisher_ = create_publisher("/v2x/cpm/objects", rclcpp::QoS{10}); + publisher_ = create_publisher("/v2x/cpm/objects", rclcpp::QoS{10}); this->declare_parameter("network_interface", "vmnet1"); this->declare_parameter("is_sender", true); @@ -47,38 +47,38 @@ namespace v2x } void V2XNode::publishObjects(std::vector *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; rclcpp::Time current_time = this->now(); output_dynamic_object_msg.header.frame_id = "map"; output_dynamic_object_msg.header.stamp = current_time; for (CpmApplication::Object obj : *objectsStack) { - autoware_perception_msgs::msg::DynamicObject object; - autoware_perception_msgs::msg::Semantic semantic; - autoware_perception_msgs::msg::Shape shape; - autoware_perception_msgs::msg::State state; + autoware_auto_perception_msgs::msg::PredictedObject object; + autoware_auto_perception_msgs::msg::ObjectClassification classification; + autoware_auto_perception_msgs::msg::Shape shape; + autoware_auto_perception_msgs::msg::PredictedObjectKinematics kinematics; - semantic.type = autoware_perception_msgs::msg::Semantic::CAR; - semantic.confidence = 0.99; + classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + 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.y = obj.shape_y / 10.0; shape.dimensions.z = obj.shape_z / 10.0; - 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.1; + kinematics.initial_pose_with_covariance.pose.position.x = obj.position_x; + kinematics.initial_pose_with_covariance.pose.position.y = obj.position_y; + kinematics.initial_pose_with_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; + kinematics.initial_pose_with_covariance.pose.orientation.x = obj.orientation_x; + kinematics.initial_pose_with_covariance.pose.orientation.y = obj.orientation_y; + kinematics.initial_pose_with_covariance.pose.orientation.z = obj.orientation_z; + kinematics.initial_pose_with_covariance.pose.orientation.w = obj.orientation_w; - object.semantic = semantic; + object.classification.emplace_back(classification); object.shape = shape; - object.state = state; + object.kinematics = kinematics; output_dynamic_object_msg.objects.push_back(object); } @@ -89,7 +89,7 @@ namespace v2x 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 msg_time = msg->header.stamp; // timestamp included in the Autoware Perception Msg. // RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] %d objects", msg->objects.size());