Update to autoware_auto_msgs

include/autoware_v2x/cpm_application.hpp

@@ -5,7 +5,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include <boost/asio/io_service.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"
 
 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;

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 <boost/asio/io_service.hpp>
 #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;

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 <boost/asio/io_service.hpp>
 #include "autoware_v2x/v2x_app.hpp"
@@ -25,13 +25,12 @@ namespace v2x
     void publishObjects(std::vector<CpmApplication::Object> *);
 
   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);
 
-    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::Publisher<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr publisher_;
+    rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr publisher_;
 
     double pos_lat_;
     double pos_lon_;

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<asn1::Cpm> visitor;
     std::shared_ptr<const asn1::Cpm> 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] 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
       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_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_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());

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");

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<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));
 
-    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<bool>("is_sender", true);
@@ -47,38 +47,38 @@ namespace v2x
   }
 
   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;
     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_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.y = obj.shape_y / 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.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());