Merge pull request #3 from tlab-wide/fix/position

Simple refactoring

include/autoware_v2x/cpm_application.hpp

@@ -58,17 +58,24 @@ namespace v2x
         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_;
+        struct Ego_station {
+            double mgrs_x;
+            double mgrs_y;
+            double latitude;
+            double longitude;
+            double altitude;
+            double heading;
+        };
+
+        Ego_station ego_;
+        
         int generationDeltaTime_;
         long long gdt_timestamp_;
 
         bool updating_objects_stack_;
         bool sending_;
+        bool is_sender_;
+        bool reflect_packet_;
     };
 }
 

include/autoware_v2x/v2x_node.hpp

@@ -33,9 +33,6 @@ namespace v2x
     rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr subscription_pos_;
     rclcpp::Publisher<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr publisher_;
 
-    rclcpp::Time received_time;
-    bool cpm_received;
-
     double pos_lat_;
     double pos_lon_;
   };

launch/v2x.launch.xml

@@ -1,6 +1,6 @@
 <launch>
   <arg name="network_interface" default="wlp5s0"/>
-  <node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen" emulate_tty="true">
+  <node pkg="autoware_v2x" exec="autoware_v2x_node" namespace="v2x" output="screen">
     <param name="network_interface" value="$(var network_interface)"/>
   </node>
 </launch>

src/cpm_application.cpp

@@ -28,54 +28,51 @@ using namespace vanetza;
 using namespace vanetza::facilities;
 using namespace std::chrono;
 
-namespace v2x
-{
+namespace v2x {
   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),
+    ego_(),
     generationDeltaTime_(0),
     updating_objects_stack_(false),
-    sending_(false)
+    sending_(false),
+    is_sender_(true),
+    reflect_packet_(false)
   {
     RCLCPP_INFO(node_->get_logger(), "CpmApplication started...");
     set_interval(milliseconds(100));
   }
 
-  void CpmApplication::set_interval(Clock::duration interval)
-  {
+  void CpmApplication::set_interval(Clock::duration interval) {
     cpm_interval_ = interval;
     runtime_.cancel(this);
     schedule_timer();
   }
 
-  void CpmApplication::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)
-  {
+  void CpmApplication::on_timer(Clock::time_point) {
     schedule_timer();
     send();
   }
 
-  CpmApplication::PortType CpmApplication::port()
-  {
+  CpmApplication::PortType CpmApplication::port() {
     return btp::ports::CPM;
   }
 
-  void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr packet)
-  {
+  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(), "[INDICATE] Received decodable CPM content");
+      RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] Received decodable CPM content");
+
+      rclcpp::Time current_time = node_->now();
+      RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r1 %ld", current_time.nanoseconds());
+
       asn1::Cpm message = *cpm;
       ItsPduHeader_t &header = message->header;
 
@@ -84,7 +81,10 @@ namespace v2x
       const auto time_now = duration_cast<milliseconds> (runtime_.now().time_since_epoch());
       uint16_t gdt = time_now.count();
       int gdt_diff = (65536 + (gdt - gdt_cpm) % 65536) % 65536;
-      RCLCPP_INFO(node_->get_logger(), "[indicate] gdt: %ld,%u,%d", gdt_cpm, gdt, gdt_diff);
+      RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] GDT_CPM: %ld", gdt_cpm);
+      RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] GDT: %u", gdt);
+      RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_R1R2: %d", gdt_diff);
+
 
       CpmManagementContainer_t &management = message->cpm.cpmParameters.managementContainer;
       double lat = management.referencePosition.latitude / 1.0e7;
@@ -149,41 +149,52 @@ namespace v2x
       } else {
         RCLCPP_INFO(node_->get_logger(), "[INDICATE] Empty POC");
       }
