diff --git a/sr_mechanism_controllers/CMakeLists.txt b/sr_mechanism_controllers/CMakeLists.txt
index cbaefb03d..53557852b 100644
--- a/sr_mechanism_controllers/CMakeLists.txt
+++ b/sr_mechanism_controllers/CMakeLists.txt
@@ -49,6 +49,7 @@ add_library(sr_mechanism_controllers
         example/srh_syntouch_controllers.cpp
         src/sr_controller.cpp
         src/sr_plain_pid.cpp
+        src/joint_state_controller.cpp
 )
 add_dependencies(sr_mechanism_controllers ${catkin_EXPORTED_TARGETS})
 target_link_libraries(sr_mechanism_controllers ${catkin_LIBRARIES})
diff --git a/sr_mechanism_controllers/controller_plugins.xml b/sr_mechanism_controllers/controller_plugins.xml
index c88a99492..6751e123e 100644
--- a/sr_mechanism_controllers/controller_plugins.xml
+++ b/sr_mechanism_controllers/controller_plugins.xml
@@ -26,4 +26,7 @@
     <class name="sr_mechanism_controllers/SrhJointVariablePidPositionController"
            type="controller::SrhJointVariablePidPositionController"
            base_class_type="controller_interface::ControllerBase"/>
+    <class name="sr_mechanism_controllers/JointStateController"
+           type="controller::JointStateController"
+           base_class_type="controller_interface::ControllerBase"/>
 </library>
diff --git a/sr_mechanism_controllers/include/sr_mechanism_controllers/joint_state_controller.h b/sr_mechanism_controllers/include/sr_mechanism_controllers/joint_state_controller.h
new file mode 100644
index 000000000..73f4c9b81
--- /dev/null
+++ b/sr_mechanism_controllers/include/sr_mechanism_controllers/joint_state_controller.h
@@ -0,0 +1,104 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2012, hiDOF INC.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of hiDOF, Inc. nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+/*
+ * Author: Wim Meeussen
+ */
+
+#pragma once
+
+
+#include <boost/circular_buffer.hpp>
+#include <boost/scoped_ptr.hpp>
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <memory>
+#include <sr_utilities/realtime_publisher.h>
+#include <sensor_msgs/JointState.h>
+
+namespace controller
+{
+
+/**
+ * \brief Controller that publishes the state of all joints in a robot.
+ *
+ * This controller publishes the state of all resources registered to a \c hardware_interface::JointStateInterface to a
+ * topic of type \c sensor_msgs/JointState. The following is a basic configuration of the controller.
+ *
+ * \code
+ * joint_state_controller:
+ *   type: joint_state_controller/JointStateController
+ *   publish_rate: 50
+ * \endcode
+ *
+ * It's possible to optionally specify a set of extra joints not contained in a
+ * \c hardware_interface::JointStateInterface with custom (and static) default values. The following is an example
+ *  configuration specifying extra joints.
+ *
+ * \code
+ * joint_state_controller:
+ *   type: joint_state_controller/JointStateController
+ *   publish_rate: 50
+ *   extra_joints:
+ *     - name:     'extra1'
+ *       position: 10.0
+ *       velocity: 20.0
+ *       effort:   30.0
+ *
+ *     - name:     'extra2'
+ *       position: -10.0
+ *
+ *     - name:     'extra3'
+ * \endcode
+ *
+ * An unspecified position, velocity or acceleration defaults to zero.
+ */
+class JointStateController: public controller_interface::Controller<hardware_interface::JointStateInterface>
+{
+public:
+  JointStateController() : publish_rate_(0.0) {}
+
+  virtual bool init(hardware_interface::JointStateInterface* hw,
+                    ros::NodeHandle&                         root_nh,
+                    ros::NodeHandle&                         controller_nh);
+  virtual void starting(const ros::Time& time);
+  virtual void update(const ros::Time& time, const ros::Duration& /*period*/);
+  virtual void stopping(const ros::Time& /*time*/);
+
+private:
+  std::vector<hardware_interface::JointStateHandle> joint_state_;
+  std::shared_ptr<sr_utilities::RealtimePublisher<sensor_msgs::JointState> > realtime_pub_;
+  boost::scoped_ptr<boost::circular_buffer<sensor_msgs::JointState> > msg_buffer_;
+  sensor_msgs::JointState msg_;
+  ros::Time last_publish_time_;
+  double publish_rate_;
+  unsigned int num_hw_joints_; ///< Number of joints present in the JointStateInterface, excluding extra joints
+
+  void addExtraJoints(const ros::NodeHandle& nh, sensor_msgs::JointState& msg);
+};
+
+}
\ No newline at end of file
diff --git a/sr_mechanism_controllers/include/sr_mechanism_controllers/sr_controller.hpp b/sr_mechanism_controllers/include/sr_mechanism_controllers/sr_controller.hpp
index 1877c4b09..d7f9cbbf1 100644
--- a/sr_mechanism_controllers/include/sr_mechanism_controllers/sr_controller.hpp
+++ b/sr_mechanism_controllers/include/sr_mechanism_controllers/sr_controller.hpp
@@ -28,13 +28,14 @@
 #define _SRH_CONTROLLER_HPP_
 
 #include <ros/node_handle.h>
