diff --git a/CMakeLists.txt b/CMakeLists.txt
index 765e692547b8d1dac996fecc1adee822f972dcec..bd507875618ae70cb4092b1098a565006bf78289 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -151,6 +151,8 @@ add_executable(${PROJECT_NAME} src/node.cpp)
 add_library(subscriber_${PROJECT_NAME}
   src/subscriber_odom2d.cpp
   src/subscriber_diffdrive.cpp)
+add_library(publisher_${PROJECT_NAME}
+  src/publisher_pose.cpp)
 add_library(visualizer src/visualizer.cpp)
 
 ## Rename C++ executable without prefix
diff --git a/include/publisher_pose.h b/include/publisher_pose.h
new file mode 100644
index 0000000000000000000000000000000000000000..4392b3525a7789d1e3ccd4433b2f0ecc52182d46
--- /dev/null
+++ b/include/publisher_pose.h
@@ -0,0 +1,30 @@
+#ifndef PUBLISHER_POSE_H
+#define PUBLISHER_POSE_H
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "core/problem/problem.h"
+
+#include "factory_publisher.h"
+#include "publisher.h"
+
+namespace wolf
+{
+
+class PublisherPose: public Publisher
+{
+        std::string map_frame_id_;
+
+    public:
+        PublisherPose();
+        virtual ~PublisherPose(){};
+        void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+        void publish(const ProblemPtr problem) override;
+        static std::shared_ptr<Publisher> create();
+};
+
+WOLF_REGISTER_PUBLISHER(PublisherPose)
+}
+
+#endif
diff --git a/src/publisher_pose.cpp b/src/publisher_pose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9a8164533e19407e1ba12b2b226459d46354dacd
--- /dev/null
+++ b/src/publisher_pose.cpp
@@ -0,0 +1,72 @@
+#include "publisher_pose.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include "tf/transform_datatypes.h"
+#include <nav_msgs/Odometry.h>
+
+namespace wolf
+{
+
+PublisherPose::PublisherPose() :
+    Publisher()
+{
+}
+
+void PublisherPose::initialize(ros::NodeHandle& nh, const std::string& topic)
+{
+    publisher_ = nh.advertise<nav_msgs::Odometry>(topic, 1);
+    nh.param<std::string>("map_frame_id", map_frame_id_, "map");
+}
+
+void PublisherPose::publish(const ProblemPtr _problem)
+{
+    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 PoseStamped msg
+    nav_msgs::Odometry msg;
+    msg.header.frame_id = map_frame_id_;
+    msg.header.stamp = ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds());
+
+    // 2D
+    if (_problem->getDim() == 2)
+    {
+        msg.pose.pose.position.x = current_state["P"](0);
+        msg.pose.pose.position.y = current_state["P"](1);
+        msg.pose.pose.position.z = 0;
+
+        msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(current_state["O"](0));
+    }
+    // 3D
+    else
+    {
+        msg.pose.pose.position.x = current_state["P"](0);
+        msg.pose.pose.position.y = current_state["P"](1);
+        msg.pose.pose.position.z = current_state["P"](2);
+
+        msg.pose.pose.orientation.x = current_state["O"](0);
+        msg.pose.pose.orientation.y = current_state["O"](1);
+        msg.pose.pose.orientation.z = current_state["O"](2);
+        msg.pose.pose.orientation.w = current_state["O"](3);
+    }
+
+    publisher_.publish(msg);
+}
+
+std::shared_ptr<Publisher> PublisherPose::create()
+{
+    return std::make_shared<PublisherPose>();
+}
+
+}