Merge pull request #5 from tlab-wide/update-to-universe

Update to universe

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_;

package.xml

@@ -9,7 +9,7 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
-  <depend>autoware_perception_msgs</depend>
+  <depend>autoware_auto_perception_msgs</depend>
   <depend>tf2_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>

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/cpm_receiver.cpp

@@ -1,123 +0,0 @@
-#include "autoware_v2x/cpm_sender.hpp"
-#include "autoware_v2x/time_trigger.hpp"
-#include "autoware_v2x/router_context.hpp"
-#include "autoware_v2x/positioning.hpp"
-#include "autoware_v2x/security.hpp"
-#include "autoware_v2x/link_layer.hpp"
-#include "autoware_v2x/cpm_application.hpp"
-
-#include "rclcpp/rclcpp.hpp"
-#include "std_msgs/msg/string.hpp"
-
-#include <vanetza/asn1/cpm.hpp>
-#include <vanetza/facilities/cpm_functions.hpp>
-#include <sstream>
-#include <memory>
-#include <GeographicLib/UTMUPS.hpp>
-#include <GeographicLib/MGRS.hpp>
-
-namespace gn = vanetza::geonet;
-
-using namespace vanetza;
-using namespace vanetza::facilities;
-using namespace std::chrono;
-
-namespace v2x
-{
-  V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : 
-    rclcpp::Node("autoware_v2x_node", node_options),
-    io_service_(),
-    trigger_(io_service_),
-    device_name_("wlp4s0"),
-    device_(device_name_),
-    mac_address_(device_.address()),
-    link_layer_(create_link_layer(io_service_, device_, "ethernet")),
-    positioning_(create_position_provider(io_service_, trigger_.runtime())),
-    security_(create_security_entity(trigger_.runtime(), *positioning_)),
-    mib_(),
-    context_(mib_, trigger_, *positioning_, security_.get()),
-    cp_(new CpmApplication(this)),
-    app_()
-  {
-    using std::placeholders::_1;
-    subscription_ = this->create_subscription<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/detection/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));
-    RCLCPP_INFO(get_logger(), "V2X Node Launched");
-
-    // device_name_ = "wlp4s0";
-    // device_(device_name_);
-    // mac_address_ = device_.address();
-
-    std::stringstream sout;
-    sout << mac_address_;
-    RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str());
-
-    // trigger_(io_service_);
-
-    // link_layer_ = create_link_layer(io_service_, device_, "ethernet");
-    mib_.itsGnLocalGnAddr.mid(mac_address_);
-    mib_.itsGnLocalGnAddr.is_manually_configured(true);
-    mib_.itsGnLocalAddrConfMethod = geonet::AddrConfMethod::Managed;
-    mib_.itsGnSecurity = false;
-    mib_.itsGnProtocolVersion = 1;
-
-    // context_.router_.set_address(mib_.itsGnLocalGnAddr);
-    context_.updateMIB(mib_);
-
-    // positioning_ = create_position_provider(io_service_, trigger_.runtime());
-    // security_ = create_security_entity(trigger_.runtime(), *positioning_);
-
-    // RouterContext context(mib_, trigger_, *positioning_, security_.get());
-    // RouterContext context_(mib_, trigger_, *positioning_, security_.get());
-    context_.set_link_layer(link_layer_.get());
-
-    // std::unique_ptr<CpmApplication> cp_ { new CpmApplication(this) };
-    // app_ = std::move(cp_);
-
-    context_.enable(cp_.get());
-
-
-    // io_service_.run();
-
-    // // Print MAC Address to logger
-    // std::stringstream sout;
-    // sout << mac_address;
-    // RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str());
-  }
-
-  void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg)
-  {
-    // RCLCPP_INFO(get_logger(), "I heard: '%s'", msg->data.c_str());
-    RCLCPP_INFO(get_logger(), "V2X: %d objects detected!", msg->objects.size());
-    // Send CPM
-    cp_->send(msg, this, pos_lat_, pos_lon_);
-  }
-
-  void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
-    // RCLCPP_INFO(get_logger(), "Ego Position: (%f, %f, %f)", msg->transforms[0].transform.translation.x, msg->transforms[0].transform.translation.y, msg->transforms[0].transform.translation.z);
-
-    float x = msg->transforms[0].transform.translation.x;
-    float y = msg->transforms[0].transform.translation.y;
-    float z = msg->transforms[0].transform.translation.z;
-
-    char mgrs[20];
-    int zone, prec;
-    bool northp;
-    double x_mgrs, y_mgrs;
-    double lat, lon;
-    sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y);
-    RCLCPP_INFO(get_logger(), "MGRS: %s", mgrs);
-
-    GeographicLib::MGRS::Reverse(mgrs, zone, northp, x_mgrs, y_mgrs, prec);
-    GeographicLib::UTMUPS::Reverse(zone, northp, x_mgrs, y_mgrs, lat, lon);
-
-    pos_lat_ = lat;
-    pos_lon_ = lon;
-
-    RCLCPP_INFO(get_logger(), "Ego Position Lat/Lon: %f, %f", lat, lon);
-  }
-}
-
-#include "rclcpp_components/register_node_macro.hpp"
-RCLCPP_COMPONENTS_REGISTER_NODE(v2x::V2XNode)

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