+#include <boost/circular_buffer.hpp>
 
 #include <controller_interface/controller.h>
 #include <ros_ethercat_model/robot_state_interface.hpp>
 #include <control_toolbox/pid.h>
 #include <boost/scoped_ptr.hpp>
 #include <boost/thread/condition.hpp>
-#include <realtime_tools/realtime_publisher.h>
+#include <sr_utilities/realtime_publisher.h>
 #include <std_msgs/Float64.h>
 #include <std_srvs/Empty.h>
 #include <control_msgs/JointControllerState.h>
@@ -145,8 +146,11 @@ class SrController :
   ros::NodeHandle node_, n_tilde_;
   std::string joint_name_;
 
-  boost::scoped_ptr<realtime_tools::RealtimePublisher
+  boost::scoped_ptr<sr_utilities::RealtimePublisher
           <control_msgs::JointControllerState> > controller_state_publisher_;
+  
+  boost::scoped_ptr<boost::circular_buffer<control_msgs::JointControllerState> > msg_buffer_;
+  control_msgs::JointControllerState msg_;
 
   boost::scoped_ptr<sr_friction_compensation::SrFrictionCompensator> friction_compensator;
 
diff --git a/sr_mechanism_controllers/src/joint_state_controller.cpp b/sr_mechanism_controllers/src/joint_state_controller.cpp
new file mode 100644
index 000000000..7853a3f81
--- /dev/null
+++ b/sr_mechanism_controllers/src/joint_state_controller.cpp
@@ -0,0 +1,205 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2012, hiDOF INC.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of hiDOF Inc nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+/*
+ * Author: Wim Meeussen
+ */
+
+#include <algorithm>
+#include <cstddef>
+#include <pluginlib/class_list_macros.hpp>
+
+#include "sr_mechanism_controllers/joint_state_controller.h"
+
+namespace controller
+{
+
+  bool JointStateController::init(hardware_interface::JointStateInterface* hw,
+                                  ros::NodeHandle&                         root_nh,
+                                  ros::NodeHandle&                         controller_nh)
+  {
+    // List of joints to be published
+    std::vector<std::string> joint_names;
+
+    // Get list of joints: This allows specifying a desired order, or
+    // alternatively, only publish states for a subset of joints. If the
+    // parameter is not set, all joint states will be published in the order
+    // specified by the hardware interface.
+    if (controller_nh.getParam("joints", joint_names)) {
+      ROS_INFO_STREAM("Joints parameter specified, publishing specified joints in desired order.");
+    } else {
+      // get all joint names from the hardware interface
+      joint_names = hw->getNames();
+    }
+
+    num_hw_joints_ = joint_names.size();
+    for (unsigned i=0; i<num_hw_joints_; i++)
+      ROS_DEBUG("Got joint %s", joint_names[i].c_str());
+
+    // get publishing period
+    if (!controller_nh.getParam("publish_rate", publish_rate_)){
+      ROS_ERROR("Parameter 'publish_rate' not set");
+      return false;
+    }
+
+    int queue_size = 50;
+    // realtime publisher
+    realtime_pub_.reset(new sr_utilities::RealtimePublisher<sensor_msgs::JointState>(root_nh, "joint_states", queue_size));
+    msg_buffer_.reset(new boost::circular_buffer<sensor_msgs::JointState>(queue_size));
+
+    // get joints and allocate message
+    for (unsigned i=0; i<num_hw_joints_; i++){
+      joint_state_.push_back(hw->getHandle(joint_names[i]));
+      msg_.name.push_back(joint_names[i]);
+      msg_.position.push_back(0.0);
+      msg_.velocity.push_back(0.0);
+      msg_.effort.push_back(0.0);
+    }
+    addExtraJoints(controller_nh, msg_);
+
+    return true;
+  }
+
+  void JointStateController::starting(const ros::Time& time)
+  {
+    // initialize time
+    last_publish_time_ = time;
+  }
+
+  void JointStateController::update(const ros::Time& time, const ros::Duration& /*period*/)
+  {
+    // limit rate of publishing
+    if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0/publish_rate_) < time){
+      
+      // populate joint state message:
+      // - fill only joints that are present in the JointStateInterface, i.e. indices [0, num_hw_joints_)
+      // - leave unchanged extra joints, which have static values, i.e. indices from num_hw_joints_ onwards
+      msg_.header.stamp = time;
+      for (unsigned i=0; i<num_hw_joints_; i++){
+        msg_.position[i] = joint_state_[i].getPosition();
+        msg_.velocity[i] = joint_state_[i].getVelocity();
+        msg_.effort[i] = joint_state_[i].getEffort();
+      }
+      msg_buffer_->push_back(msg_);
+
+      // try to publish
+      if (realtime_pub_->trylock()){
+        // we're actually publishing, so increment time
+        last_publish_time_ = last_publish_time_ + ros::Duration(1.0/publish_rate_);
+
+        while(!msg_buffer_->empty())
+        {
+            realtime_pub_->msg_buffer_->push_back(msg_buffer_->front());
+            msg_buffer_->pop_front();
+        }
+        
+        realtime_pub_->unlockAndPublish();
+      }
+    }
+  }
+
+  void JointStateController::stopping(const ros::Time& /*time*/)
+  {}
+
+  void JointStateController::addExtraJoints(const ros::NodeHandle& nh, sensor_msgs::JointState& msg)
+  {
+
+    // Preconditions
+    XmlRpc::XmlRpcValue list;
+    if (!nh.getParam("extra_joints", list))
+    {
+      ROS_DEBUG("No extra joints specification found.");
+      return;
+    }
+
+    if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
+    {
+      ROS_ERROR("Extra joints specification is not an array. Ignoring.");
+      return;
+    }
+
+    for(std::size_t i = 0; i < list.size(); ++i)
+    {
+      XmlRpc::XmlRpcValue& elem = list[i];
+
+      if (elem.getType() != XmlRpc::XmlRpcValue::TypeStruct)
+      {
+        ROS_ERROR_STREAM("Extra joint specification is not a struct, but rather '" << elem.getType() <<
+                         "'. Ignoring.");
+        continue;
+      }
+
+      if (!elem.hasMember("name"))
+      {
+        ROS_ERROR_STREAM("Extra joint does not specify name. Ignoring.");
+        continue;
+      }
+
+      const std::string name = elem["name"];
+      if (std::find(msg.name.begin(), msg.name.end(), name) != msg.name.end())
+      {
+        ROS_WARN_STREAM("Joint state interface already contains specified extra joint '" << name << "'.");
+        continue;
+      }
+
+      const bool has_pos = elem.hasMember("position");
+      const bool has_vel = elem.hasMember("velocity");
+      const bool has_eff = elem.hasMember("effort");
+
+      const XmlRpc::XmlRpcValue::Type typeDouble = XmlRpc::XmlRpcValue::TypeDouble;
+      if (has_pos && elem["position"].getType() != typeDouble)
+      {
+        ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default position. Ignoring.");
+        continue;
+      }
+      if (has_vel && elem["velocity"].getType() != typeDouble)
+      {
+        ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default velocity. Ignoring.");
+        continue;
+      }
+      if (has_eff && elem["effort"].getType() != typeDouble)
+      {
+        ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default effort. Ignoring.");
+        continue;
+      }
+
+      // State of extra joint
+      const double pos = has_pos ? static_cast<double>(elem["position"]) : 0.0;
+      const double vel = has_vel ? static_cast<double>(elem["velocity"]) : 0.0;
+      const double eff = has_eff ? static_cast<double>(elem["effort"])   : 0.0;
+
+      // Add extra joints to message
+      msg.name.push_back(name);
+      msg.position.push_back(pos);
+      msg.velocity.push_back(vel);
+      msg.effort.push_back(eff);
+    }
+  }
+
+}
+
+PLUGINLIB_EXPORT_CLASS( controller::JointStateController, controller_interface::ControllerBase)
\ No newline at end of file
diff --git a/sr_mechanism_controllers/src/srh_joint_effort_controller.cpp b/sr_mechanism_controllers/src/srh_joint_effort_controller.cpp
index 29d84bbee..1580df7c4 100644
--- a/sr_mechanism_controllers/src/srh_joint_effort_controller.cpp
+++ b/sr_mechanism_controllers/src/srh_joint_effort_controller.cpp
@@ -70,8 +70,10 @@ namespace controller
       return false;
     }
 
