Update to include packet reflection

include/autoware_v2x/cpm_application.hpp

@@ -69,6 +69,8 @@ namespace v2x
 
         bool updating_objects_stack_;
         bool sending_;
+        bool is_sender_;
+        bool reflect_packet_;
     };
 }
 

src/cpm_application.cpp

@@ -41,43 +41,43 @@ namespace v2x
     ego_heading_(0),
     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) {
+
     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_r2 %ld", current_time.nanoseconds());
+      RCLCPP_INFO(node_->get_logger(), "[CpmApplication::indicate] [measure] T_receive_r1 %ld", current_time.nanoseconds());
 
       asn1::Cpm message = *cpm;
       ItsPduHeader_t &header = message->header;
@@ -155,40 +155,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)
+  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)
+  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, long long *gdt_timestamp)
+  void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) {
-  {
     generationDeltaTime_ = *gdt;
     gdt_timestamp_ = *gdt_timestamp; // ETSI-epoch milliseconds timestamp
   }
 
-  void CpmApplication::updateHeading(double *yaw)
+  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)
+  void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) {
-  {
     updating_objects_stack_ = true;
 
     if (sending_) {
@@ -259,9 +271,11 @@ namespace v2x
   }
 
   void CpmApplication::send() {
+
+    if (is_sender_) {
       sending_ = true;
       
-    RCLCPP_INFO(node_->get_logger(), "[SEND] Sending CPM...");
+      RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Sending CPM...");
 
       vanetza::asn1::Cpm message;
 
@@ -271,57 +285,37 @@ namespace v2x
       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);
+      // 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);
-
-    // 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;
       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>();
 
         for (CpmApplication::Object object : objectsStack) {
-        // if (object.xDistance > 10000) continue;
-        // if (object.yDistance > 10000) continue;
-
           PerceivedObject *pObj = vanetza::asn1::allocate<PerceivedObject>();
           pObj->objectID = object.objectID;
           pObj->timeOfMeasurement = object.timeOfMeasurement;
@@ -355,13 +349,11 @@ namespace v2x
         }
       } else {
         cpm.cpmParameters.perceivedObjectContainer = NULL;
-      RCLCPP_INFO(node_->get_logger(), "[SEND] Empty POC");
+        RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] Empty POC");
       }
       
       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);
 
@@ -373,20 +365,13 @@ namespace v2x
       Application::DataConfirm confirm = Application::request(request, std::move(payload), node_);
 
       if (!confirm.accepted()) {
-      throw std::runtime_error("[SEND] CPM application data request failed");
+        throw std::runtime_error("[CpmApplication::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");
       rclcpp::Time current_time = node_->now();
       RCLCPP_INFO(node_->get_logger(), "[CpmApplication::send] [measure] T_depart_r1 %ld", current_time.nanoseconds());
+
+    }
   }
 }