moved package with joint state controller

joint_state_controller/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+rosbuild_add_library(${PROJECT_NAME} src/joint_state_controller.cpp)
+
+#rosbuild_add_executable(dummy_controller src/dummy.cpp)
+#target_link_libraries(dummy_controller ${PROJECT_NAME})

joint_state_controller/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

joint_state_controller/include/joint_state_controller/joint_state_controller.h

@@ -0,0 +1,61 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 <controller_interface/controller.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <pluginlib/class_list_macros.h>
+#include <sensor_msgs/JointState.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <boost/shared_ptr.hpp>
+
+namespace joint_state_controller
+{
+
+// this controller gets access to the JointStateInterface
+class JointStateController: public controller_interface::Controller<hardware_interface::JointStateInterface>
+{
+public:
+  JointStateController(){}
+
+  virtual bool init(hardware_interface::JointStateInterface* hw, ros::NodeHandle &n);
+  virtual void starting(const ros::Time& time);
+  virtual void update(const ros::Time& time);
+  virtual void stopping(const ros::Time& time);
+
+private:
+  std::vector<hardware_interface::JointStateHandle> joint_state_;
+  boost::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::JointState> > realtime_pub_;
+  ros::Time last_publish_time_;
+  double publish_rate_;
+};
+
+}

joint_state_controller/joint_state_controller.launch

@@ -0,0 +1,9 @@
+<launch>
+  <!-- load configuration -->
+  <rosparam command="load" file="$(find joint_state_controller)/joint_state_controller.yaml" />
+
+  <!-- spawn controller -->
+  <node name="joint_state_controller_spawner" pkg="controller_manager" type="spawner" output="screen" args="joint_state_controller" />
+
+
+</launch>

joint_state_controller/joint_state_controller.yaml

@@ -0,0 +1,3 @@
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50

joint_state_controller/joint_state_plugin.xml

@@ -0,0 +1,7 @@
+<library path="lib/libjoint_state_controller">
+  <class name="joint_state_controller/JointStateController" type="joint_state_controller::JointStateController" base_class_type="controller_interface::ControllerBase">
+  <description>
+    The joint state controller is publishes the current joint state as a sensor_msgs/JointState message. The joint state controller expects a JointStateInterface type of hardware interface.
+  </description>
+  </class>
+</library>

joint_state_controller/manifest.xml

@@ -0,0 +1,21 @@
+<package>
+  <description>
+    Controller to publish joint state
+  </description>
+  <author>Wim Meeussen</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>https://bitbucket.org/hidof/controller-manager/wiki/</url>
+
+  <depend package="realtime_tools"/>
+  <depend package="roscpp"/>
+  <depend package="hardware_interface"/>
+  <depend package="pluginlib"/>
+  <depend package="controller_interface"/>
+  <depend package="sensor_msgs"/>
+
+  <export>
+    <controller_interface plugin="${prefix}/joint_state_plugin.xml" />
+  </export>
+
+</package>

joint_state_controller/src/joint_state_controller.cpp

@@ -0,0 +1,103 @@
+///////////////////////////////////////////////////////////////////////////////
+// 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 "joint_state_controller/joint_state_controller.h"
+
+
+
+namespace joint_state_controller
+{
+
+  bool JointStateController::init(hardware_interface::JointStateInterface* hw, ros::NodeHandle &n)
+  {
+    // get all joint states from the hardware interface
+    const std::vector<std::string>& joint_names = hw->getJointNames();
+    for (unsigned i=0; i<joint_names.size(); i++)
+      ROS_DEBUG("Got joint %s", joint_names[i].c_str());
+
+    // get publishing period
+    if (!n.getParam("publish_rate", publish_rate_)){
+      ROS_ERROR("Parameter 'publish_rate' not set");
+      return false;
+    }
+
+    // realtime publisher
+    ros::NodeHandle nh;
+    realtime_pub_.reset(new realtime_tools::RealtimePublisher<sensor_msgs::JointState>(nh, "joint_states", 4));
+
+    // get joints and allocate message
+    for (unsigned i=0; i<joint_names.size(); i++){
+      joint_state_.push_back(hw->getJointStateHandle(joint_names[i]));
+      realtime_pub_->msg_.name.push_back(joint_names[i]);
+      realtime_pub_->msg_.position.push_back(0.0);
+      realtime_pub_->msg_.velocity.push_back(0.0);
+      realtime_pub_->msg_.effort.push_back(0.0);
+    }    
+
+    return true;
+  }
+
+  void JointStateController::starting(const ros::Time& time)
+  {
+    // initialize time
+    last_publish_time_ = time;
+  }
+
+  void JointStateController::update(const ros::Time& time)
+  {
+    // limit rate of publishing
+    if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0/publish_rate_) < time){
+
+      // 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_);
+
+	// populate joint state message
+	realtime_pub_->msg_.header.stamp = time;
+	for (unsigned i=0; i<joint_state_.size(); i++){
+	  realtime_pub_->msg_.position[i] = joint_state_[i].getPosition();
+	  realtime_pub_->msg_.velocity[i] = joint_state_[i].getVelocity();
+	  realtime_pub_->msg_.effort[i] = joint_state_[i].getEffort();
+	}
+	realtime_pub_->unlockAndPublish();
+      }
+    }
+  }
+
+  void JointStateController::stopping(const ros::Time& time)
+  {}
+
+}
+
+
+PLUGINLIB_DECLARE_CLASS(joint_state_controller, JointStateController, joint_state_controller::JointStateController, controller_interface::ControllerBase)