Update cpm_application

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 */

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)
+void CpmApplication::updateMGRS(double *x, double *y) {
-{
+  RCLCPP_INFO(node_->get_logger(), "Update MGRS");
-  RCLCPP_INFO(node->get_logger(), "Sending CPM...");
+  ego_x_ = *x;
-  // Send all objects in 1 CPM message
+  ego_y_ = *y;
-  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 HelloApplication::schedule_timer()
+void CpmApplication::updateRP(double *lat, double *lon, double *altitude)
-// {
+{
-//     timer_.expires_from_now(interval_);
+  RCLCPP_INFO(node_->get_logger(), "Update RP");
-//     timer_.async_wait(std::bind(&HelloApplication::on_timer, this, std::placeholders::_1));
+  ego_lat_ = *lat;
-// }
+  ego_lon_ = *lon;
+  ego_altitude_ = *altitude;
+}
 
-// void HelloApplication::on_timer(const boost::system::error_code& ec)
+void CpmApplication::updateGenerationDeltaTime(int *gdt)
-// {
+{
-//     if (ec != boost::asio::error::operation_aborted) {
+  RCLCPP_INFO(node_->get_logger(), "Update GDT");
-//         DownPacketPtr packet { new DownPacket() };
+  generationDeltaTime_ = *gdt;
-//         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");
-//         }
 
-//         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");
+}