-    controller_state_publisher_.reset(new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>
-                                              (node_, "state", 1));
+    int queue_size = 50;
+    controller_state_publisher_.reset(new sr_utilities::RealtimePublisher<control_msgs::JointControllerState>
+                                              (node_, "state", queue_size));
+    msg_buffer_.reset(new boost::circular_buffer<control_msgs::JointControllerState>(queue_size));
 
     ROS_DEBUG(" --------- ");
     ROS_DEBUG_STREAM("Init: " << joint_name_);
@@ -193,25 +195,32 @@ namespace controller
 
     joint_state_->commanded_effort_ = commanded_effort;
 
-    if (loop_count_ % 10 == 0)
+    if (true) //(loop_count_ % 10 == 0)
     {
+      msg_.header.stamp = time;
+      msg_.set_point = command_;
+      msg_.process_value = joint_state_->effort_;
+      // @todo compute the derivative of the effort.
+      msg_.process_value_dot = -1.0;
+      msg_.error = commanded_effort - joint_state_->effort_;
+      msg_.time_step = period.toSec();
+      msg_.command = commanded_effort;
+
+      double dummy;
+      getGains(msg_.p,
+                msg_.i,
+                msg_.d,
+                msg_.i_clamp,
+                dummy);
+      msg_buffer_->push_back(msg_);
+
       if (controller_state_publisher_ && controller_state_publisher_->trylock())
       {
-        controller_state_publisher_->msg_.header.stamp = time;
-        controller_state_publisher_->msg_.set_point = command_;
-        controller_state_publisher_->msg_.process_value = joint_state_->effort_;
-        // @todo compute the derivative of the effort.
-        controller_state_publisher_->msg_.process_value_dot = -1.0;
-        controller_state_publisher_->msg_.error = commanded_effort - joint_state_->effort_;
-        controller_state_publisher_->msg_.time_step = period.toSec();
-        controller_state_publisher_->msg_.command = commanded_effort;
-
-        double dummy;
-        getGains(controller_state_publisher_->msg_.p,
-                 controller_state_publisher_->msg_.i,
-                 controller_state_publisher_->msg_.d,
-                 controller_state_publisher_->msg_.i_clamp,
-                 dummy);
+        while(!msg_buffer_->empty())
+        {
+            controller_state_publisher_->msg_buffer_->push_back(msg_buffer_->front());
+            msg_buffer_->pop_front();
+        }
         controller_state_publisher_->unlockAndPublish();
       }
     }
