From e6f2b3bf1310763bfc3a0d01e76119ccf29072ec Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 25 Nov 2015 21:09:58 +0900 Subject: [PATCH] add joint_state_publisher.py from https://github.com/arebgun/dynamixel_motor/pull/27 --- dynamixel_7dof_arm/CMakeLists.txt | 2 + dynamixel_7dof_arm/setup.py | 11 ++ .../src/dynamixel_7dof_arm/__init__.py | 0 .../joint_state_publisher.py | 124 ++++++++++++++++++ 4 files changed, 137 insertions(+) create mode 100644 dynamixel_7dof_arm/setup.py create mode 100644 dynamixel_7dof_arm/src/dynamixel_7dof_arm/__init__.py create mode 100644 dynamixel_7dof_arm/src/dynamixel_7dof_arm/joint_state_publisher.py diff --git a/dynamixel_7dof_arm/CMakeLists.txt b/dynamixel_7dof_arm/CMakeLists.txt index d7b70e4b..302981b2 100644 --- a/dynamixel_7dof_arm/CMakeLists.txt +++ b/dynamixel_7dof_arm/CMakeLists.txt @@ -3,6 +3,8 @@ project(dynamixel_7dof_arm) find_package(catkin REQUIRED COMPONENTS dynamixel_controllers roseus rostest) # add roseus to gen messages +catkin_python_setup() + catkin_package() add_rostest(test/test-dxl-7dof-arm.test) \ No newline at end of file diff --git a/dynamixel_7dof_arm/setup.py b/dynamixel_7dof_arm/setup.py new file mode 100644 index 00000000..7cbfe647 --- /dev/null +++ b/dynamixel_7dof_arm/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['dynamixel_7dof_arm'], + package_dir={'': 'src'}, + ) + +setup(**d) diff --git a/dynamixel_7dof_arm/src/dynamixel_7dof_arm/__init__.py b/dynamixel_7dof_arm/src/dynamixel_7dof_arm/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/dynamixel_7dof_arm/src/dynamixel_7dof_arm/joint_state_publisher.py b/dynamixel_7dof_arm/src/dynamixel_7dof_arm/joint_state_publisher.py new file mode 100644 index 00000000..d3df7812 --- /dev/null +++ b/dynamixel_7dof_arm/src/dynamixel_7dof_arm/joint_state_publisher.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +##### +##### +# This script is copied from https://github.com/arebgun/dynamixel_motor/pull/27 +##### +##### +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2015, Kei Okada. +# 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 Kei Okada 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. + +from threading import Thread + +import rospy + +from sensor_msgs.msg import JointState + +class JointStatePublisher(): + def __init__(self, controller_namespace, controllers): + self.update_rate = 1000 + self.state_update_rate = 50 + self.trajectory = [] + + self.controller_namespace = controller_namespace + self.joint_names = [c.joint_name for c in controllers] + + self.joint_to_controller = {} + for c in controllers: + self.joint_to_controller[c.joint_name] = c + + self.port_to_joints = {} + for c in controllers: + if c.port_namespace not in self.port_to_joints: self.port_to_joints[c.port_namespace] = [] + self.port_to_joints[c.port_namespace].append(c.joint_name) + + self.port_to_io = {} + for c in controllers: + if c.port_namespace in self.port_to_io: continue + self.port_to_io[c.port_namespace] = c.dxl_io + + def initialize(self): + self.msg = JointState() + return True + + + def start(self): + self.running = True + self.state_pub = rospy.Publisher('joint_states', JointState , queue_size=100) + + Thread(target=self.update_state).start() + + + def stop(self): + self.running = False + + def update_state(self): + rate = rospy.Rate(self.state_update_rate) + while self.running and not rospy.is_shutdown(): + self.msg.header.stamp = rospy.Time.now() + self.msg.name = [] + self.msg.position = [] + self.msg.velocity = [] + self.msg.effort = [] + + for port, joints in self.port_to_joints.items(): + vals = [] + rospy.logdebug("joints : "+" ".join(joints)) + for joint in joints: + j = self.joint_names.index(joint) + + motor_id = self.joint_to_controller[joint].motor_id + co = self.joint_to_controller[joint] + io = self.port_to_io[port] + rospy.logdebug("port={} id={}, {}".format(port, motor_id, joint)) + + self.msg.name.append(joint) + po = ve = ef = 0 + try: + ret = io.get_feedback(motor_id) + po = self.raw_to_rad_pos(ret['position'],co) + ve = self.raw_to_rad_spd(ret['speed'],co) + ef = self.raw_to_rad_spd(ret['load'],co) + except Exception as e: + rospy.logerr(e) + self.msg.position.append(po) + self.msg.velocity.append(ve) + self.msg.effort.append(ef) + + self.state_pub.publish(self.msg) + rate.sleep() + + def raw_to_rad_pos(self, raw, c): + return (c.initial_position_raw - raw if c.flipped else raw - c.initial_position_raw) * c.RADIANS_PER_ENCODER_TICK + def raw_to_rad_spd(self, raw, c): + return (- raw if c.flipped else raw ) * c.RADIANS_PER_ENCODER_TICK