From 2e9c860ef25bcb143ea33cd55cbce040ba3d8664 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 2 Nov 2022 17:48:08 +0100
Subject: [PATCH] changed R to q for rotations in GNSS
---
src/publisher_gnss_tf.cpp | 17 ++++++++++-------
1 file changed, 10 insertions(+), 7 deletions(-)
diff --git a/src/publisher_gnss_tf.cpp b/src/publisher_gnss_tf.cpp
index 54da496..8d2a1d6 100644
--- a/src/publisher_gnss_tf.cpp
+++ b/src/publisher_gnss_tf.cpp
@@ -66,11 +66,12 @@ void PublisherGnssTf::publishDerived()
// ENU-MAP
tf::Vector3 tf_t_enu_map;
- tf::Matrix3x3 tf_R_enu_map;
+ tf::Quaternion tf_q_enu_map;
- tf::matrixEigenToTF(sensor_gnss_->getREnuMap(), tf_R_enu_map);
+ tf::quaternionEigenToTF(sensor_gnss_->getQEnuMap(), tf_q_enu_map);
tf::vectorEigenToTF(sensor_gnss_->gettEnuMap(), tf_t_enu_map);
- T_enu_map_ .setData(tf::Transform(tf_R_enu_map, tf_t_enu_map));
+
+ T_enu_map_ .setData(tf::Transform(tf_q_enu_map, tf_t_enu_map));
T_enu_map_ .stamp_ = ros::Time::now();
tfb_.sendTransform(T_enu_map_);
@@ -79,11 +80,13 @@ void PublisherGnssTf::publishDerived()
if (publish_ecef_)
{
tf::Vector3 tf_t_enu_ecef;
- tf::Matrix3x3 tf_R_enu_ecef;
- T_ecef_enu_.setData(tf::Transform(tf_R_enu_ecef, tf_t_enu_ecef).inverse());
- T_ecef_enu_.stamp_ = ros::Time::now();
- tf::matrixEigenToTF(sensor_gnss_->getREnuEcef(), tf_R_enu_ecef);
+ tf::Quaternion tf_q_enu_ecef;
+
+ tf::quaternionEigenToTF(sensor_gnss_->getQEnuEcef(), tf_q_enu_ecef);
tf::vectorEigenToTF(sensor_gnss_->gettEnuEcef(), tf_t_enu_ecef);
+
+ T_ecef_enu_.setData(tf::Transform(tf_q_enu_ecef, tf_t_enu_ecef).inverse());
+ T_ecef_enu_.stamp_ = ros::Time::now();
tfb_.sendTransform(T_ecef_enu_);
}
--
GitLab