diff --git a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp
index c33d651e6..0070a69d5 100644
--- a/sr_mechanism_controllers/src/srh_joint_position_controller.cpp
+++ b/sr_mechanism_controllers/src/srh_joint_position_controller.cpp
@@ -80,8 +80,10 @@ namespace controller
       return false;
     }
 
-    controller_state_publisher_.reset(new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>
-                                              (node_, "state", 1));
+    int queue_size = 50;
+    controller_state_publisher_.reset(new sr_utilities::RealtimePublisher<control_msgs::JointControllerState>
+                                              (node_, "state", queue_size));
+    msg_buffer_.reset(new boost::circular_buffer<control_msgs::JointControllerState>(queue_size));
 
     ROS_DEBUG(" --------- ");
     ROS_DEBUG_STREAM("Init: " << joint_name_);
@@ -223,7 +225,8 @@ namespace controller
       error_position = joint_state_->position_ - command_;
     }
 
-    bool in_deadband = hysteresis_deadband.is_in_deadband(command_, error_position, position_deadband);
+    // setting nb_errors_for_avg to 1:
+    bool in_deadband = hysteresis_deadband.is_in_deadband(command_, error_position, position_deadband, 5.0, 1);
 
     // don't compute the error if we're in the deadband.
     if (in_deadband)
