From e8238701aeb4a69b43c772a30da2d7e5bbd546b9 Mon Sep 17 00:00:00 2001
From: Yu Asabe <yuasabe@g.ecc.u-tokyo.ac.jp>
Date: Thu, 11 Nov 2021 00:21:32 +0900
Subject: [PATCH] Major updates

---
 include/autoware_v2x/cpm_application.hpp | 103 ++---
 include/autoware_v2x/v2x_app.hpp         |   7 +-
 include/autoware_v2x/v2x_node.hpp        |  28 +-
 src/cpm_application.cpp                  | 486 ++++++++++++-----------
 src/v2x_app.cpp                          |  19 +-
 src/v2x_node.cpp                         | 114 +++---
 6 files changed, 387 insertions(+), 370 deletions(-)

diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp
index 7570336..757fe62 100644
--- a/include/autoware_v2x/cpm_application.hpp
+++ b/include/autoware_v2x/cpm_application.hpp
@@ -8,56 +8,63 @@
 #include "autoware_perception_msgs/msg/dynamic_object_array.hpp"
 #include "autoware_v2x/positioning.hpp"
 
-class CpmApplication : public Application
+namespace v2x
 {
-public:
-    CpmApplication(rclcpp::Node *node, vanetza::Runtime&);
-    PortType port() override;
-    void indicate(const DataIndication &, UpPacketPtr) override;
-    void set_interval(vanetza::Clock::duration);
-    void updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr);
-    void updateMGRS(double *, double *);
-    void updateRP(double *, double *, double *);
-    void updateGenerationDeltaTime(int *);
-    void updateHeading(double *);
-    void send();
+    class V2XNode;
+    class CpmApplication : public Application
+    {
+    public:
+        CpmApplication(V2XNode *node, vanetza::Runtime &);
+        PortType port() override;
+        void indicate(const DataIndication &, UpPacketPtr) override;
+        void set_interval(vanetza::Clock::duration);
+        void updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr);
+        void updateMGRS(double *, double *);
+        void updateRP(double *, double *, double *);
+        void updateGenerationDeltaTime(int *);
+        void updateHeading(double *);
+        void send();
 
-private:
-    void schedule_timer();
-    void on_timer(vanetza::Clock::time_point);
-    
-    rclcpp::Node* node_;
-    vanetza::Runtime& runtime_;
-    vanetza::Clock::duration cpm_interval_;
-    struct Object {
-        int objectID; // 0-255
-        rclcpp::Time timestamp;
-        double position_x;
-        double position_y;
-        double position_z;
-        double orientation_x;
-        double orientation_y;
-        double orientation_z;
-        double orientation_w;
-        int xDistance;
-        int yDistance;
-        double xSpeed;
-        double ySpeed;
-        vanetza::PositionFix position;
-        int timeOfMeasurement;
+        struct Object
+        {
+            int objectID; // 0-255
+            rclcpp::Time timestamp;
+            double position_x;
+            double position_y;
+            double position_z;
+            double orientation_x;
+            double orientation_y;
+            double orientation_z;
+            double orientation_w;
+            int xDistance;
+            int yDistance;
+            double xSpeed;
+            double ySpeed;
+            vanetza::PositionFix position;
+            int timeOfMeasurement;
+        };
+        std::vector<CpmApplication::Object> objectsStack;
+        std::vector<CpmApplication::Object> receivedObjectsStack;
+
+    private:
+        void schedule_timer();
+        void on_timer(vanetza::Clock::time_point);
+
+        V2XNode *node_;
+        vanetza::Runtime &runtime_;
+        vanetza::Clock::duration cpm_interval_;
+
+        double ego_x_;
+        double ego_y_;
+        double ego_lat_;
+        double ego_lon_;
+        double ego_altitude_;
+        double ego_heading_;
+        int generationDeltaTime_;
+
+        bool updating_objects_stack_;
+        bool sending_;
     };