+
+      if (reflect_packet_) {
+        Application::DownPacketPtr packet{new DownPacket()};
+        std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
+
+        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;
+
+        Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
+
+        if (!confirm.accepted()) {
+          throw std::runtime_error("[CpmApplication::indicate] Packet reflection failed");
+        }
+      }
+
+
     } else {
       RCLCPP_INFO(node_->get_logger(), "[INDICATE] 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::updateMGRS(double *x, double *y) {
+    ego_.mgrs_x = *x;
+    ego_.mgrs_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::updateRP(double *lat, double *lon, double *altitude) {
+    ego_.latitude = *lat;
+    ego_.longitude = *lon;
+    ego_.altitude = *altitude;
   }
 
-  void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp)
-  {
+  void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) {
     generationDeltaTime_ = *gdt;
-    gdt_timestamp_ = *gdt_timestamp;
-    // RCLCPP_INFO(node_->get_logger(), "[updateGDT] %d %lu", generationDeltaTime_, gdt_timestamp_);
+    gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
   }
 
-  void CpmApplication::updateHeading(double *yaw)
-  {
-    // RCLCPP_INFO(node_->get_logger(), "Update Heading : %f", *yaw);
-    ego_heading_ = *yaw;
+  void CpmApplication::updateHeading(double *yaw) {
+    ego_.heading = *yaw;
   }
 
-  void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg)
-  {
+  void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) {
     updating_objects_stack_ = true;
 
     if (sending_) {
@@ -209,8 +220,10 @@ namespace v2x
         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);
-        object.xDistance = std::lround(((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100.0);
-        object.yDistance = std::lround(((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100.0);
+        // object.xDistance = std::lround(((object.position_x - ego_x_) * cos(-ego_heading_) - (object.position_y - ego_y_) * sin(-ego_heading_)) * 100.0);
+        // object.yDistance = std::lround(((object.position_x - ego_x_) * sin(-ego_heading_) + (object.position_y - ego_y_) * cos(-ego_heading_)) * 100.0);
+        object.xDistance = std::lround(((object.position_x - ego_.mgrs_x) * cos(-ego_.heading) - (object.position_y - ego_.mgrs_y) * sin(-ego_.heading)) * 100.0);
+        object.yDistance = std::lround(((object.position_x - ego_.mgrs_x) * sin(-ego_.heading) + (object.position_y - ego_.mgrs_y) * cos(-ego_.heading)) * 100.0);
         if (object.xDistance < -132768 || object.xDistance > 132767) {
           continue;
         }
@@ -230,152 +243,131 @@ namespace v2x
           object.yawAngle = std::lround((yaw * 180.0 / M_PI) * 10.0); // 0 - 3600
         }
         
-        long long timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec;
-        object.timeOfMeasurement = (gdt_timestamp_ - timestamp) / 1e6;
+        long long msg_timestamp_sec = msg->header.stamp.sec;
+        long long msg_timestamp_nsec = msg->header.stamp.nanosec;
+        msg_timestamp_sec -= 1072915200; // convert to etsi-epoch
+        long long msg_timestamp_msec = msg_timestamp_sec * 1000 + msg_timestamp_nsec / 1000000;
+        // long long timestamp = msg->header.stamp.sec * 1e9 + msg->header.stamp.nanosec;
+        object.timeOfMeasurement = gdt_timestamp_ - msg_timestamp_msec;
         // RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] %ld %ld %d", gdt_timestamp_, timestamp, object.timeOfMeasurement);
         if (object.timeOfMeasurement < -1500 || object.timeOfMeasurement > 1500) {
+          RCLCPP_INFO(node_->get_logger(), "[updateObjectsStack] timeOfMeasurement out of bounds: %d", object.timeOfMeasurement);
           continue;
         }
         objectsStack.push_back(object);
         ++i;
-        // RCLCPP_INFO(node_->get_logger(), "Added to stack: %f %f %f", obj.shape.dimensions.x, obj.shape.dimensions.y, obj.shape.dimensions.z);
-        // RCLCPP_INFO(node_->get_logger(), "Added to stack: %f %f %f", obj.shape.dimensions.x * 10.0, obj.shape.dimensions.y * 10.0, obj.shape.dimensions.z * 10.0);
-        // RCLCPP_INFO(node_->get_logger(), "Added to stack: %d %d %d", std::lround(obj.shape.dimensions.x * 10.0), std::lround(obj.shape.dimensions.y * 10.0), std::lround(obj.shape.dimensions.z * 10.0));
+        
         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());
     updating_objects_stack_ = false;
   }
 
-  void CpmApplication::send()
-  {
-    sending_ = true;
-    
-    RCLCPP_INFO(node_->get_logger(), "[SEND] Sending CPM...");
+  void CpmApplication::send() {
 
-    vanetza::asn1::Cpm message;
+    if (is_sender_) {
+      sending_ = true;
+      
+      RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM...");
 
-    // ITS PDU Header
-    ItsPduHeader_t &header = message->header;
-    header.protocolVersion = 1;
-    header.messageID = 14;
-    header.stationID = 1;
+      vanetza::asn1::Cpm message;
 
-    // const auto time_now = duration_cast<milliseconds>(system_clock::now().time_since_epoch());
-    // uint16_t gen_delta_time = time_now.count();
+      // ITS PDU Header
+      ItsPduHeader_t &header = message->header;
+      header.protocolVersion = 1;
+      header.messageID = 14;
+      header.stationID = 1;
 
-    CollectivePerceptionMessage_t &cpm = message->cpm;
-    cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec;
+      CollectivePerceptionMessage_t &cpm = message->cpm;
 
-    // auto position = positioning->position_specify(pos_lat, pos_lon);
+      // Set GenerationDeltaTime
+      cpm.generationDeltaTime = generationDeltaTime_ * GenerationDeltaTime_oneMilliSec;
 
-    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);
+      CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer;
+      management.stationType = StationType_passengerCar;
+      PositionFix fix;
+      fix.latitude = ego_.latitude * units::degree;
+      fix.longitude = ego_.longitude * 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.numberOfPerceivedObjects = objectsStack.size();
 
-    // 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>();
+      sdc->present = StationDataContainer_PR_originatingVehicleContainer;
+      OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer;
+      ovc.speed.speedValue = 0;
+      ovc.speed.speedConfidence = 1;
+      int heading = std::lround(((-ego_.heading * 180.0 / M_PI) + 90.0) * 10.0);
+      if (heading < 0) heading += 3600;
+      ovc.heading.headingValue = heading;
+      ovc.heading.headingConfidence = 1;
 
-    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;
-    int heading = std::lround(((-ego_heading_ * 180 / M_PI) + 90.0) * 10);
-    if (heading < 0) heading += 3600;
-    ovc.heading.headingValue = heading;
-    ovc.heading.headingConfidence = 1;
-    // RCLCPP_INFO(node_->get_logger(), "[SEND] headingValue: %f %d", ego_heading_, ovc.heading.headingValue);
+      if (objectsStack.size() > 0) {
+        PerceivedObjectContainer_t *&poc = cpm.cpmParameters.perceivedObjectContainer;
+        poc = vanetza::asn1::allocate<PerceivedObjectContainer_t>();
 
-    if (objectsStack.size() > 0) {
-      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;
 
-      for (CpmApplication::Object object : objectsStack) {
-        // if (object.xDistance > 10000) continue;
-        // if (object.yDistance > 10000) continue;
+          pObj->planarObjectDimension1 = vanetza::asn1::allocate<ObjectDimension_t>();
+          pObj->planarObjectDimension2 = vanetza::asn1::allocate<ObjectDimension_t>();
+          pObj->verticalObjectDimension = vanetza::asn1::allocate<ObjectDimension_t>();
 
-        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;
+          (*(pObj->planarObjectDimension1)).value = object.shape_y;
+          (*(pObj->planarObjectDimension1)).confidence = 1;
+          (*(pObj->planarObjectDimension2)).value = object.shape_x;
+          (*(pObj->planarObjectDimension2)).confidence = 1;
+          (*(pObj->verticalObjectDimension)).value = object.shape_z;
+          (*(pObj->verticalObjectDimension)).confidence = 1;
 
-        pObj->planarObjectDimension1 = vanetza::asn1::allocate<ObjectDimension_t>();
-        pObj->planarObjectDimension2 = vanetza::asn1::allocate<ObjectDimension_t>();
-        pObj->verticalObjectDimension = vanetza::asn1::allocate<ObjectDimension_t>();
+          pObj->yawAngle = vanetza::asn1::allocate<CartesianAngle>();
+          (*(pObj->yawAngle)).value = object.yawAngle;
+          (*(pObj->yawAngle)).confidence = 1;
 
-        (*(pObj->planarObjectDimension1)).value = object.shape_y;
-        (*(pObj->planarObjectDimension1)).confidence = 1;
-        (*(pObj->planarObjectDimension2)).value = object.shape_x;
-        (*(pObj->planarObjectDimension2)).confidence = 1;
-        (*(pObj->verticalObjectDimension)).value = object.shape_z;
-        (*(pObj->verticalObjectDimension)).confidence = 1;
+          RCLCPP_INFO(node_->get_logger(), "[SEND] Added: #%d (%d, %d) (%d, %d) (%d, %d, %d) %d", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_y, object.shape_x, object.shape_z, object.yawAngle);
 
-        pObj->yawAngle = vanetza::asn1::allocate<CartesianAngle>();
-        (*(pObj->yawAngle)).value = object.yawAngle;
-        (*(pObj->yawAngle)).confidence = 1;
-
-        RCLCPP_INFO(node_->get_logger(), "[SEND] Added: #%d (%d, %d) (%d, %d) (%d, %d, %d) %d", object.objectID, object.xDistance, object.yDistance, object.xSpeed, object.ySpeed, object.shape_y, object.shape_x, object.shape_z, object.yawAngle);
-
-        ASN_SEQUENCE_ADD(poc, pObj);
+          ASN_SEQUENCE_ADD(poc, pObj);
+        }
+      } else {
+        cpm.cpmParameters.perceivedObjectContainer = NULL;
+        RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Empty POC");
       }
-    } else {
-      cpm.cpmParameters.perceivedObjectContainer = NULL;
-      RCLCPP_INFO(node_->get_logger(), "[SEND] Empty POC");
+      
+      Application::DownPacketPtr packet{new DownPacket()};
+      std::unique_ptr<geonet::DownPacket> payload{new geonet::DownPacket()};
+
+      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;
+
+      Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
+
+      if (!confirm.accepted()) {
+        throw std::runtime_error("[CpmApplication::send] CPM application data request failed");
+      }
+
+      sending_ = false;
+      rclcpp::Time current_time = node_->now();
+      RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds());
+
     }
-    
-    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;
-
-    Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
-
-    if (!confirm.accepted()) {
-      throw std::runtime_error("[SEND] CPM application data request failed");
-    }
-
-      // try {
-      //   Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
-      //   if (!confirm.accepted()) {
-      //     throw std::runtime_error("CPM application data request failed");
-      //   }
-      // } catch (...) {
-      //   RCLCPP_INFO(node_->get_logger(), "Request Failed");
-      // }
-    sending_ = false;
-    // RCLCPP_INFO(node->get_logger(), "Application::request END");
   }
 }

src/v2x_app.cpp

@@ -40,13 +40,16 @@ namespace v2x
 
   void V2XApp::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) {
     // RCLCPP_INFO(node_->get_logger(), "V2XApp: msg received");
+    if (!tf_received_) {
+      RCLCPP_WARN(node_->get_logger(), "[V2XApp::objectsCallback] tf not received yet");
+    }
     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");
+
     tf_received_ = true;
 
     double x = msg->transforms[0].transform.translation.x;
@@ -57,12 +60,8 @@ namespace v2x
     long long timestamp_nsec = msg->transforms[0].header.stamp.nanosec; // nanoseconds
     timestamp_sec -= 1072915200; // convert to etsi-epoch
     long long timestamp_msec = timestamp_sec * 1000 + timestamp_nsec / 1000000;
-    // long long gdt_timestamp = gdt_timestamp_sec * 1e9 + gdt_timestamp_nsec; // nanoseconds
-    // long long gdt_timestamp_msec = gdt_timestamp / 1000000;
-    // long long gdt_timestamp_msec_etsi_epoch = gdt_timestamp_msec - 1072915200000;
-    // int gdt = gdt_timestamp_msec_etsi_epoch % 65536; // milliseconds
     int gdt = timestamp_msec % 65536;
-    RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec);
+    // RCLCPP_INFO(node_->get_logger(), "[tfCallback] %d,%lld,%lld,%lld", gdt, timestamp_msec, timestamp_sec, timestamp_nsec);
 
     double rot_x = msg->transforms[0].transform.rotation.x;
     double rot_y = msg->transforms[0].transform.rotation.y;
@@ -75,23 +74,16 @@ namespace v2x
     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);
