From f2cfa4ad8255d8e15015e3b69800b3f1cafcf949 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Tue, 18 Oct 2022 15:09:58 +0200
Subject: [PATCH] optionally publishing ecef TF
---
include/publisher_gnss_tf.h | 1 +
src/publisher_gnss_tf.cpp | 57 +++++++++++++++++++------------------
2 files changed, 30 insertions(+), 28 deletions(-)
diff --git a/include/publisher_gnss_tf.h b/include/publisher_gnss_tf.h
index 6d49cbe..c46d977 100644
--- a/include/publisher_gnss_tf.h
+++ b/include/publisher_gnss_tf.h
@@ -37,6 +37,7 @@ namespace wolf
class PublisherGnssTf: public Publisher
{
+ bool publish_ecef_;
std::string map_frame_id_, enu_frame_id_, ecef_frame_id_;
tf::StampedTransform T_enu_map_, T_ecef_enu_;
tf::TransformBroadcaster tfb_;
diff --git a/src/publisher_gnss_tf.cpp b/src/publisher_gnss_tf.cpp
index b90dd8f..98085f5 100644
--- a/src/publisher_gnss_tf.cpp
+++ b/src/publisher_gnss_tf.cpp
@@ -36,18 +36,23 @@ PublisherGnssTf::PublisherGnssTf(const std::string& _unique_name,
ProblemConstPtr _problem) :
Publisher(_unique_name, _server, _problem)
{
- sensor_gnss_ = std::static_pointer_cast<const SensorGnss>(_problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor_gnss_name")));
+ auto sensor = std::dynamic_pointer_cast<const SensorGnss>(_problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor_gnss_name")));
+ if (not sensor_gnss_)
+ throw std::runtime_error("PublisherGnssTf: 'sensor_gnss_name' not found or wrong sensor type.");
T_enu_map_.setIdentity();
T_enu_map_.frame_id_ = "ENU";
T_enu_map_.stamp_ = ros::Time::now();
-
- T_ecef_enu_.setIdentity();
- T_ecef_enu_.frame_id_ = "ECEF";
- T_ecef_enu_.child_frame_id_ = "ENU";
- T_ecef_enu_.stamp_ = ros::Time::now();
-
T_enu_map_.child_frame_id_ = _server.getParam<std::string>(prefix_ + "/map_frame_id");
+ publish_ecef_ = _server.getParam<bool>(prefix_ + "/publish_ecef");
+
+ if (publish_ecef_)
+ {
+ T_ecef_enu_.setIdentity();
+ T_ecef_enu_.frame_id_ = "ECEF";
+ T_ecef_enu_.child_frame_id_ = "ENU";
+ T_ecef_enu_.stamp_ = ros::Time::now();
+ }
}
void PublisherGnssTf::initialize(ros::NodeHandle& nh, const std::string& topic)
@@ -56,36 +61,32 @@ void PublisherGnssTf::initialize(ros::NodeHandle& nh, const std::string& topic)
void PublisherGnssTf::publishDerived()
{
- tf::Matrix3x3 tf_R_enu_map, tf_R_enu_ecef;
- tf::Vector3 tf_t_enu_map, tf_t_enu_ecef;
-
if (not sensor_gnss_->isEnuDefined())
return;
- //Eigen::Matrix3d ei_R_enu_ecef = sensor_gnss_->getREnuEcef();
- //Eigen::Vector3d ei_t_enu_ecef = sensor_gnss_->gettEnuEcef();
-
- //Eigen::Matrix3d ei_R_enu_map = sensor_gnss_->getREnuMap();
- //Eigen::Vector3d ei_t_enu_map = sensor_gnss_->gettEnuMap();
-
- //tf::matrixEigenToTF(ei_R_enu_ecef, tf_R_enu_ecef);
- //tf::vectorEigenToTF(ei_t_enu_ecef, tf_t_enu_ecef);
- tf::matrixEigenToTF(sensor_gnss_->getREnuEcef(), tf_R_enu_ecef);
- tf::vectorEigenToTF(sensor_gnss_->gettEnuEcef(), tf_t_enu_ecef);
+ // ENU-MAP
+ tf::Vector3 tf_t_enu_map;
+ tf::Matrix3x3 tf_R_enu_map;
- //tf::matrixEigenToTF(ei_R_enu_map, tf_R_enu_map);
- //tf::vectorEigenToTF(ei_t_enu_map, tf_t_enu_map);
tf::matrixEigenToTF(sensor_gnss_->getREnuMap(), tf_R_enu_map);
tf::vectorEigenToTF(sensor_gnss_->gettEnuMap(), tf_t_enu_map);
-
- T_ecef_enu_.setData(tf::Transform(tf_R_enu_ecef, tf_t_enu_ecef).inverse());
T_enu_map_ .setData(tf::Transform(tf_R_enu_map, tf_t_enu_map));
-
- T_ecef_enu_.stamp_ = ros::Time::now();
T_enu_map_ .stamp_ = ros::Time::now();
-
- tfb_.sendTransform(T_ecef_enu_);
+
tfb_.sendTransform(T_enu_map_);
+
+ // ECEF-ENU
+ 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::vectorEigenToTF(sensor_gnss_->gettEnuEcef(), tf_t_enu_ecef);
+
+ tfb_.sendTransform(T_ecef_enu_);
+ }
}
}
--
GitLab