diff --git a/CMakeLists.txt b/CMakeLists.txt
index a6e5679..5227c07 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -32,6 +32,7 @@ include_directories(
 
 ament_auto_add_library(autoware_v2x SHARED
   src/v2x_node.cpp
+  src/v2x_app.cpp
   src/application.cpp
   src/cpm_application.cpp
   src/ethernet_device.cpp
diff --git a/include/autoware_v2x/cpm_application.hpp b/include/autoware_v2x/cpm_application.hpp
index 47916d5..79f8d52 100644
--- a/include/autoware_v2x/cpm_application.hpp
+++ b/include/autoware_v2x/cpm_application.hpp
@@ -11,13 +11,53 @@
 class CpmApplication : public Application
 {
 public:
-    CpmApplication(rclcpp::Node *node);
+    CpmApplication(rclcpp::Node *node, vanetza::Runtime&);
     PortType port() override;
     void indicate(const DataIndication &, UpPacketPtr) override;
-    void send(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr, rclcpp::Node *, double, double);
+    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;
+        double xDistance;
+        double yDistance;
+        double xSpeed;
+        double ySpeed;
+        vanetza::PositionFix position;
+        int timeOfMeasurement;
+    };
+    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
new file mode 100644
index 0000000..8a66e2d
--- /dev/null
+++ b/include/autoware_v2x/v2x_app.hpp
@@ -0,0 +1,38 @@
+#ifndef V2X_APP_HPP_EUIC2VFR
+#define V2X_APP_HPP_EUIC2VFR
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "autoware_perception_msgs/msg/dynamic_object_array.hpp"
+#include "tf2_msgs/msg/tf_message.hpp"
+#include <boost/asio/io_service.hpp>
+#include "autoware_v2x/cpm_application.hpp"
+#include "autoware_v2x/time_trigger.hpp"
+#include "autoware_v2x/link_layer.hpp"
+#include "autoware_v2x/ethernet_device.hpp"
+#include "autoware_v2x/positioning.hpp"
+#include "autoware_v2x/security.hpp"
+#include "autoware_v2x/router_context.hpp"
+
+namespace v2x
+{
+  class V2XApp
+  {
+  public:
+    V2XApp(rclcpp::Node*);
+    void start();
+    void objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr);
+    void tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr);
+
+    CpmApplication *cp;
+
+  private:
+    friend class CpmApplication;
+    friend class Application;
+    rclcpp::Node* node_;
+    bool tf_received_;
+    bool cp_started_;
+  };
+}
+
+#endif
\ No newline at end of file
diff --git a/include/autoware_v2x/v2x_node.hpp b/include/autoware_v2x/v2x_node.hpp
index fb3de5d..6990263 100644
--- a/include/autoware_v2x/v2x_node.hpp
+++ b/include/autoware_v2x/v2x_node.hpp
@@ -3,6 +3,7 @@
 #include "autoware_perception_msgs/msg/dynamic_object_array.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 #include <boost/asio/io_service.hpp>
+#include "autoware_v2x/v2x_app.hpp"
 #include "autoware_v2x/cpm_application.hpp"
 #include "autoware_v2x/time_trigger.hpp"
 #include "autoware_v2x/link_layer.hpp"
@@ -17,32 +18,35 @@ namespace v2x
   {
   public:
     explicit V2XNode(const rclcpp::NodeOptions &node_options);
+    V2XApp *app;
 
   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;
+    // friend class CpmApplication;
+    // friend class Application;
     rclcpp::Subscription<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr subscription_;
     rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_pos_;
 
-    std::unique_ptr<CpmApplication> cp_;
-    std::unique_ptr<Application> app_;
-    boost::asio::io_service io_service_;
-    TimeTrigger trigger_;
-    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_;
+    // 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_;
 
     double pos_lat_;
     double pos_lon_;
+    // vanetza::PositionFix reference_position_;
 
   };
 }
\ No newline at end of file
diff --git a/src/cpm_application.cpp b/src/cpm_application.cpp
index a8d1111..1d98020 100644
--- a/src/cpm_application.cpp
+++ b/src/cpm_application.cpp
@@ -14,15 +14,47 @@
 #include <sstream>
 #include <exception>
 
+#define _USE_MATH_DEFINES
+#include <math.h>
+
 // This is a very simple application that sends BTP-B messages with the content 0xc0ffee.
 
 using namespace vanetza;
 using namespace vanetza::facilities;
 using namespace std::chrono;
 