@@ -258,33 +261,38 @@ namespace controller
 
     joint_state_->commanded_effort_ = commanded_effort;
 
-    if (loop_count_ % 10 == 0)
+    if (true) // if (loop_count_ % 10 == 0)
     {
+      msg_.header.stamp = time;
+      msg_.set_point = command_;
+      if (has_j2)
+      {
+        msg_.process_value = joint_state_->position_ + joint_state_2->position_;
+        msg_.process_value_dot = joint_state_->velocity_ + joint_state_2->velocity_;
+      }
+      else
+      {
+        msg_.process_value = joint_state_->position_;
+        msg_.process_value_dot = joint_state_->velocity_;
+      }
+      msg_.error = error_position;
+      msg_.time_step = period.toSec();
+      msg_.command = commanded_effort;
+
+      double dummy;
+      getGains(msg_.p,
+                msg_.i,
+                msg_.d,
+                msg_.i_clamp,
+                dummy);
+      msg_buffer_->push_back(msg_);
       if (controller_state_publisher_ && controller_state_publisher_->trylock())
       {
-        controller_state_publisher_->msg_.header.stamp = time;
-        controller_state_publisher_->msg_.set_point = command_;
-        if (has_j2)
-        {
-          controller_state_publisher_->msg_.process_value = joint_state_->position_ + joint_state_2->position_;
-          controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_ + joint_state_2->velocity_;
-        }
-        else
+        while(!msg_buffer_->empty())
         {
-          controller_state_publisher_->msg_.process_value = joint_state_->position_;
-          controller_state_publisher_->msg_.process_value_dot = joint_state_->velocity_;
+            controller_state_publisher_->msg_buffer_->push_back(msg_buffer_->front());
+            msg_buffer_->pop_front();
         }
-
-        controller_state_publisher_->msg_.error = error_position;
-        controller_state_publisher_->msg_.time_step = period.toSec();
-        controller_state_publisher_->msg_.command = commanded_effort;
-
-        double dummy;
-        getGains(controller_state_publisher_->msg_.p,
-                 controller_state_publisher_->msg_.i,
-                 controller_state_publisher_->msg_.d,
-                 controller_state_publisher_->msg_.i_clamp,
-                 dummy);
         controller_state_publisher_->unlockAndPublish();
       }
     }
diff --git a/sr_mechanism_controllers/src/srh_joint_variable_pid_position_controller.cpp b/sr_mechanism_controllers/src/srh_joint_variable_pid_position_controller.cpp
index 84610467b..4caa4ca9a 100644
--- a/sr_mechanism_controllers/src/srh_joint_variable_pid_position_controller.cpp
+++ b/sr_mechanism_controllers/src/srh_joint_variable_pid_position_controller.cpp
@@ -93,7 +93,7 @@ namespace controller
     double dummy;
     pid_controller_position_->getGains(p_init_, i_init_, d_init_, i_clamp_, dummy);
 
-    controller_state_publisher_.reset(new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>
+    controller_state_publisher_.reset(new sr_utilities::RealtimePublisher<control_msgs::JointControllerState>
                                               (node_, "state", 1));
 
     ROS_DEBUG(" --------- ");
diff --git a/sr_mechanism_controllers/src/srh_joint_velocity_controller.cpp b/sr_mechanism_controllers/src/srh_joint_velocity_controller.cpp
index 3d3e95a9f..cc357606b 100644
--- a/sr_mechanism_controllers/src/srh_joint_velocity_controller.cpp
+++ b/sr_mechanism_controllers/src/srh_joint_velocity_controller.cpp
@@ -80,7 +80,7 @@ namespace controller
       return false;
     }
 
