F fix j0 pos vel filter

sr_hardware_interface/include/sr_hardware_interface/sr_actuator.hpp

@@ -114,6 +114,8 @@ class SrActuatorState
 
   std::vector<int> raw_sensor_values_;
   std::vector<double> calibrated_sensor_values_;
+  std::vector<double> filtered_calibrated_position_values_;
+  std::vector<double> filtered_calibrated_velocity_values_;
 
   /**
    * a vector containing human readable flags:

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

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>

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

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>
@@ -88,6 +89,7 @@ class SrController :
   bool has_j2;
   /**< true if this is a joint 0. */
   double command_;                                /**< Last commanded position. */
+  double last_commanded_effort, last_error_position;
 
 protected:
   // true if this is joint 0
@@ -145,8 +147,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;
 

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)