first simple controller for testing

effort_controllers/CMakeLists.txt

@@ -0,0 +1,30 @@
+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 the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+#rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+rosbuild_add_library(${PROJECT_NAME} src/joint_effort_controller.cpp include/effort_controllers/joint_effort_controller.h)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})

effort_controllers/Makefile

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

effort_controllers/effort_controllers_plugins.xml

@@ -0,0 +1,8 @@
+<library path="lib/libeffort_controllers">
+  <class name="effort_controllers/JointPositionController" type="effort_controllers::JointPositionController" base_class_type="controller_interface::ControllerBase">
+  <description>
+    The JointPositionController tracks position commands. It expects a JointEffortCommandInterface type of hardware interface.
+  </description>
+  </class>
+
+</library>

effort_controllers/include/effort_controllers/joint_effort_controller.h

@@ -0,0 +1,88 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  Copyright (c) 2012, hiDOF, 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 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.
+ *********************************************************************/
+
+#ifndef EFFORT_CONTROLLERS_JOINT_EFFORT_CONTROLLER_H
+#define EFFORT_CONTROLLERS_JOINT_EFFORT_CONTROLLER_H
+
+/**
+   @class effort_controllers:JointEffortController
+   @brief Joint Effort Controller (torque or force)
+
+   This class passes the commanded effort down to the joint
+
+   @section ROS interface
+
+   @param type Must be "JointEffortController"
+   @param joint Name of the joint to control.
+
+   Subscribes to:
+   - @b command (std_msgs::Float64) : The joint effort to apply
+*/
+
+#include <boost/thread/condition.hpp>
+#include <ros/node_handle.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <std_msgs/Float64.h>
+
+
+namespace effort_controllers
+{
+
+class JointEffortController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+{
+public:
+
+  JointEffortController();
+  ~JointEffortController();
+
+  bool init(hardware_interface::EffortJointInterface* robot, const std::string &joint_name);
+  bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
+
+  void starting(const ros::Time& time) { command_ = 0.0;}
+  void update(const ros::Time& time);
+
+  hardware_interface::JointHandle joint_;
+  double command_;
+
+private:
+  ros::Subscriber sub_command_;
+  void commandCB(const std_msgs::Float64ConstPtr& msg);
+
+};
+
+}
+
+#endif

effort_controllers/mainpage.dox

@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b effort_controllers 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/

effort_controllers/manifest.xml

@@ -0,0 +1,23 @@
+<package>
+  <description brief="effort_controllers">
+
+     effort_controllers
+
+  </description>
+  <author>Vijay Pradeep</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/effort_controllers</url>
+
+  <depend package="controller_interface"/>
+  <depend package="control_toolbox" />
+  <depend package="realtime_tools" />
+  <depend package="pr2_controllers_msgs" />
+
+  <export>
+    <controller_interface plugin="${prefix}/effort_controllers_plugin.xml" />
+  </export>
+
+</package>
+
+

effort_controllers/src/joint_effort_controller.cpp

@@ -0,0 +1,83 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  Copyright (c) 2012, hiDOF, 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 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.
+ *********************************************************************/
+
+#include <effort_controllers/joint_effort_controller.h>
+#include <pluginlib/class_list_macros.h>
+
+
+namespace effort_controllers {
+
+JointEffortController::JointEffortController()
+: command_(0)
+{}
+
+JointEffortController::~JointEffortController()
+{
+  sub_command_.shutdown();
+}
+
+bool JointEffortController::init(hardware_interface::EffortJointInterface *robot, const std::string &joint_name)
+{
+  joint_ = robot->getJointHandle(joint_name);
+  return true;
+}
+
+
+bool JointEffortController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
+{
+  std::string joint_name;
+  if (!n.getParam("joint", joint_name))
+  {
+    ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
+    return false;
+  }
+  joint_ = robot->getJointHandle(joint_name);
+  sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointEffortController::commandCB, this);
+  return true;
+}
+
+void JointEffortController::update(const ros::Time& time)
+{
+  joint_.setCommand(command_);
+}
+
+void JointEffortController::commandCB(const std_msgs::Float64ConstPtr& msg)
+{
+  command_ = msg->data;
+}
+}// namespace
+
+PLUGINLIB_DECLARE_CLASS(effort_controllers, JointEffortController, effort_controllers::JointEffortController, controller_interface::ControllerBase)
+