publisher fix added, not working yet

include/publisher_fix.h

+#ifndef PUBLISHER_FIX_H
+#define PUBLISHER_FIX_H
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "core/problem/problem.h"
+
+#include "publisher.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <visualization_msgs/Marker.h>
+#include <tf/transform_listener.h>
+
+namespace wolf
+{
+
+class PublishFix: public Publisher
+{
+        geometry_msgs::PoseWithCovarianceStamped pose_msg_;
+        SensorBasePtr sensor_;
+        std::string frame_id_, map_frame_id_;
+
+        ros::Publisher pub_pose_;
+
+    public:
+        PublishFix(const std::string& _unique_name,
+                      const ParamsServer& _server,
+                      const ProblemPtr _problem);
+        WOLF_PUBLISHER_CREATE(PublishFix);
+
+        virtual ~PublishFix(){};
+
+        void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+
+        void publishDerived() override;
+
+        void publishPose(const geometry_msgs::Pose pose, const ros::Time& stamp);
+
+    protected:
+
+        bool listenTf();
+        Eigen::Quaterniond q_frame_;
+        Eigen::Vector3d t_frame_;
+        tf::TransformListener tfl_;
+};
+
+WOLF_REGISTER_PUBLISHER(PublishFix)
+}
+
+#endif

src/publisher_fix.cpp

+#include "publisher_fix.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include "tf/transform_datatypes.h"
+#include "tf_conversions/tf_eigen.h"
+
+namespace wolf
+{
+
+PublishFix::PublishFix(const std::string& _unique_name,
+                             const ParamsServer& _server,
+                             const ProblemPtr _problem) :
+        Publisher(_unique_name, _server, _problem)
+{
+    sensor_     = _problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor"));
+    frame_id_   = _server.getParam<std::string>(prefix_ + "/frame_id");
+}
+
+void PublishFix::initialize(ros::NodeHandle& nh, const std::string& topic)
+{
+    nh.param<std::string>("map_frame_id", map_frame_id_, "map");
+
+    // initialize msg and publisher
+    pose_msg_.header.frame_id = frame_id_;
+
+    pub_pose_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>(topic, 1);
+}
+
+void PublishFix::publishDerived()
+{
+    VectorComposite current_state = problem_->getState("PO");
+    TimeStamp loc_ts = problem_->getTimeStamp();
+
+    // state not ready
+    if (current_state.count('P') == 0 or
+        current_state.count('O') == 0 or
+        loc_ts == TimeStamp(0))
+    {
+        return;
+    }
+
+
+    // fill vector and quaternion
+    Eigen::Vector3d p = Eigen::Vector3d::Zero();
+    Eigen::Quaterniond q;
+
+    // 2D
+    if (problem_->getDim() == 2)
+    {
+        if (extrinsics_)
+        {
+            p.head(2) = current_state['P'] + Eigen::Rotation2Dd(current_state['O'](0)) * sensor_->getP()->getState().head(2);
+            if (sensor_->getO())
+                q = Eigen::Quaterniond(Eigen::AngleAxisd(current_state['O'](0) + sensor_->getO()->getState()(0),
+                                                         Eigen::Vector3d::UnitZ()));
+            else
+                q = Eigen::Quaterniond(Eigen::AngleAxisd(current_state['O'](0),
+                                                         Eigen::Vector3d::UnitZ()));
+        }
+        else
+        {
+            p.head(2) = current_state['P'];
+            q = Eigen::Quaterniond(Eigen::AngleAxisd(current_state['O'](0), Eigen::Vector3d::UnitZ()));
+        }
+    }
+    // 3D
+    else
+    {
+        if (extrinsics_)
+        {
+            p = current_state['P'] + Eigen::Quaterniond(Eigen::Vector4d(current_state['O'])) * sensor_->getP()->getState();
+            if (sensor_->getO())
+                q = Eigen::Quaterniond(Eigen::Vector4d(current_state['O'])) * Eigen::Quaterniond(Eigen::Vector4d(sensor_->getO()->getState()));
+            else
+                q = Eigen::Quaterniond(Eigen::Vector4d(current_state['O']));
+        }
+        else
+        {
+            p = current_state['P'];
+            q = Eigen::Quaterniond(Eigen::Vector4d(current_state['O']));
+        }
+    }
+
+    // Change frame
+    if (frame_id_ != map_frame_id_ and listenTf())
+    {
+        p = t_frame_ + q_frame_ * p;
+        q = q_frame_ * q;
+    }
+
+    // Fill Pose msg
+    geometry_msgs::Pose pose_msg;
+    pose_msg.position.x = p(0);
+    pose_msg.position.y = p(1);
+    pose_msg.position.z = p(2);
+
+    pose_msg.orientation.x = q.x();
+    pose_msg.orientation.y = q.y();
+    pose_msg.orientation.z = q.z();
+    pose_msg.orientation.w = q.w();
+    publishPose(pose_msg, ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds()));
+}
+
+void PublishFix::publishPose(const geometry_msgs::Pose pose, const ros::Time& stamp)
+{
+    // fill msgs and publish
+    if (pose_array_)
+    {
+        pose_array_msg_.header.stamp = stamp;
+        pose_array_msg_.poses.push_back(pose);
+
+        pub_pose_array_.publish(pose_array_msg_);
+    }
+    if (marker_)
+    {
+        marker_msg_.header.stamp = stamp;
+        marker_msg_.points.push_back(pose.position);
+
+        pub_marker_.publish(marker_msg_);
+    }
+}
+
+bool PublishFix::listenTf()
+{
+    tf::StampedTransform T;
+    if ( tfl_.waitForTransform(frame_id_, map_frame_id_, ros::Time(0), ros::Duration(0.01)) )
+    {
+        tfl_.lookupTransform(frame_id_, map_frame_id_, ros::Time(0), T);
+
+        Eigen::Matrix3d R;
+        tf::matrixTFToEigen(T.getBasis(), R);
+        tf::vectorTFToEigen(T.getOrigin(), t_frame_);
+        q_frame_ = Eigen::Quaterniond(R);
+
+        return true;
+    }
+    return false;
+}
+
+}