-    std::vector<CpmApplication::Object> objectsStack;
-
-    double ego_x_;
-    double ego_y_;
-    double ego_lat_;
-    double ego_lon_;
-    double ego_altitude_;
-    double ego_heading_;
-    int generationDeltaTime_;
-
-    bool updating_objects_stack_;
-    bool sending_;
-};
+}
 
 #endif /* CPM_APPLICATION_HPP_EUIC2VFR */
diff --git a/include/autoware_v2x/v2x_app.hpp b/include/autoware_v2x/v2x_app.hpp
index 8a66e2d..26e80b8 100644
--- a/include/autoware_v2x/v2x_app.hpp
+++ b/include/autoware_v2x/v2x_app.hpp
@@ -13,23 +13,26 @@
 #include "autoware_v2x/positioning.hpp"
 #include "autoware_v2x/security.hpp"
 #include "autoware_v2x/router_context.hpp"
+// #include "autoware_v2x/v2x_node.hpp"
 
 namespace v2x
 {
+  class V2XNode;
   class V2XApp
   {
   public:
-    V2XApp(rclcpp::Node*);
+    V2XApp(V2XNode *);
     void start();
     void objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr);
     void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr);
 
     CpmApplication *cp;
+    // V2XNode *v2x_node;
 
   private:
     friend class CpmApplication;
     friend class Application;
-    rclcpp::Node* node_;
+    V2XNode* node_;
     bool tf_received_;
     bool cp_started_;
   };
diff --git a/include/autoware_v2x/v2x_node.hpp b/include/autoware_v2x/v2x_node.hpp
index 6990263..9fd9cb3 100644
--- a/include/autoware_v2x/v2x_node.hpp
+++ b/include/autoware_v2x/v2x_node.hpp
@@ -1,3 +1,6 @@
+#ifndef V2X_NODE_HPP_EUIC2VFR
+#define V2X_NODE_HPP_EUIC2VFR
+
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "autoware_perception_msgs/msg/dynamic_object_array.hpp"
@@ -19,34 +22,23 @@ namespace v2x
   public:
     explicit V2XNode(const rclcpp::NodeOptions &node_options);
     V2XApp *app;
+    void publishObjects(std::vector<CpmApplication::Object> *);
 
   private:
     void objectsCallback(
         const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg);
     void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg);
 
-    // friend class CpmApplication;
-    // friend class Application;
     rclcpp::Subscription<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr subscription_;
     rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_pos_;
+    rclcpp::Publisher<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr publisher_;
 
-    // std::unique_ptr<CpmApplication> cp_;
-    // std::unique_ptr<Application> app_;
-    // boost::asio::io_service io_service_;
-    // TimeTrigger trigger_;
-    // vanetza::Runtime& runtime_;
-    // const char *device_name_;
-    // EthernetDevice device_;
-    // vanetza::MacAddress mac_address_;
-    // std::unique_ptr<LinkLayer> link_layer_;
-    // vanetza::geonet::MIB mib_;
-    // std::unique_ptr<vanetza::PositionProvider> positioning_;
-    // std::unique_ptr<vanetza::security::SecurityEntity> security_;
-    // RouterContext context_;
+    rclcpp::Time received_time;
+    bool cpm_received;
 
     double pos_lat_;
     double pos_lon_;
-    // vanetza::PositionFix reference_position_;
-
   };
-}
\ No newline at end of file
+}
+
+#endif
\ No newline at end of file
diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp
index 076cf5b..a639a87 100644
--- a/src/cpm_application.cpp
+++ b/src/cpm_application.cpp
@@ -2,6 +2,7 @@
 #include "autoware_v2x/positioning.hpp"
 #include "autoware_v2x/security.hpp"
 #include "autoware_v2x/link_layer.hpp"
+#include "autoware_v2x/v2x_node.hpp"
 
 #include "rclcpp/rclcpp.hpp"
 #include <vanetza/btp/ports.hpp>
@@ -13,6 +14,9 @@
 #include <iostream>
 #include <sstream>
 #include <exception>