-    controller_state_publisher_.reset(new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>
+    controller_state_publisher_.reset(new sr_utilities::RealtimePublisher<control_msgs::JointControllerState>
                                               (node_, "state", 1));
 
     ROS_DEBUG(" --------- ");
diff --git a/sr_utilities/include/sr_utilities/realtime_publisher.h b/sr_utilities/include/sr_utilities/realtime_publisher.h
new file mode 100644
index 000000000..34a776ebe
--- /dev/null
+++ b/sr_utilities/include/sr_utilities/realtime_publisher.h
@@ -0,0 +1,292 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+ /*
+* Copyright 2021 Shadow Robot Company Ltd.
+*
+* This program is free software: you can redistribute it and/or modify it
+* under the terms of the GNU General Public License as published by the Free
+* Software Foundation version 2 of the License.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License along
+* with this program. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*
+ * Publishing ROS messages is difficult, as the publish function is
+ * not realtime safe.  This class provides the proper locking so that
+ * you can call publish in realtime and a separate (non-realtime)
+ * thread will ensure that the message gets published over ROS.
+ *
+ * Author: Stuart Glaser
+ */
+#ifndef SR_UTILITIES__REALTIME_PUBLISHER_H_
+#define SR_UTILITIES__REALTIME_PUBLISHER_H_
+
+#include <ros/node_handle.h>
+
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <thread>
+#include <pthread.h>
+#include <sched.h>
+
+namespace sr_utilities {
+
+template <class Msg>
+class RealtimePublisher
+{
+
+public:
+  // TODO delete this field when all controllers have been adapted
+  Msg msg_;
+  /// The msg_buffer_ contains the msgs that will be inserted in the outgoing_msg_buffer_
+  boost::scoped_ptr<boost::circular_buffer<Msg> > msg_buffer_;
+
+  /**  \brief Constructor for the realtime publisher
+   *
+   * \param node the nodehandle that specifies the namespace (or prefix) that is used to advertise the ROS topic
+   * \param topic the topic name to advertise
+   * \param queue_size the size of the outgoing ROS buffer
+   * \param latched . optional argument (defaults to false) to specify is publisher is latched or not
+   */
+  RealtimePublisher(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
+    : topic_(topic), node_(node), is_running_(false), keep_running_(false), turn_(LOOP_NOT_STARTED)
+  {
+    construct(queue_size, latched);
+  }
+
+  RealtimePublisher()
+    : is_running_(false), keep_running_(false), turn_(LOOP_NOT_STARTED)
+  {
+  }
+
+  /// Destructor
+  ~RealtimePublisher()
+  {
+    stop();
+    while (is_running())
+    {
+      std::this_thread::sleep_for(std::chrono::microseconds(100));
+    }
+
+    if (thread_.joinable())
+    {
+      thread_.join();
+    }
+    publisher_.shutdown();
+  }
+
+  void init(const ros::NodeHandle &node, const std::string &topic, int queue_size, bool latched=false)
+  {
+    topic_ = topic;
+    node_ = node;
+    construct(queue_size, latched);
+  }
+
+  /// Stop the realtime publisher from sending out more ROS messages
+  void stop()
+  {
+    keep_running_ = false;
+#ifdef NON_POLLING
+    updated_cond_.notify_one();  // So the publishing loop can exit
+#endif
+  }
+
+  /**  \brief Try to get the data lock from realtime
+   *
+   * To publish data from the realtime loop, you need to run trylock to
+   * attempt to get unique access to the msg_ variable. Trylock returns
+   * true if the lock was aquired, and false if it failed to get the lock.
+   */
+  bool trylock()
+  {
+    if (msg_mutex_.try_lock())
+    {
+      if (turn_ == REALTIME)
+      {
+        return true;
+      }
+      else
+      {
+        msg_mutex_.unlock();
+        return false;
+      }
+    }
+    else
+    {
+      return false;
+    }
+  }
+
+  /**  \brief Unlock the msg_ variable
+   *
+   * After a successful trylock and after the data is written to the mgs_
+   * variable, the lock has to be released for the message to get
+   * published on the specified topic.
+   */
+  void unlockAndPublish()
+  {
+    turn_ = NON_REALTIME;
+    msg_mutex_.unlock();
+#ifdef NON_POLLING
+    updated_cond_.notify_one();
+#endif
+  }
+
+  /**  \brief Get the data lock form non-realtime
+   *
+   * To publish data from the realtime loop, you need to run trylock to
+   * attempt to get unique access to the msg_ variable. Trylock returns
+   * true if the lock was aquired, and false if it failed to get the lock.
+   */
+  void lock()
+  {
+#ifdef NON_POLLING
+    msg_mutex_.lock();
+#else
+    // never actually block on the lock
+    while (!msg_mutex_.try_lock())
+    {
+      std::this_thread::sleep_for(std::chrono::microseconds(200));
+    }
+#endif
+  }
+
+  /**  \brief Unlocks the data without publishing anything
+   *
+   */
+  void unlock()
+  {
+    msg_mutex_.unlock();
+  }
+
+private:
+  // non-copyable
+  RealtimePublisher(const RealtimePublisher &) = delete;
+  RealtimePublisher & operator=(const RealtimePublisher &) = delete;
+
+  void construct(int queue_size, bool latched=false)
+  {
+    publisher_ = node_.advertise<Msg>(topic_, queue_size, latched);
+    msg_buffer_.reset(new boost::circular_buffer<Msg>(queue_size));
+    outgoing_msg_buffer_.reset(new boost::circular_buffer<Msg>(queue_size));
+    keep_running_ = true;
+    thread_ = std::thread(&RealtimePublisher::publishingLoop, this);
+  }
+
+
+  bool is_running() const { return is_running_; }
+
+  void publishingLoop()
+  {
+    // Set to realtime scheduler for this thread
+    struct sched_param thread_param;
+    int policy = SCHED_FIFO;
+    thread_param.sched_priority = sched_get_priority_max(policy) - 10;
+    pthread_setschedparam(pthread_self(), policy, &thread_param);
+    
+    is_running_ = true;
+    turn_ = REALTIME;
+
+    while (keep_running_)
+    {
+      Msg outgoing;
+
+      // Locks msg_ and copies it
+      lock();
+      while (turn_ != NON_REALTIME && keep_running_)
+      {
+#ifdef NON_POLLING
+        updated_cond_.wait(lock);
+#else
+        unlock();
+        std::this_thread::sleep_for(std::chrono::microseconds(500));
+        lock();
+#endif
+      }
+
+      while(!msg_buffer_->empty())
+      {
+          outgoing_msg_buffer_->push_back(msg_buffer_->front());
+          msg_buffer_->pop_front();
+      }
+    //   outgoing = msg_;
+      turn_ = REALTIME;
+
+      unlock();
+
+      // Sends the outgoing message
+      if (keep_running_)
+      {
+        while(!outgoing_msg_buffer_->empty())
+        {
+            publisher_.publish(outgoing_msg_buffer_->front());
+            outgoing_msg_buffer_->pop_front();
+        }
+      }
+        // publisher_.publish(outgoing);
+    }
+    is_running_ = false;
+  }
+
+  std::string topic_;
+  ros::NodeHandle node_;
+  ros::Publisher publisher_;
+  std::atomic<bool> is_running_;
+  std::atomic<bool> keep_running_;
+
+  std::thread thread_;
+
+  std::mutex msg_mutex_;  // Protects msg_
+
+#ifdef NON_POLLING
+  std::condition_variable updated_cond_;
+#endif
+
+  enum {REALTIME, NON_REALTIME, LOOP_NOT_STARTED};
+  std::atomic<int> turn_;  // Who's turn is it to use msg_?
+  /// The msg_buffer_ contains the data that will get published on the ROS topic.
+  boost::scoped_ptr<boost::circular_buffer<Msg> > outgoing_msg_buffer_;
+};
+
+template <class Msg>
+using RealtimePublisherSharedPtr = std::shared_ptr<RealtimePublisher<Msg> >;
+
+}
+
+#endif
\ No newline at end of file