From be90e07acae670b88749a4cd3f61b85f75886063 Mon Sep 17 00:00:00 2001 From: Wim Meeussen Date: Sun, 23 Dec 2012 11:16:29 -0800 Subject: [PATCH] moved package with joint state controller --- joint_state_controller/CMakeLists.txt | 20 ++++ joint_state_controller/Makefile | 1 + .../joint_state_controller.h | 61 +++++++++++ .../joint_state_controller.launch | 9 ++ .../joint_state_controller.yaml | 3 + joint_state_controller/joint_state_plugin.xml | 7 ++ joint_state_controller/manifest.xml | 21 ++++ .../src/joint_state_controller.cpp | 103 ++++++++++++++++++ 8 files changed, 225 insertions(+) create mode 100644 joint_state_controller/CMakeLists.txt create mode 100644 joint_state_controller/Makefile create mode 100644 joint_state_controller/include/joint_state_controller/joint_state_controller.h create mode 100644 joint_state_controller/joint_state_controller.launch create mode 100644 joint_state_controller/joint_state_controller.yaml create mode 100644 joint_state_controller/joint_state_plugin.xml create mode 100644 joint_state_controller/manifest.xml create mode 100644 joint_state_controller/src/joint_state_controller.cpp diff --git a/joint_state_controller/CMakeLists.txt b/joint_state_controller/CMakeLists.txt new file mode 100644 index 000000000..dd12b2737 --- /dev/null +++ b/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}) diff --git a/joint_state_controller/Makefile b/joint_state_controller/Makefile new file mode 100644 index 000000000..b75b928f2 --- /dev/null +++ b/joint_state_controller/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/joint_state_controller/include/joint_state_controller/joint_state_controller.h b/joint_state_controller/include/joint_state_controller/joint_state_controller.h new file mode 100644 index 000000000..202cba19c --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +namespace joint_state_controller +{ + +// this controller gets access to the JointStateInterface +class JointStateController: public controller_interface::Controller +{ +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 joint_state_; + boost::shared_ptr > realtime_pub_; + ros::Time last_publish_time_; + double publish_rate_; +}; + +} diff --git a/joint_state_controller/joint_state_controller.launch b/joint_state_controller/joint_state_controller.launch new file mode 100644 index 000000000..e0fb3b4fa --- /dev/null +++ b/joint_state_controller/joint_state_controller.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/joint_state_controller/joint_state_controller.yaml b/joint_state_controller/joint_state_controller.yaml new file mode 100644 index 000000000..d9fe32c05 --- /dev/null +++ b/joint_state_controller/joint_state_controller.yaml @@ -0,0 +1,3 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 \ No newline at end of file diff --git a/joint_state_controller/joint_state_plugin.xml b/joint_state_controller/joint_state_plugin.xml new file mode 100644 index 000000000..8276a91f4 --- /dev/null +++ b/joint_state_controller/joint_state_plugin.xml @@ -0,0 +1,7 @@ + + + + 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. + + + \ No newline at end of file diff --git a/joint_state_controller/manifest.xml b/joint_state_controller/manifest.xml new file mode 100644 index 000000000..93b99caa1 --- /dev/null +++ b/joint_state_controller/manifest.xml @@ -0,0 +1,21 @@ + + + Controller to publish joint state + + Wim Meeussen + BSD + + https://bitbucket.org/hidof/controller-manager/wiki/ + + + + + + + + + + + + + diff --git a/joint_state_controller/src/joint_state_controller.cpp b/joint_state_controller/src/joint_state_controller.cpp new file mode 100644 index 000000000..521116381 --- /dev/null +++ b/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& joint_names = hw->getJointNames(); + for (unsigned i=0; i(nh, "joint_states", 4)); + + // get joints and allocate message + for (unsigned i=0; igetJointStateHandle(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; imsg_.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)