+#include <GeographicLib/UTMUPS.hpp>
+#include <GeographicLib/MGRS.hpp>
+#include <string>
 
 #define _USE_MATH_DEFINES
 #include <math.h>
@@ -21,245 +25,269 @@ using namespace vanetza;
 using namespace vanetza::facilities;
 using namespace std::chrono;
 
-CpmApplication::CpmApplication(rclcpp::Node *node, Runtime &rt) : 
-  node_(node),
-  runtime_(rt),
-  ego_x_(0),
-  ego_y_(0),
-  ego_lat_(0),
-  ego_lon_(0),
-  ego_altitude_(0),
-  ego_heading_(0),
-  generationDeltaTime_(0),
-  updating_objects_stack_(false),
-  sending_(false)
+namespace v2x
 {
-  RCLCPP_INFO(node_->get_logger(), "CpmApplication started...");
-  set_interval(milliseconds(1000));
-}
-
-void CpmApplication::set_interval(Clock::duration interval)
-{
-  cpm_interval_ = interval;
-  runtime_.cancel(this);
-  schedule_timer();
-}
-
-void CpmApplication::schedule_timer()
-{
-  runtime_.schedule(cpm_interval_, std::bind(&CpmApplication::on_timer, this, std::placeholders::_1), this);
-}
-
-void CpmApplication::on_timer(Clock::time_point)
-{
-  schedule_timer();
-  send();
-}
-
-CpmApplication::PortType CpmApplication::port()
-{
-  return btp::ports::CPM;
-}
-
-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)
+  CpmApplication::CpmApplication(V2XNode *node, Runtime &rt) :     
+    node_(node),
+    runtime_(rt),
+    ego_x_(0),
+    ego_y_(0),
+    ego_lat_(0),
+    ego_lon_(0),
+    ego_altitude_(0),
+    ego_heading_(0),
+    generationDeltaTime_(0),
+    updating_objects_stack_(false),
+    sending_(false)
   {
-    RCLCPP_INFO(node_->get_logger(), "Received decodable CPM content");
-    asn1::Cpm message = *cpm;
-    ItsPduHeader_t &header = message->header;
-
-    CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer;
-    double lat = management.referencePosition.latitude;
-    double lon = management.referencePosition.longitude;
-    RCLCPP_INFO(node_->get_logger(), "cpm.(reference position) = %f, %f", lat, lon);
-  }
-  else
-  {
-    RCLCPP_INFO(node_->get_logger(), "Received broken content");
-  }
-}
-
-void CpmApplication::updateMGRS(double *x, double *y) {
-  RCLCPP_INFO(node_->get_logger(), "Update MGRS");
-  ego_x_ = *x;
-  ego_y_ = *y;
-}
-
-void CpmApplication::updateRP(double *lat, double *lon, double *altitude)
-{
-  RCLCPP_INFO(node_->get_logger(), "Update RP");
-  ego_lat_ = *lat;
-  ego_lon_ = *lon;
-  ego_altitude_ = *altitude;
-}
-
-void CpmApplication::updateGenerationDeltaTime(int *gdt)
-{
-  RCLCPP_INFO(node_->get_logger(), "Update GDT");
-  generationDeltaTime_ = *gdt;
-}
-
-void CpmApplication::updateHeading(double *yaw)
-{
-  RCLCPP_INFO(node_->get_logger(), "Update Heading : %f", *yaw);
-  ego_heading_ = *yaw;
-}
-
-void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg)
-{
-  RCLCPP_INFO(node_->get_logger(), "Update ObjectsStack");
-  updating_objects_stack_ = true;
-
-  if (sending_)
-  {
-    return;
+    RCLCPP_INFO(node_->get_logger(), "CpmApplication started...");
+    set_interval(milliseconds(1000));
   }
 
-  if (msg->objects.size() > 0)
+  void CpmApplication::set_interval(Clock::duration interval)
   {
-    RCLCPP_INFO(node_->get_logger(), "At least 1 object detected");
+    cpm_interval_ = interval;
+    runtime_.cancel(this);
+    schedule_timer();
+  }
 
-    // Initialize ObjectsStack
-    // std::vector<CpmApplication::Object> objectsStack;
-    objectsStack.clear();
+  void CpmApplication::schedule_timer()
+  {
+    runtime_.schedule(cpm_interval_, std::bind(&CpmApplication::on_timer, this, std::placeholders::_1), this);
+  }
 
-    // Create CpmApplication::Object per msg->object and add it to objectsStack
-    int i = 0;
-    for (auto obj : msg->objects)
+  void CpmApplication::on_timer(Clock::time_point)
+  {
+    schedule_timer();
+    send();
+  }
+
+  CpmApplication::PortType CpmApplication::port()
+  {
+    return btp::ports::CPM;
+  }
+
+  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)
     {
-      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.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);
-      object.timeOfMeasurement = 100;
-      objectsStack.push_back(object);
-      ++i;
+      RCLCPP_INFO(node_->get_logger(), "Received decodable CPM content");
+      asn1::Cpm message = *cpm;
+      ItsPduHeader_t &header = message->header;
+
+      CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer;
+      double lat = management.referencePosition.latitude / 1.0e7;
+      double lon = management.referencePosition.longitude / 1.0e7;
+      RCLCPP_INFO(node_->get_logger(), "cpm.(reference position) = %f, %f", lat, lon);
+
+      std::string mgrs;
+      int zone;
+      bool northp;
+      double x, y;
+      GeographicLib::UTMUPS::Forward(lat, lon, zone, northp, x, y);
+      GeographicLib::MGRS::Forward(zone, northp, x, y, lat, 5, mgrs);
+
+      int x_mgrs = std::stoi(mgrs.substr(5, 5));
+      int y_mgrs = std::stoi(mgrs.substr(10, 5));
+      RCLCPP_INFO(node_->get_logger(), "cpm.(RP).mgrs = %s, %d, %d", mgrs.c_str(), x_mgrs, y_mgrs);
+
+      // Calculate orientation from Heading
+      OriginatingVehicleContainer_t &ovc = message->cpm.cpmParameters.stationDataContainer->choice.originatingVehicleContainer;
+      int heading = ovc.heading.headingValue;
+      double orientation = 1.5708 - (M_PI * heading / 10) / 180;
+      RCLCPP_INFO(node_->get_logger(), "cpm: heading = %d, orientation = %f", heading, orientation);
+
+      // Get PerceivedObjects
+      receivedObjectsStack.clear();
+      PerceivedObjectContainer_t *&poc = message->cpm.cpmParameters.perceivedObjectContainer;
+      for (int i = 0; i < poc->list.count; ++i)
+      {
+        // RCLCPP_INFO(node_->get_logger(), "cpm: %d", poc->list.array[i]->objectID);
+        CpmApplication::Object object;
+        double x1 = poc->list.array[i]->xDistance.value;
+        double y1 = poc->list.array[i]->yDistance.value;
+        object.position_x = x_mgrs - (cos(orientation) * x1 - sin(orientation) * y1);
+        object.position_y = y_mgrs - (sin(orientation) * x1 + cos(orientation) * y1);
+        receivedObjectsStack.push_back(object);
+      }
+      node_->publishObjects(&receivedObjectsStack);
+    }
+    else
+    {
+      RCLCPP_INFO(node_->get_logger(), "Received broken content");
     }
   }
