From e23705ebf19cd8b4ef842da962f4682d676d45aa Mon Sep 17 00:00:00 2001 From: Wim Meeussen Date: Sun, 23 Dec 2012 11:44:44 -0800 Subject: [PATCH] first simple controller for testing --- effort_controllers/CMakeLists.txt | 30 +++++++ effort_controllers/Makefile | 1 + .../effort_controllers_plugins.xml | 8 ++ .../joint_effort_controller.h | 88 +++++++++++++++++++ effort_controllers/mainpage.dox | 14 +++ effort_controllers/manifest.xml | 23 +++++ .../src/joint_effort_controller.cpp | 83 +++++++++++++++++ 7 files changed, 247 insertions(+) create mode 100644 effort_controllers/CMakeLists.txt create mode 100644 effort_controllers/Makefile create mode 100644 effort_controllers/effort_controllers_plugins.xml create mode 100644 effort_controllers/include/effort_controllers/joint_effort_controller.h create mode 100644 effort_controllers/mainpage.dox create mode 100644 effort_controllers/manifest.xml create mode 100644 effort_controllers/src/joint_effort_controller.cpp diff --git a/effort_controllers/CMakeLists.txt b/effort_controllers/CMakeLists.txt new file mode 100644 index 000000000..e1f5a1d55 --- /dev/null +++ b/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}) diff --git a/effort_controllers/Makefile b/effort_controllers/Makefile new file mode 100644 index 000000000..b75b928f2 --- /dev/null +++ b/effort_controllers/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/effort_controllers/effort_controllers_plugins.xml b/effort_controllers/effort_controllers_plugins.xml new file mode 100644 index 000000000..3ca713e8d --- /dev/null +++ b/effort_controllers/effort_controllers_plugins.xml @@ -0,0 +1,8 @@ + + + + The JointPositionController tracks position commands. It expects a JointEffortCommandInterface type of hardware interface. + + + + diff --git a/effort_controllers/include/effort_controllers/joint_effort_controller.h b/effort_controllers/include/effort_controllers/joint_effort_controller.h new file mode 100644 index 000000000..75f2276bb --- /dev/null +++ b/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 +#include +#include +#include +#include + + +namespace effort_controllers +{ + +class JointEffortController: public controller_interface::Controller +{ +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 diff --git a/effort_controllers/mainpage.dox b/effort_controllers/mainpage.dox new file mode 100644 index 000000000..2b3a59f47 --- /dev/null +++ b/effort_controllers/mainpage.dox @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b effort_controllers + + + +--> + + +*/ diff --git a/effort_controllers/manifest.xml b/effort_controllers/manifest.xml new file mode 100644 index 000000000..7fa01c74e --- /dev/null +++ b/effort_controllers/manifest.xml @@ -0,0 +1,23 @@ + + + + effort_controllers + + + Vijay Pradeep + BSD + + http://ros.org/wiki/effort_controllers + + + + + + + + + + + + + diff --git a/effort_controllers/src/joint_effort_controller.cpp b/effort_controllers/src/joint_effort_controller.cpp new file mode 100644 index 000000000..5ee63c582 --- /dev/null +++ b/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 +#include + + +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("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) +