From 7c839771ea1cb4c132d4cb96688a527c09acbf3d Mon Sep 17 00:00:00 2001
From: Facundo Garcia <facundo.garcia@fixposition.com>
Date: Fri, 24 Nov 2023 16:07:03 +0100
Subject: [PATCH] Added ROS2 changes

---
 .../fixposition_driver_ros2/data_to_ros2.hpp  |  1 +
 .../fixposition_driver_node.hpp               |  4 ++--
 .../src/fixposition_driver_node.cpp           | 24 +++++++++++--------
 3 files changed, 17 insertions(+), 12 deletions(-)

diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp
index e429924..0bcdc16 100644
--- a/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp
+++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/data_to_ros2.hpp
@@ -25,6 +25,7 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/imu.hpp>
diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp
index 20a3875..609ca39 100644
--- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp
+++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp
@@ -76,8 +76,8 @@ class FixpositionDriverNode : public FixpositionDriver {
     rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr poiimu_pub_;             //!< Bias corrected IMU from ODOMETRY
     rclcpp::Publisher<fixposition_driver_ros2::msg::VRTK>::SharedPtr vrtk_pub_;  //!< VRTK message
     rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometry_enu0_pub_;    //!< ENU0 Odometry
-    rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr eul_pub_;  //!< Euler angles Yaw-Pitch-Roll in local ENU
-    rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr
+    rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr eul_pub_;  //!< Euler angles Yaw-Pitch-Roll in local ENU
+    rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr
         eul_imu_pub_;  //!< Euler angles Pitch-Roll as estimated from the IMU in
                        // local horizontal
 
diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
index edf25bd..0046534 100644
--- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp
+++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
@@ -47,8 +47,8 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr<rclcpp::Node> node,
       poiimu_pub_(node_->create_publisher<sensor_msgs::msg::Imu>("/fixposition/poiimu", 100)),
       vrtk_pub_(node_->create_publisher<fixposition_driver_ros2::msg::VRTK>("/fixposition/vrtk", 100)),
       odometry_enu0_pub_(node_->create_publisher<nav_msgs::msg::Odometry>("/fixposition/odometry_enu", 100)),
-      eul_pub_(node_->create_publisher<geometry_msgs::msg::Vector3>("/fixposition/ypr", 100)),
-      eul_imu_pub_(node_->create_publisher<geometry_msgs::msg::Vector3>("/fixposition/imu_ypr", 100)),
+      eul_pub_(node_->create_publisher<geometry_msgs::msg::Vector3Stamped>("/fixposition/ypr", 100)),
+      eul_imu_pub_(node_->create_publisher<geometry_msgs::msg::Vector3Stamped>("/fixposition/imu_ypr", 100)),
       br_(std::make_shared<tf2_ros::TransformBroadcaster>(node_)),
       static_br_(std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_)) {
     ws_sub_ = node_->create_subscription<fixposition_driver_ros2::msg::Speed>(
@@ -110,10 +110,12 @@ void FixpositionDriverNode::RegisterObservers() {
                         vrtk_pub_->publish(vrtk);
                     }
                     if (eul_pub_->get_subscription_count() > 0) {
-                        geometry_msgs::msg::Vector3 ypr;
-                        ypr.set__x(data.eul.x());
-                        ypr.set__y(data.eul.y());
-                        ypr.set__z(data.eul.z());
+                        geometry_msgs::msg::Vector3Stamped ypr;
+                        ypr.header.stamp = GpsTimeToMsgTime(data.odometry.stamp);
+                        ypr.header.frame_id = "FP_POI";
+                        ypr.vector.set__x(data.eul.x());
+                        ypr.vector.set__y(data.eul.y());
+                        ypr.vector.set__z(data.eul.z());
                         eul_pub_->publish(ypr);
                     }
 
@@ -169,10 +171,12 @@ void FixpositionDriverNode::RegisterObservers() {
                     // Publish Pitch Roll based on IMU only
                     Eigen::Vector3d imu_ypr_eigen = gnss_tf::QuatToEul(data.rotation);
                     imu_ypr_eigen.x() = 0.0;  // the yaw value is not observable using IMU alone
-                    geometry_msgs::msg::Vector3 imu_ypr;
-                    imu_ypr.set__x(imu_ypr_eigen.x());
-                    imu_ypr.set__y(imu_ypr_eigen.y());
-                    imu_ypr.set__z(imu_ypr_eigen.z());
+                    geometry_msgs::msg::Vector3Stamped imu_ypr;
+                    imu_ypr.header.stamp = tf.header.stamp;
+                    imu_ypr.header.frame_id = "FP_POI";
+                    imu_ypr.vector.set__x(imu_ypr_eigen.x());
+                    imu_ypr.vector.set__y(imu_ypr_eigen.y());
+                    imu_ypr.vector.set__z(imu_ypr_eigen.z());
                     eul_imu_pub_->publish(imu_ypr);
 
                 } else {