-  RCLCPP_INFO(node_->get_logger(), "%d objects added to objectsStack", objectsStack.size());
-  updating_objects_stack_ = false;
-}
 
-void CpmApplication::send()
-{
-  sending_ = true;
-  if (!updating_objects_stack_)
+  void CpmApplication::updateMGRS(double *x, double *y)
   {
-    RCLCPP_INFO(node_->get_logger(), "Sending CPM...");
-
-    // Send all objects in 1 CPM message
-    vanetza::asn1::Cpm message;
-
-    // ITS PDU Header
-    ItsPduHeader_t &header = message->header;
-    header.protocolVersion = 1;
-    header.messageID = 14;
-    header.stationID = 1;
-
-    // const auto time_now = duration_cast<milliseconds>(system_clock::now().time_since_epoch());
-    // uint16_t gen_delta_time = time_now.count();
-
-    CollectivePerceptionMessage_t &cpm = message->cpm;
-    cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec;
-
-    // auto position = positioning->position_specify(pos_lat, pos_lon);
-
-    CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer;
-    management.stationType = StationType_passengerCar;
-    // management.referencePosition.latitude = pos_lat;
-    // management.referencePosition.longitude = pos_lon;
-    // management.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 1.0;
-    // management.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 1.0;
-    PositionFix fix;
-    // fix.timestamp = time_now;
-    fix.latitude = ego_lat_ * units::degree;
-    fix.longitude = ego_lon_ * units::degree;
-    // fix.altitude = ego_altitude_;
-    fix.confidence.semi_major = 1.0 * units::si::meter;
-    fix.confidence.semi_minor = fix.confidence.semi_major;
-    copy(fix, management.referencePosition);
-
-    // cpm.cpmParameters.stationDataContainer = NULL;
-    // cpm.cpmParameters.perceivedObjectContainer = NULL;
-    cpm.cpmParameters.numberOfPerceivedObjects = objectsStack.size();
-
-    StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer;
-    sdc = vanetza::asn1::allocate<StationDataContainer_t>();
-    // RCLCPP_INFO(node->get_logger(), "Allocated sdc");
-    sdc->present = StationDataContainer_PR_originatingVehicleContainer;
-    OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer;
-    ovc.speed.speedValue = 0;
-    ovc.speed.speedConfidence = 1;
-    // ovc.heading.headingValue = (int) (1.5708 - ego_heading_) * M_PI / 180;
-    RCLCPP_INFO(node_->get_logger(), "headingValue...");
-    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...");
-    PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer;
-    poc = vanetza::asn1::allocate<PerceivedObjectContainer_t>();
-    // cpm.cpmParameters.perceivedObjectContainer = poc;
-    // PerceivedObjectContainer_t pocc = *poc;
-    // RCLCPP_INFO(node->get_logger(), "Allocated poc");
-
-    for (CpmApplication::Object object : objectsStack)
-    {
-      PerceivedObject *pObj = vanetza::asn1::allocate<PerceivedObject>();
-      pObj->objectID = object.objectID;
-      pObj->timeOfMeasurement = object.timeOfMeasurement;
-      pObj->xDistance.value = object.xDistance;
-      pObj->xDistance.confidence = 1;
-      pObj->yDistance.value = object.yDistance;
-      pObj->yDistance.confidence = 1;
-      pObj->xSpeed.value = object.xSpeed;
-      pObj->xSpeed.confidence = 1;
-      pObj->ySpeed.value = object.ySpeed;
-      pObj->ySpeed.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(), "1");
-    Application::DownPacketPtr packet{new DownPacket()};
-    RCLCPP_INFO(node_->get_logger(), "2");
-    std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
-    RCLCPP_INFO(node_->get_logger(), "3");
-    // std::shared_ptr<asn1::Cpm> message_p = std::make_shared<asn1::Cpm>(message);
-    // std::unique_ptr<convertible::byte_buffer> buffer { new convertible::byte_buffer_impl<asn1::Cpm>(&message)};
-
-    payload->layer(OsiLayer::Application) = std::move(message);
-
-    RCLCPP_INFO(node_->get_logger(), "4");
-
-    Application::DataRequest request;
-    request.its_aid = aid::CP;
-    request.transport_type = geonet::TransportType::SHB;
-    request.communication_profile = geonet::CommunicationProfile::ITS_G5;
-
-    // RCLCPP_INFO(node->get_logger(), "Packet Size: %d", payload->size());
-
-    // RCLCPP_INFO(node->get_logger(), "Going to Application::request...");
-    RCLCPP_INFO(node_->get_logger(), "5");
-    Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
-    RCLCPP_INFO(node_->get_logger(), "6");
-    if (!confirm.accepted())
-    {
-      throw std::runtime_error("CPM application data request failed");
-    }
+    // RCLCPP_INFO(node_->get_logger(), "Update MGRS");
+    ego_x_ = *x;
+    ego_y_ = *y;
   }
-  sending_ = false;
-  // RCLCPP_INFO(node->get_logger(), "Application::request END");
-}
+
+  void CpmApplication::updateRP(double *lat, double *lon, double *altitude)
+  {
+    // RCLCPP_INFO(node_->get_logger(), "Update RP");
+    ego_lat_ = *lat;
+    ego_lon_ = *lon;
+    ego_altitude_ = *altitude;
+  }
+
+  void CpmApplication::updateGenerationDeltaTime(int *gdt)
+  {
+    // RCLCPP_INFO(node_->get_logger(), "Update GDT");
+    generationDeltaTime_ = *gdt;
+  }
+
+  void CpmApplication::updateHeading(double *yaw)
+  {
+    // RCLCPP_INFO(node_->get_logger(), "Update Heading : %f", *yaw);
+    ego_heading_ = *yaw;
+  }
+
+  void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg)
+  {
+    RCLCPP_INFO(node_->get_logger(), "Update ObjectsStack");
+    updating_objects_stack_ = true;
+
+    if (sending_)
+    {
+      RCLCPP_INFO(node_->get_logger(), "updateObjectsStack Skipped...");
+      return;
+    }
+
+    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
+      int i = 0;
+      for (auto obj : msg->objects)
+      {
+        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.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);
+        object.timeOfMeasurement = 100;
+        objectsStack.push_back(object);
+        ++i;
+      }
+    }
+    RCLCPP_INFO(node_->get_logger(), "%d objects added to objectsStack", objectsStack.size());
+    updating_objects_stack_ = false;
+  }
+
+  void CpmApplication::send()
+  {
+    sending_ = true;
+    if (!updating_objects_stack_ && objectsStack.size() > 0)
+    {
+      RCLCPP_INFO(node_->get_logger(), "Sending CPM...");
+
+      // Send all objects in 1 CPM message
+      vanetza::asn1::Cpm message;
+
+      // ITS PDU Header
+      ItsPduHeader_t &header = message->header;
+      header.protocolVersion = 1;
+      header.messageID = 14;
+      header.stationID = 1;
+
+      // const auto time_now = duration_cast<milliseconds>(system_clock::now().time_since_epoch());
+      // uint16_t gen_delta_time = time_now.count();
+
+      CollectivePerceptionMessage_t &cpm = message->cpm;
+      cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec;
+
+      // auto position = positioning->position_specify(pos_lat, pos_lon);
+
+      CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer;
+      management.stationType = StationType_passengerCar;
+      // management.referencePosition.latitude = pos_lat;
+      // management.referencePosition.longitude = pos_lon;
+      // management.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 1.0;
+      // management.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 1.0;
+      PositionFix fix;
+      // fix.timestamp = time_now;
+      fix.latitude = ego_lat_ * units::degree;
+      fix.longitude = ego_lon_ * units::degree;
+      // fix.altitude = ego_altitude_;
+      fix.confidence.semi_major = 1.0 * units::si::meter;
+      fix.confidence.semi_minor = fix.confidence.semi_major;
+      copy(fix, management.referencePosition);
+
+      // cpm.cpmParameters.stationDataContainer = NULL;
+      // cpm.cpmParameters.perceivedObjectContainer = NULL;
+      cpm.cpmParameters.numberOfPerceivedObjects = objectsStack.size();
+
+      StationDataContainer_t *&sdc = cpm.cpmParameters.stationDataContainer;
+      sdc = vanetza::asn1::allocate<StationDataContainer_t>();
+      // RCLCPP_INFO(node->get_logger(), "Allocated sdc");
+      sdc->present = StationDataContainer_PR_originatingVehicleContainer;
+      OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer;
+      ovc.speed.speedValue = 0;
+      ovc.speed.speedConfidence = 1;
+      // ovc.heading.headingValue = (int) (1.5708 - ego_heading_) * M_PI / 180;
+      // RCLCPP_INFO(node_->get_logger(), "headingValue...");
+      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...");
+      PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer;
+      poc = vanetza::asn1::allocate<PerceivedObjectContainer_t>();
+
+      for (CpmApplication::Object object : objectsStack)
+      {
+        PerceivedObject *pObj = vanetza::asn1::allocate<PerceivedObject>();
+        pObj->objectID = object.objectID;
+        pObj->timeOfMeasurement = object.timeOfMeasurement;
+        pObj->xDistance.value = object.xDistance;
+        pObj->xDistance.confidence = 1;
+        pObj->yDistance.value = object.yDistance;
+        pObj->yDistance.confidence = 1;
+        pObj->xSpeed.value = object.xSpeed;
+        pObj->xSpeed.confidence = 1;
+        pObj->ySpeed.value = object.ySpeed;
+        pObj->ySpeed.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);
+      }
+      Application::DownPacketPtr packet{new DownPacket()};
+      std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
+      // std::shared_ptr<asn1::Cpm> message_p = std::make_shared<asn1::Cpm>(message);
+      // std::unique_ptr<convertible::byte_buffer> buffer { new convertible::byte_buffer_impl<asn1::Cpm>(&message)};
+
+      payload->layer(OsiLayer::Application) = std::move(message);
+
+      Application::DataRequest request;
+      request.its_aid = aid::CP;
+      request.transport_type = geonet::TransportType::SHB;
+      request.communication_profile = geonet::CommunicationProfile::ITS_G5;
+      // RCLCPP_INFO(node->get_logger(), "Packet Size: %d", payload->size());
+      // RCLCPP_INFO(node->get_logger(), "Going to Application::request...");
+      Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
+      if (!confirm.accepted())
+      {
+        throw std::runtime_error("CPM application data request failed");
+      }
+    }
+    sending_ = false;
+    // RCLCPP_INFO(node->get_logger(), "Application::request END");
+  }
+}
\ No newline at end of file
diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp
index afecd95..15b25fd 100644
--- a/src/v2x_app.cpp
+++ b/src/v2x_app.cpp
@@ -1,4 +1,5 @@
 #include "autoware_v2x/v2x_app.hpp"