@@ -132,7 +124,7 @@ namespace v2x
     context.set_link_layer(link_layer.get());
 
     cp = new CpmApplication(node_, trigger.runtime());
-
+    
     context.enable(cp);
 
     cp_started_ = true;

src/v2x_node.cpp

@@ -25,14 +25,13 @@ using namespace std::chrono;
 
 namespace v2x
 {
-  V2XNode::V2XNode(const rclcpp::NodeOptions &node_options) : rclcpp::Node("autoware_v2x_node", node_options), received_time(this->now()), cpm_received(false)
-  {
+  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_pos_ = this->create_subscription<tf2_msgs::msg::TFMessage>("/tf", 10, std::bind(&V2XNode::tfCallback, this, _1));
 
-    publisher_ = create_publisher<autoware_perception_msgs::msg::DynamicObjectArray>("/perception/object_recognition/objects", rclcpp::QoS{10});
+    publisher_ = create_publisher<autoware_perception_msgs::msg::DynamicObjectArray>("/v2x/cpm/objects", rclcpp::QoS{10});
 
     this->declare_parameter<std::string>("network_interface", "vmnet1");
 
@@ -40,13 +39,16 @@ namespace v2x
     boost::thread v2xApp(boost::bind(&V2XApp::start, app));
 
     RCLCPP_INFO(get_logger(), "V2X Node Launched");
+
+    rclcpp::Time current_time = this->now();
+    RCLCPP_INFO(get_logger(), "[V2XNode::V2XNode] [measure] T_R1 %ld", current_time.nanoseconds());
+
   }
 
   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;
 
@@ -79,27 +81,25 @@ namespace v2x
 
       output_dynamic_object_msg.objects.push_back(object);
     }
-    cpm_received = true;
+
+    current_time = this->now();
+    RCLCPP_INFO(get_logger(), "[V2XNode::publishObjects] [measure] T_obj_r2 %ld", current_time.nanoseconds());
+
     publisher_->publish(output_dynamic_object_msg);
   }
 
-  void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg)
-  {
+  void V2XNode::objectsCallback(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr 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);
-    }
+    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());
+
+    // Measuring T_A1R1
+    RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj %ld", msg_time.nanoseconds());
+    RCLCPP_INFO(get_logger(), "[V2XNode::objectsCallback] [measure] T_obj_receive %ld", current_time.nanoseconds());
+    app->objectsCallback(msg);
   }
 
-  void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg)
-  {
+  void V2XNode::tfCallback(const tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
     app->tfCallback(msg);
   }
 }