Added Vector3Stamped to ypr and imu_ypr topics in ROS1 (#42)

* Added Vector3Stamped to ypr and imu_ypr topics in ROS1 * Added ROS2 changes --------- Co-authored-by: Facundo Garcia <[email protected]>
fixposition · Dec 6, 2023 · c4440d9 · c4440d9
1 parent 38eb997
commit c4440d9
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 18 deletions.
diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/data_to_ros1.hpp
@@ -16,6 +16,7 @@
 #define __FIXPOSITION_DRIVER_ROS1_DATA_TO_ROS1__
 /* ROS */
 #include <eigen_conversions/eigen_msg.h>
+#include <geometry_msgs/Vector3Stamped.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <nav_msgs/Odometry.h>
 #include <sensor_msgs/Imu.h>

diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
@@ -46,8 +46,8 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para
       poiimu_pub_(nh_.advertise<sensor_msgs::Imu>("/fixposition/poiimu", 100)),
       vrtk_pub_(nh_.advertise<fixposition_driver_ros1::VRTK>("/fixposition/vrtk", 100)),
       odometry_enu0_pub_(nh_.advertise<nav_msgs::Odometry>("/fixposition/odometry_enu", 100)),
-      eul_pub_(nh_.advertise<geometry_msgs::Vector3>("/fixposition/ypr", 100)),
-      eul_imu_pub_(nh_.advertise<geometry_msgs::Vector3>("/fixposition/imu_ypr", 100)) {
+      eul_pub_(nh_.advertise<geometry_msgs::Vector3Stamped>("/fixposition/ypr", 100)),
+      eul_imu_pub_(nh_.advertise<geometry_msgs::Vector3Stamped>("/fixposition/imu_ypr", 100)) {
     ws_sub_ = nh_.subscribe<fixposition_driver_ros1::Speed>(params_.customer_input.speed_topic, 100,
                                                             &FixpositionDriverNode::WsCallback, this,
                                                             ros::TransportHints().tcpNoDelay());
@@ -85,8 +85,10 @@ void FixpositionDriverNode::RegisterObservers() {
                         vrtk_pub_.publish(vrtk);
                     }
                     if (eul_pub_.getNumSubscribers() > 0) {
-                        geometry_msgs::Vector3 ypr;
-                        tf::vectorEigenToMsg(data.eul, ypr);
+                        geometry_msgs::Vector3Stamped ypr;
+                        ypr.header.stamp = ros::Time::fromBoost(fixposition::times::GpsTimeToPtime(data.odometry.stamp));
+                        ypr.header.frame_id = "FP_POI";
+                        tf::vectorEigenToMsg(data.eul, ypr.vector);
                         eul_pub_.publish(ypr);
                     }
 
@@ -142,8 +144,10 @@ 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::Vector3 imu_ypr;
-                    tf::vectorEigenToMsg(imu_ypr_eigen, imu_ypr);
+                    geometry_msgs::Vector3Stamped imu_ypr;
+                    imu_ypr.header.stamp = tf.header.stamp;
+                    imu_ypr.header.frame_id = "FP_POI";
+                    tf::vectorEigenToMsg(imu_ypr_eigen, imu_ypr.vector);
                     eul_imu_pub_.publish(imu_ypr);
 
                 } else {

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
@@ -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
@@ -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
@@ -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 {