+#include "autoware_v2x/v2x_node.hpp"
 #include "autoware_v2x/time_trigger.hpp"
 #include "autoware_v2x/router_context.hpp"
 #include "autoware_v2x/positioning.hpp"
@@ -29,7 +30,7 @@ using namespace std::chrono;
 
 namespace v2x
 {
-  V2XApp::V2XApp(rclcpp::Node *node) : 
+  V2XApp::V2XApp(V2XNode *node) : 
     node_(node),
     tf_received_(false),
     cp_started_(false)
@@ -37,14 +38,14 @@ namespace v2x
   }
 
   void V2XApp::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) {
-    RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received");
+    // RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received");
     if (tf_received_ && cp_started_) {
       cp->updateObjectsStack(msg);
     }
   }
 
   void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
-    RCLCPP_INFO(node_->get_logger(), "V2XApp: tf msg received");
+    // RCLCPP_INFO(node_->get_logger(), "V2XApp: tf msg received");
     tf_received_ = true;
 
     double x = msg->transforms[0].transform.translation.x;
@@ -71,15 +72,15 @@ namespace v2x
     double x_mgrs, y_mgrs;
     double lat, lon;
     sprintf(mgrs, "54SVE%.5d%.5d", (int)x, (int)y);
-    RCLCPP_INFO(node_->get_logger(), "MGRS: %s", mgrs);
+    // RCLCPP_INFO(node_->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);
 
