optionally publishing ecef TF

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_;

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_);
+    }
 }
 
 }