diff --git a/CMakeLists.txt b/CMakeLists.txt
index 161bfba7b1dc0d8bcba7e67d89e98a92e0413c5a..79f3a083a35fade4c72b0ea711b365d937864f2f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
iri_gnss_msgs
std_msgs
tf
+ tf_conversions
dynamic_reconfigure
wolf_ros_node
novatel_oem7_msgs
@@ -135,9 +136,13 @@ include_directories(
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/wolf_ros.cpp
# )
+add_library(publisher_${PROJECT_NAME}
+ src/publisher_gnss_tf.cpp)
add_library(subscriber_${PROJECT_NAME}
src/subscriber_gnss.cpp
src/subscriber_gnss_fix.cpp
+ src/subscriber_gnss_fix_publisher_ecef.cpp
+ src/subscriber_gnss_fix_novatel_publisher_ecef.cpp
src/subscriber_gnss_tdcp.cpp
src/subscriber_gnss_receiver.cpp
src/subscriber_gnss_ublox.cpp
@@ -171,6 +176,13 @@ target_link_libraries(subscriber_${PROJECT_NAME}
${wolf_LIBRARIES}
${wolfgnss_LIBRARIES}
${iri_gnss_msgs_LIBRARIES}
+ ${catkin_LIBRARIES}
+ )
+target_link_libraries(publisher_${PROJECT_NAME}
+ ${wolf_LIBRARIES}
+ ${wolfgnss_LIBRARIES}
+ ${iri_gnss_msgs_LIBRARIES}
+ ${catkin_LIBRARIES}
)
#############
diff --git a/include/publisher_gnss_tf.h b/include/publisher_gnss_tf.h
new file mode 100644
index 0000000000000000000000000000000000000000..b88c88b3b428992868f049e759ac095ef072cdf9
--- /dev/null
+++ b/include/publisher_gnss_tf.h
@@ -0,0 +1,40 @@
+#ifndef PUBLISHER_GNSS_TF_H
+#define PUBLISHER_GNSS_TF_H
+
+/**************************
+ * WOLF includes *
+ **************************/
+#include "core/problem/problem.h"
+#include "gnss/sensor/sensor_gnss.h"
+
+#include "publisher.h"
+
+#include <tf/transform_broadcaster.h>
+
+namespace wolf
+{
+
+class PublisherGnssTf: public Publisher
+{
+ std::string map_frame_id_, enu_frame_id_, ecef_frame_id_;
+ tf::StampedTransform T_enu_map_, T_ecef_enu_;
+ tf::TransformBroadcaster tfb_;
+ SensorGnssPtr sensor_gnss_;
+
+ public:
+ PublisherGnssTf(const std::string& _unique_name,
+ const ParamsServer& _server,
+ const ProblemPtr _problem);
+ WOLF_PUBLISHER_CREATE(PublisherGnssTf);
+
+ virtual ~PublisherGnssTf(){};
+
+ void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+
+ void publishDerived() override;
+};
+
+WOLF_REGISTER_PUBLISHER(PublisherGnssTf)
+}
+
+#endif
diff --git a/include/subscriber_gnss_fix_novatel_publisher_ecef.h b/include/subscriber_gnss_fix_novatel_publisher_ecef.h
new file mode 100644
index 0000000000000000000000000000000000000000..7d905ad1792d23fffed60bae3dc5a19980b53581
--- /dev/null
+++ b/include/subscriber_gnss_fix_novatel_publisher_ecef.h
@@ -0,0 +1,41 @@
+#ifndef SUBSCRIBER_GNSS_FIX_NOVATEL_PUBLISHER_ECEF_H
+#define SUBSCRIBER_GNSS_FIX_NOVATEL_PUBLISHER_ECEF_H
+
+/**************************
+ * ROS includes *
+ **************************/
+#include <ros/ros.h>
+#include "novatel_oem7_msgs/BESTPOS.h"
+
+/**************************
+ * STD includes *
+ **************************/
+#include <iostream>
+#include <iomanip>
+#include <queue>
+
+/**************************
+ * WOLF-ROS includes *
+ **************************/
+#include "subscriber_gnss_fix_publisher_ecef.h"
+
+namespace wolf
+{
+
+class SubscriberGnssFixNovatelPublisherEcef : public SubscriberGnssFixPublisherEcef
+{
+ public:
+ // Constructor
+ SubscriberGnssFixNovatelPublisherEcef(const std::string& _unique_name,
+ const ParamsServer& _server,
+ const SensorBasePtr _sensor_ptr);
+ WOLF_SUBSCRIBER_CREATE(SubscriberGnssFixNovatelPublisherEcef);
+
+ void initialize(ros::NodeHandle& nh, const std::string& topic) override;
+
+ void callback(const novatel_oem7_msgs::BESTPOS::ConstPtr& msg);
+};
+WOLF_REGISTER_SUBSCRIBER(SubscriberGnssFixNovatelPublisherEcef)
+
+}
+#endif
diff --git a/include/subscriber_gnss_fix_publisher_ecef.h b/include/subscriber_gnss_fix_publisher_ecef.h
new file mode 100644
index 0000000000000000000000000000000000000000..71221dae6467a43c2a4c353a967a0d73251ed61e
--- /dev/null
+++ b/include/subscriber_gnss_fix_publisher_ecef.h
@@ -0,0 +1,57 @@
+#ifndef SUBSCRIBER_GNSS_FIX_PUBLISHER_ECEF_H
+#define SUBSCRIBER_GNSS_FIX_PUBLISHER_ECEF_H
+
+/**************************
+ * ROS includes *
+ **************************/
+#include <ros/ros.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <nav_msgs/Odometry.h>
+#include <tf/transform_listener.h>
+
+/**************************
+ * STD includes *
+ **************************/
+#include <iostream>
+#include <iomanip>
+#include <queue>
+
+/**************************
+ * WOLF-ROS includes *
+ **************************/
+#include "subscriber.h"
+
+namespace wolf
+{
+
+class SubscriberGnssFixPublisherEcef : public Subscriber
+{
+ protected:
+ ros::Publisher pub_;
+ tf::TransformListener tfl_;
+ bool ENU_defined_;
+ Eigen::Matrix3d R_enu_ecef_;
+ Eigen::Vector3d t_enu_ecef_;
+ nav_msgs::Odometry odom_msg_;
+ ros::Time last_publish_stamp_;
+ double period_;
+
+ public:
+ // Constructor
+ SubscriberGnssFixPublisherEcef(const std::string& _unique_name,
+ const ParamsServer& _server,
+ const SensorBasePtr _sensor_ptr);
+ WOLF_SUBSCRIBER_CREATE(SubscriberGnssFixPublisherEcef);
+
+ virtual void initialize(ros::NodeHandle& nh, const std::string& topic);
+
+ void callback(const sensor_msgs::NavSatFix::ConstPtr& msg);
+
+ void computeAndPublish(const Eigen::Vector3d& LatLonAlt, const ros::Time& stamp);
+
+ bool listenTf();
+};
+WOLF_REGISTER_SUBSCRIBER(SubscriberGnssFixPublisherEcef)
+
+}
+#endif
diff --git a/src/publisher_gnss_tf.cpp b/src/publisher_gnss_tf.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eafb1bbca4c2198999921d55993ac47bd6ed730f
--- /dev/null
+++ b/src/publisher_gnss_tf.cpp
@@ -0,0 +1,70 @@
+#include "publisher_gnss_tf.h"
+
+/**************************
+ * ROS includes *
+ **************************/
+#include <ros/ros.h>
+#include "tf/transform_datatypes.h"
+#include "tf_conversions/tf_eigen.h"
+
+namespace wolf
+{
+
+PublisherGnssTf::PublisherGnssTf(const std::string& _unique_name,
+ const ParamsServer& _server,
+ const ProblemPtr _problem) :
+ Publisher(_unique_name, _server, _problem)
+{
+ sensor_gnss_ = std::static_pointer_cast<SensorGnss>(_problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor_gnss_name")));
+
+ T_enu_map_.setIdentity();
+ T_enu_map_.frame_id_ = "ENU";
+ T_enu_map_.child_frame_id_ = "map";
+ 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();
+}
+
+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);
+
+ //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_);
+}
+
+}
diff --git a/src/subscriber_gnss_fix_novatel_publisher_ecef.cpp b/src/subscriber_gnss_fix_novatel_publisher_ecef.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..85d22cf475bab2c014c4d2791f4b8c57b519597d
--- /dev/null
+++ b/src/subscriber_gnss_fix_novatel_publisher_ecef.cpp
@@ -0,0 +1,31 @@
+#include "subscriber_gnss_fix_novatel_publisher_ecef.h"
+#include "gnss_utils/utils/transformations.h"
+#include "tf/transform_datatypes.h"
+
+namespace wolf
+{
+
+// Constructor
+SubscriberGnssFixNovatelPublisherEcef::SubscriberGnssFixNovatelPublisherEcef(const std::string& _unique_name,
+ const ParamsServer& _server,
+ const SensorBasePtr _sensor_ptr)
+ : SubscriberGnssFixPublisherEcef(_unique_name, _server, _sensor_ptr)
+{
+}
+
+
+void SubscriberGnssFixNovatelPublisherEcef::initialize(ros::NodeHandle& nh, const std::string& topic)
+{
+ sub_ = nh.subscribe(topic, 1, &SubscriberGnssFixNovatelPublisherEcef::callback, this);
+ pub_ = nh.advertise<nav_msgs::Odometry>(topic+"_ECEF", 1);
+}
+
+void SubscriberGnssFixNovatelPublisherEcef::callback(const novatel_oem7_msgs::BESTPOS::ConstPtr& _msg)
+{
+ computeAndPublish(Eigen::Vector3d(_msg->lat * M_PI / 180.0,
+ _msg->lon * M_PI / 180.0,
+ _msg->hgt),
+ _msg->header.stamp);
+}
+
+}
diff --git a/src/subscriber_gnss_fix_publisher_ecef.cpp b/src/subscriber_gnss_fix_publisher_ecef.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..90f8729d931ffab4b221e9cb01b85fbb3be85708
--- /dev/null
+++ b/src/subscriber_gnss_fix_publisher_ecef.cpp
@@ -0,0 +1,77 @@
+#include "subscriber_gnss_fix_publisher_ecef.h"
+#include "gnss_utils/utils/transformations.h"
+#include "tf/transform_datatypes.h"
+#include "tf_conversions/tf_eigen.h"
+
+namespace wolf
+{
+
+// Constructor
+SubscriberGnssFixPublisherEcef::SubscriberGnssFixPublisherEcef(const std::string& _unique_name,
+ const ParamsServer& _server,
+ const SensorBasePtr _sensor_ptr)
+ : Subscriber(_unique_name, _server, _sensor_ptr)
+ , ENU_defined_(false)
+{
+ period_ = _server.getParam<double>(prefix_ + "/period");
+ odom_msg_.header.frame_id = "ENU";
+ odom_msg_.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,M_PI/2,0);
+}
+
+
+void SubscriberGnssFixPublisherEcef::initialize(ros::NodeHandle& nh, const std::string& topic)
+{
+ sub_ = nh.subscribe(topic, 1, &SubscriberGnssFixPublisherEcef::callback, this);
+ pub_ = nh.advertise<nav_msgs::Odometry>(topic+"_ECEF", 1);
+}
+
+void SubscriberGnssFixPublisherEcef::callback(const sensor_msgs::NavSatFix::ConstPtr& _msg)
+{
+ computeAndPublish(Eigen::Vector3d(_msg->latitude * M_PI / 180.0,
+ _msg->longitude * M_PI / 180.0,
+ _msg->altitude),
+ _msg->header.stamp);
+}
+
+void SubscriberGnssFixPublisherEcef::computeAndPublish(const Eigen::Vector3d& LatLonAlt, const ros::Time& stamp)
+{
+ // listen TF ECEF-ENU once
+ if (not ENU_defined_)
+ if (not listenTf())
+ return;
+
+ if ((stamp - last_publish_stamp_).toSec() < period_)
+ return;
+
+ // ECEF-ENU is defined
+ Eigen::Vector3d fix_ENU = R_enu_ecef_ * GnssUtils::latLonAltToEcef(LatLonAlt) + t_enu_ecef_;
+
+ // Fill Odometry msg
+ odom_msg_.header.stamp = stamp;
+
+ odom_msg_.pose.pose.position.x = fix_ENU(0);
+ odom_msg_.pose.pose.position.y = fix_ENU(1);
+ odom_msg_.pose.pose.position.z = fix_ENU(2);
+
+ pub_.publish(odom_msg_);
+
+ last_publish_stamp_ = stamp;
+}
+
+bool SubscriberGnssFixPublisherEcef::listenTf()
+{
+ tf::StampedTransform T_ecef2enu;
+ if ( tfl_.waitForTransform("ENU", "ECEF", ros::Time(0), ros::Duration(0.01)) )
+ {
+ tfl_.lookupTransform("ENU", "ECEF", ros::Time(0), T_ecef2enu);
+
+ tf::matrixTFToEigen(T_ecef2enu.getBasis(), R_enu_ecef_);
+ tf::vectorTFToEigen(T_ecef2enu.getOrigin(), t_enu_ecef_);
+
+ ENU_defined_ = true;
+ return true;
+ }
+ return false;
+}
+
+}