Fix timeOfMeasurment bug

include/autoware_v2x/cpm_application.hpp

@@ -39,8 +39,8 @@ private:
         double orientation_y;
         double orientation_z;
         double orientation_w;
-        double xDistance;
+        int xDistance;
-        double yDistance;
+        int yDistance;
         double xSpeed;
         double ySpeed;
         vanetza::PositionFix position;

src/cpm_application.cpp

@@ -17,13 +17,12 @@
 #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, Runtime &rt) : node_(node),
+CpmApplication::CpmApplication(rclcpp::Node *node, Runtime &rt) : 
+  node_(node),
   runtime_(rt),
   ego_x_(0),
   ego_y_(0),
@@ -69,21 +68,19 @@ void CpmApplication::indicate(const DataIndication &indication, UpPacketPtr pack
   if (cpm)
   {
     RCLCPP_INFO(node_->get_logger(), "Received decodable CPM content");
-  }
-  else
-  {
-    RCLCPP_INFO(node_->get_logger(), "Received broken content");
-  }
     asn1::Cpm message = *cpm;
     ItsPduHeader_t &header = message->header;
-  RCLCPP_INFO(node_->get_logger(), "cpm.ItsPduHeader.messageId = %d", header.messageID);
-  RCLCPP_INFO(node_->get_logger(), "cpm.ItsPduHeader.stationId = %d", header.stationID);
 
     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");
@@ -143,9 +140,10 @@ void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::Dyn
       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.xDistance = (int)((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;
+      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: %f, yDistance: %f", object.xDistance, object.yDistance);
+      RCLCPP_INFO(node_->get_logger(), "xDistance: %d, yDistance: %d", object.xDistance, object.yDistance);
+      object.timeOfMeasurement = 100;
       objectsStack.push_back(object);
       ++i;
     }
@@ -205,9 +203,11 @@ void CpmApplication::send()
     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;
@@ -229,16 +229,21 @@ void CpmApplication::send()
       pObj->ySpeed.confidence = 1;
 
       ASN_SEQUENCE_ADD(poc, pObj);
-      // RCLCPP_INFO(node->get_logger(), "Added one object to poc->list");
+      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;
@@ -247,7 +252,9 @@ void CpmApplication::send()
     // 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");

src/v2x_app.cpp

@@ -33,67 +33,12 @@ namespace v2x
     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_) {
+    if (tf_received_ && cp_started_) {
       cp->updateObjectsStack(msg);
     }
   }
@@ -165,7 +110,6 @@ namespace v2x
     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);
@@ -175,7 +119,6 @@ namespace v2x
 
     cp = new CpmApplication(node_, trigger.runtime());
 
-    // context.enable(cp_.get());
     context.enable(cp);
 
     cp_started_ = true;