-CpmApplication::CpmApplication(rclcpp::Node *node) : node_(node)
+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)
 {
   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()
@@ -34,9 +66,12 @@ void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr pack
 {
   asn1::PacketVisitor<asn1::Cpm> visitor;
   std::shared_ptr<const asn1::Cpm> cpm = boost::apply_visitor(visitor, *packet);
-  if (cpm) {
+  if (cpm)
+  {
     RCLCPP_INFO(node_->get_logger(), "Received decodable CPM content");
-  } else {
+  }
+  else
+  {
     RCLCPP_INFO(node_->get_logger(), "Received broken content");
   }
   asn1::Cpm message = *cpm;
@@ -50,121 +85,174 @@ void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr pack
   RCLCPP_INFO(node_->get_logger(), "cpm.(reference position) = %f, %f", lat, lon);
 }
 
-void CpmApplication::send(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg, rclcpp::Node *node, double pos_lat, double pos_lon)
-{
-  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 = gen_delta_time * 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 = pos_lat * units::degree;
-  fix.longitude = pos_lon * units::degree;
-  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 = msg->objects.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 = 0;
-  ovc.heading.headingConfidence = 1;
-
-  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 (auto object : msg->objects) {
-    PerceivedObject *pObj = vanetza::asn1::allocate<PerceivedObject>();
-    pObj->objectID = 1;
-    pObj->timeOfMeasurement = 10;
-    pObj->xDistance.value = 100;
-    pObj->xDistance.confidence = 1;
-    pObj->yDistance.value = 200;
-    pObj->yDistance.confidence = 1;
-    pObj->xSpeed.value = 5;
-    pObj->xSpeed.confidence = 1;
-    pObj->ySpeed.value = 0;
-    pObj->ySpeed.confidence = 1;
-      
-    ASN_SEQUENCE_ADD(poc, pObj);
-    // RCLCPP_INFO(node->get_logger(), "Added one object to poc->list");
-  }
-
-
-  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");
-  }
-  // RCLCPP_INFO(node->get_logger(), "Application::request END");
+void CpmApplication::updateMGRS(double *x, double *y) {
+  RCLCPP_INFO(node_->get_logger(), "Update MGRS");
+  ego_x_ = *x;
+  ego_y_ = *y;
 }
 
-// void HelloApplication::schedule_timer()
-// {
-//     timer_.expires_from_now(interval_);
-//     timer_.async_wait(std::bind(&HelloApplication::on_timer, this, std::placeholders::_1));
-// }
+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 HelloApplication::on_timer(const boost::system::error_code& ec)
-// {
-//     if (ec != boost::asio::error::operation_aborted) {
-//         DownPacketPtr packet { new DownPacket() };
-//         packet->layer(OsiLayer::Application) = ByteBuffer { 0xC0, 0xFF, 0xEE };
-//         DataRequest request;
-//         request.transport_type = geonet::TransportType::SHB;
-//         request.communication_profile = geonet::CommunicationProfile::ITS_G5;
-//         request.its_aid = aid::CA;
-//         auto confirm = Application::request(request, std::move(packet));
-//         if (!confirm.accepted()) {
-//             throw std::runtime_error("Hello application data request failed");
-//         }
+void CpmApplication::updateGenerationDeltaTime(int *gdt)
+{
+  RCLCPP_INFO(node_->get_logger(), "Update GDT");
+  generationDeltaTime_ = *gdt;
+}
 
-//         schedule_timer();
-//     }
-// }
+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;
+  }
+
+  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 = ((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100;
+      object.yDistance = ((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100;
+      // RCLCPP_INFO(node_->get_logger(), "xDistance: %f, yDistance: %f", object.xDistance, object.yDistance);
+      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_)
+  {
+    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;
+    ovc.heading.headingValue = (int) std::fmod((1.5708-ego_heading_) * 180 / M_PI, 360.0) * 10;
+    ovc.heading.headingConfidence = 1;
+
+    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(), "Added one object to poc->list");
+    }
+
+    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");
+}
diff --git a/src/v2x_app.cpp b/src/v2x_app.cpp
new file mode 100644
index 0000000..a6d9fdc
--- /dev/null
+++ b/src/v2x_app.cpp
@@ -0,0 +1,185 @@
+#include "autoware_v2x/v2x_app.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 "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Matrix3x3.h"
+
+#include <vanetza/asn1/cpm.hpp>
+#include <vanetza/facilities/cpm_functions.hpp>
+#include <sstream>
+#include <memory>
+#include <GeographicLib/UTMUPS.hpp>
+#include <GeographicLib/MGRS.hpp>
+#include <boost/thread.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+
+namespace gn = vanetza::geonet;
+
+using namespace vanetza;
+using namespace vanetza::facilities;
+using namespace std::chrono;
+
+namespace v2x
+{
+  V2XApp::V2XApp(rclcpp::Node *node) : 
+    node_(node),
+    tf_received_(false),
+    cp_started_(false)
+    // 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(node_, trigger_.runtime())),
+    // context_(mib_, trigger_, *positioning_, security_.get()),
+    // app_()
+  {
+
+    // device_name_ = "wlp4s0";
+    // device_(device_name_);
+    // mac_address_ = device_.address();
+
+    // std::stringstream sout;
+    // sout << mac_address_;
+    // RCLCPP_INFO(node_->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_));
+
+    // // Print MAC Address to logger
+    // std::stringstream sout;
+    // sout << mac_address;
+    // RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str());
+  }
+
+  void V2XApp::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) {
+    RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received");
+    if (tf_received_) {
+      cp->updateObjectsStack(msg);
+    }
+  }
+
+  void V2XApp::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
+    RCLCPP_INFO(node_->get_logger(), "V2XApp: tf msg received");
+    tf_received_ = true;
+
+    double x = msg->transforms[0].transform.translation.x;
+    double y = msg->transforms[0].transform.translation.y;
+    double z = msg->transforms[0].transform.translation.z;
+    int timestamp = msg->transforms[0].header.stamp.sec;
+    int gdt = timestamp % 65536;
+
+    double rot_x = msg->transforms[0].transform.rotation.x;
+    double rot_y = msg->transforms[0].transform.rotation.y;
+    double rot_z = msg->transforms[0].transform.rotation.z;
+    double rot_w = msg->transforms[0].transform.rotation.w;
+
+    // Convert the quarternion to euler (yaw, pitch, roll)
+    tf2::Quaternion quat(rot_x, rot_y, rot_z, rot_w);
+    tf2::Matrix3x3 matrix(quat);
+    double roll, pitch, yaw;
+    matrix.getRPY(roll, pitch, yaw);
+
+
+    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(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);
+
+    if (cp && cp_started_) {
+      cp->updateMGRS(&x, &y);
+      cp->updateRP(&lat, &lon, &z);
+      cp->updateHeading(&yaw);
+      cp->updateGenerationDeltaTime(&gdt);
+    }
+  }
+
+  void V2XApp::start() {
+    RCLCPP_INFO(node_->get_logger(), "V2X App Launched");
+
+    boost::asio::io_service io_service;
+    TimeTrigger trigger(io_service);
+
+    const char* device_name = "wlp4s0";
+    EthernetDevice device(device_name);
+    vanetza::MacAddress mac_address = device.address();
+
+    std::stringstream sout;
+    sout << mac_address;
+    RCLCPP_INFO(node_->get_logger(), "MAC Address: '%s'", sout.str().c_str());
+
+    gn::MIB mib;
+    mib.itsGnLocalGnAddr.mid(mac_address);
+    mib.itsGnLocalGnAddr.is_manually_configured(true);
+    mib.itsGnLocalAddrConfMethod = geonet::AddrConfMethod::Managed;
+    mib.itsGnSecurity = false;
+    mib.itsGnProtocolVersion = 1;
+
+    // context_.updateMIB(mib_);
+    auto link_layer =  create_link_layer(io_service, device, "ethernet");
+    auto positioning = create_position_provider(io_service, trigger.runtime());
+    auto security = create_security_entity(trigger.runtime(), *positioning);
+    RouterContext context(mib, trigger, *positioning, security.get());
+
+    context.set_link_layer(link_layer.get());
+
+    cp = new CpmApplication(node_, trigger.runtime());
+
+    // context.enable(cp_.get());
+    context.enable(cp);
+
+    cp_started_ = true;
+
+    io_service.run();
+  }
+}
\ No newline at end of file
diff --git a/src/v2x_node.cpp b/src/v2x_node.cpp
index 6ddc439..f80daf8 100644
--- a/src/v2x_node.cpp
+++ b/src/v2x_node.cpp
@@ -1,4 +1,5 @@
 #include "autoware_v2x/v2x_node.hpp"