-    RCLCPP_INFO(node_->get_logger(), "Ego Position Lat/Lon: %f, %f", lat, lon);
-    RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f, %f, %f, %f", rot_x, rot_y, rot_z, rot_w);
-    RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f %f %f", roll, pitch, yaw);
-    RCLCPP_INFO(node_->get_logger(), "Timestamp: %d, GDT: %d", timestamp, gdt);
+    // RCLCPP_INFO(node_->get_logger(), "Ego Position Lat/Lon: %f, %f", lat, lon);
+    // RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f, %f, %f, %f", rot_x, rot_y, rot_z, rot_w);
+    // RCLCPP_INFO(node_->get_logger(), "Ego Orientation: %f %f %f", roll, pitch, yaw);
+    // RCLCPP_INFO(node_->get_logger(), "Timestamp: %d, GDT: %d", timestamp, gdt);
 
     if (cp && cp_started_) {
       cp->updateMGRS(&x, &y);
@@ -95,7 +96,7 @@ namespace v2x
     boost::asio::io_service io_service;
     TimeTrigger trigger(io_service);
 
-    const char* device_name = "wlp4s0";
+    const char* device_name = "vmnet1";
     EthernetDevice device(device_name);
     vanetza::MacAddress mac_address = device.address();
 
diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp
index f80daf8..210fbd6 100644
--- a/src/v2x_node.cpp
+++ b/src/v2x_node.cpp
@@ -25,88 +25,74 @@ 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_),
-  // runtime_(new Runtime(Clock::at(boost::posix_time::microsec_clock::universal_time()))),
-  // 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_(),
-  // cp_(new CpmApplication(this, runtime_)),
-  // context_(mib_, trigger_, *positioning_, security_.get()),
-  // app_()
+  V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options), received_time(this->now()), cpm_received(false)
   {
     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_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();
+    publisher_ = create_publisher<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/objects", rclcpp::QoS{10});
 
-    // 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_(mib_, trigger_, *positioning_, security_.get()),
-    // 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();
-    // boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_));
     app = new V2XApp(this);
     boost::thread v2xApp(boost::bind(&V2XApp::start, app));
-    // boost::thread v2xApp(V2XApp);
-    // boost::thread v2xApp(&V2XApp, this);
 
-    // // Print MAC Address to logger
-    // std::stringstream sout;
-    // sout << mac_address;
-    // RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str());
+    RCLCPP_INFO(get_logger(), "V2X Node Launched");
+  }
+
+  void V2XNode::publishObjects(std::vector<CpmApplication::Object> *objectsStack) {
+    autoware_perception_msgs::msg::DynamicObjectArray output_dynamic_object_msg;
+    std_msgs::msg::Header header;
+    rclcpp::Time current_time = this->now();
+    received_time = current_time;
+    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;
+
+      semantic.type = autoware_perception_msgs::msg::Semantic::CAR;
+      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;
+
+      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;
+
+      object.semantic = semantic;
+      object.shape = shape;
+      object.state = state;
+
+      output_dynamic_object_msg.objects.push_back(object);
+    }
+    cpm_received = true;
+    publisher_->publish(output_dynamic_object_msg);
   }
 
   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_);
-    // cp_->updateObjectsStack(msg, this);
-    app->objectsCallback(msg);
-
+    rclcpp::Time current_time = this->now();
+    // RCLCPP_INFO(get_logger(), "%ld", current_time.nanoseconds() - received_time.nanoseconds());
+    if (cpm_received) {
+      // if time difference is more than 10ms
+      if (current_time - received_time > rclcpp::Duration(std::chrono::nanoseconds(10000000))) {
+        app->objectsCallback(msg);
+      }
+      cpm_received = false;
+    } else {
+      app->objectsCallback(msg);
+    }
   }
 
   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);
-
     app->tfCallback(msg);
   }
 }