subscribers and publishers to visualize things

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}
             )
 
 #############

include/publisher_gnss_tf.h

+#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

include/subscriber_gnss_fix_novatel_publisher_ecef.h

+#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

include/subscriber_gnss_fix_publisher_ecef.h

+#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

src/publisher_gnss_tf.cpp

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

src/subscriber_gnss_fix_novatel_publisher_ecef.cpp

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

src/subscriber_gnss_fix_publisher_ecef.cpp

+#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;
+}
+
+}