+#include "autoware_v2x/v2x_app.hpp"
 #include "autoware_v2x/time_trigger.hpp"
 #include "autoware_v2x/router_context.hpp"
 #include "autoware_v2x/positioning.hpp"
@@ -13,9 +14,8 @@
 #include <vanetza/facilities/cpm_functions.hpp>
 #include <sstream>
 #include <memory>
-#include <GeographicLib/UTMUPS.hpp>
-#include <GeographicLib/MGRS.hpp>
 #include <boost/thread.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
 
 namespace gn = vanetza::geonet;
 
@@ -25,23 +25,23 @@ 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_(),
-    cp_(new CpmApplication(this)),
-    context_(mib_, trigger_, *positioning_, security_.get()),
-    app_()
+  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_()
   {
     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_ = 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");
@@ -50,38 +50,41 @@ namespace v2x
     // device_(device_name_);
     // mac_address_ = device_.address();
 
-    std::stringstream sout;
-    sout << mac_address_;
-    RCLCPP_INFO(get_logger(), "MAC Address: '%s'", sout.str().c_str());
+    // 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;
+    // 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_);
+    // 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());
+    // context_.set_link_layer(link_layer_.get());
 
     // std::unique_ptr<CpmApplication> cp_ { new CpmApplication(this) };
     // app_ = std::move(cp_);
 
-    context_.enable(cp_.get());
-
+    // context_.enable(cp_.get());
 
     // io_service_.run();
-    boost::thread(boost::bind(&boost::asio::io_service::run, &io_service_));
+    // 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;
@@ -94,31 +97,17 @@ namespace v2x
     // 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_->send(msg, this, pos_lat_, pos_lon_);
+    // cp_->updateObjectsStack(msg, this);
+    app->objectsCallback(msg);
+
   }
 
-  void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr 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);
 
-    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);
+    app->tfCallback(msg);
   }
 }