fixing positioning

include/autoware_v2x/cpm_application.hpp

@@ -58,12 +58,17 @@ namespace v2x
         vanetza::Runtime &runtime_;
         vanetza::Clock::duration cpm_interval_;
 
-        double ego_x_;
+        struct Ego_station {
-        double ego_y_;
+            double mgrs_x;
-        double ego_lat_;
+            double mgrs_y;
-        double ego_lon_;
+            double latitude;
-        double ego_altitude_;
+            double longitude;
-        double ego_heading_;
+            double altitude;
+            double heading;
+        };
+
+        Ego_station ego_;
+        
         int generationDeltaTime_;
         long long gdt_timestamp_;
 

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,17 +28,11 @@ 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_(),
-    ego_y_(0),
-    ego_lat_(0),
-    ego_lon_(0),
-    ego_altitude_(0),
-    ego_heading_(0),
     generationDeltaTime_(0),
     updating_objects_stack_(false),
     sending_(false),
@@ -181,14 +175,14 @@ namespace v2x
   }
 
   void CpmApplication::updateMGRS(double *x, double *y) {
-    ego_x_ = *x;
+    ego_.mgrs_x = *x;
-    ego_y_ = *y;
+    ego_.mgrs_y = *y;
   }
 
   void CpmApplication::updateRP(double *lat, double *lon, double *altitude) {
-    ego_lat_ = *lat;
+    ego_.latitude = *lat;
-    ego_lon_ = *lon;
+    ego_.longitude = *lon;
-    ego_altitude_ = *altitude;
+    ego_.altitude = *altitude;
   }
 
   void CpmApplication::updateGenerationDeltaTime(int *gdt, long long *gdt_timestamp) {
@@ -197,7 +191,7 @@ namespace v2x
   }
 
   void CpmApplication::updateHeading(double *yaw) {
-    ego_heading_ = *yaw;
+    ego_.heading = *yaw;
   }
 
   void CpmApplication::updateObjectsStack(const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr msg) {
@@ -226,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.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.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;
         }
@@ -260,9 +256,7 @@ namespace v2x
         }
         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);
       }
     }
@@ -295,8 +289,8 @@ namespace v2x
       CpmManagementContainer_t &management = cpm.cpmParameters.managementContainer;
       management.stationType = StationType_passengerCar;
       PositionFix fix;
-      fix.latitude = ego_lat_ * units::degree;
+      fix.latitude = ego_.latitude * units::degree;
-      fix.longitude = ego_lon_ * 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);
@@ -308,7 +302,7 @@ namespace v2x
       OriginatingVehicleContainer_t &ovc = sdc->choice.originatingVehicleContainer;
       ovc.speed.speedValue = 0;
       ovc.speed.speedConfidence = 1;
-      int heading = std::lround(((-ego_heading_ * 180 / M_PI) + 90.0) * 10);
+      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;

src/v2x_app.cpp

@@ -49,7 +49,7 @@ namespace v2x
   }
 
   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;
@@ -60,10 +60,6 @@ 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);
 
@@ -78,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);