From 8ac9860b075a8813fe286a73e44c657d4678ae80 Mon Sep 17 00:00:00 2001 From: Tokyo Opensource Robotics Programmer 534o <534o@opensource-robotics.tokyo.jp> Date: Thu, 2 Jul 2015 04:09:02 -0700 Subject: [PATCH] add control manager for dummy/joint_state_publisher option --- spur_bringup/launch/spur_dynamixel.launch | 2 +- spur_controller/CMakeLists.txt | 2 + .../config/spur_controller_configuration.yaml | 2 +- spur_controller/nodes/controller_manager.py | 388 ++++++++++++++++++ spur_controller/setup.py | 11 + .../src/spur_controller/__init__.py | 0 .../spur_controller/joint_state_publisher.py | 112 +++++ 7 files changed, 515 insertions(+), 2 deletions(-) create mode 100755 spur_controller/nodes/controller_manager.py create mode 100644 spur_controller/setup.py create mode 100644 spur_controller/src/spur_controller/__init__.py create mode 100755 spur_controller/src/spur_controller/joint_state_publisher.py diff --git a/spur_bringup/launch/spur_dynamixel.launch b/spur_bringup/launch/spur_dynamixel.launch index fe0f75b..a0a6df5 100644 --- a/spur_bringup/launch/spur_dynamixel.launch +++ b/spur_bringup/launch/spur_dynamixel.launch @@ -11,7 +11,7 @@ - diff --git a/spur_controller/CMakeLists.txt b/spur_controller/CMakeLists.txt index ca0aeff..8504c8e 100644 --- a/spur_controller/CMakeLists.txt +++ b/spur_controller/CMakeLists.txt @@ -3,6 +3,8 @@ project(spur_controller) find_package(catkin REQUIRED) +catkin_python_setup() + catkin_package() install(DIRECTORY launch nodes config diff --git a/spur_controller/config/spur_controller_configuration.yaml b/spur_controller/config/spur_controller_configuration.yaml index 3a6d4a9..af45585 100644 --- a/spur_controller/config/spur_controller_configuration.yaml +++ b/spur_controller/config/spur_controller_configuration.yaml @@ -4,7 +4,7 @@ joint_state_publisher: controller: - package: dynamixel_controllers + package: spur_controller module: joint_state_publisher type: JointStatePublisher diff --git a/spur_controller/nodes/controller_manager.py b/spur_controller/nodes/controller_manager.py new file mode 100755 index 0000000..f3ccca0 --- /dev/null +++ b/spur_controller/nodes/controller_manager.py @@ -0,0 +1,388 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +##### +##### +# This script is copied from https://github.com/arebgun/dynamixel_motor/pull/28 +##### +##### +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2010, Antons Rebguns, Cody Jorgensen, Cara Slutter +# 2010-2011, Antons Rebguns +# 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 University of Arizona 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__ = 'Antons Rebguns, Cody Jorgensen, Cara Slutter' +__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns, Cody Jorgensen, Cara Slutter' + +__license__ = 'BSD' +__maintainer__ = 'Antons Rebguns' +__email__ = 'anton@email.arizona.edu' + + +from threading import Thread, Lock + +import sys +from optparse import OptionParser + +import rospy + +#from dynamixel_driver.dynamixel_serial_proxy import SerialProxy +#from dynamixel_driver.dynamixel_dummy_proxy import DummyProxy + +from threading import Thread, Lock + +from dynamixel_driver.dynamixel_serial_proxy import SerialProxy + +import rospy + +class DummyDynamixelIO(object): + """ Provides low level IO with the Dynamixel servos through pyserial. Has the + ability to write instruction packets, request and read register value + packets, send and receive a response to a ping packet, and send a SYNC WRITE + multi-servo instruction packet. + """ + + def __init__(self, port, baudrate, readback_echo=False): + """ Constructor takes serial port and baudrate as arguments. """ + self.serial_mutex = Lock() + self.port_name = port + self.readback_echo = readback_echo + + self.goal = {} + self.position = {} + self.speed = {} + + def ping(self, servo_id): + self.goal[servo_id] = 2048 + self.position[servo_id] = 2048 + self.speed[servo_id] = 0 + return True; + + def get_model_number(self, servo_id): + """ Reads the servo's model number (e.g. 12 for AX-12+). """ + return 12 + + def get_angle_limits(self, servo_id): + cwLimit = 0 + ccwLimit = 4096 + return {'min':cwLimit, 'max':ccwLimit} + + def get_voltage(self, servo_id): + return 12 + + def get_voltage_limits(self, servo_id): + # return the data in a dictionary + min_voltage = 8 + max_voltage = 12 + return {'min':min_voltage, 'max':max_voltage} + + def get_position(self, servo_id): + return self.position[servo_id] + + def get_speed(self, servo_id): + return self.speed[servo_id] + + def get_current(self, servo_id): + return 0 + + def get_firmware_version(self, servo_id): + return None + + def get_return_delay_time(self, servo_id): + """ Reads the servo's return delay time. """ + return 5 + + def get_feedback(self, servo_id): + """ + Returns the id, goal, position, error, speed, load, voltage, temperature + and moving values from the specified servo. + """ + + error = 0 + if self.speed[servo_id] != 0 : + speed = self.speed[servo_id] + self.goal[servo_id] += speed + else: + speed = (self.goal[servo_id] - self.position[servo_id])*0.1 + self.position[servo_id] += speed + return { 'timestamp': rospy.Time.now().to_sec(), + 'id': servo_id, + 'goal': self.goal[servo_id], + 'position': self.position[servo_id], + 'error': error, + 'speed': speed} + + def set_multi_speed(self, valueTuples): + for servo_id,val in valueTuples: + self.speed[servo_id] = val + return True + + def set_multi_position(self, valueTuples): + for servo_id,val in valueTuples: + self.goal[servo_id] = val + self.speed[servo_id] = 0 + return True + + +class DummyProxy(SerialProxy): + def connect(self): + self.dxl_io = DummyDynamixelIO(self.port_name, self.baud_rate) # self.readback_echo: release deb version does not have readback_echo + self._SerialProxy__find_motors() + self.running = True + if self.update_rate > 0: Thread(target=self._SerialProxy__update_motor_states).start() + if self.diagnostics_rate > 0: Thread(target=self._SerialProxy__publish_diagnostic_information).start() +## + + +from diagnostic_msgs.msg import DiagnosticArray +from diagnostic_msgs.msg import DiagnosticStatus +from diagnostic_msgs.msg import KeyValue + +from dynamixel_controllers.srv import StartController +from dynamixel_controllers.srv import StartControllerResponse +from dynamixel_controllers.srv import StopController +from dynamixel_controllers.srv import StopControllerResponse +from dynamixel_controllers.srv import RestartController +from dynamixel_controllers.srv import RestartControllerResponse + +class ControllerManager: + def __init__(self): + rospy.init_node('dynamixel_controller_manager', anonymous=True) + rospy.on_shutdown(self.on_shutdown) + + self.waiting_meta_controllers = [] + self.controllers = {} + self.serial_proxies = {} + self.diagnostics_rate = rospy.get_param('~diagnostics_rate', 1) + + self.start_controller_lock = Lock() + self.stop_controller_lock = Lock() + + manager_namespace = rospy.get_param('~namespace') + serial_ports = rospy.get_param('~serial_ports') + + for port_namespace,port_config in serial_ports.items(): + port_name = port_config['port_name'] + baud_rate = port_config['baud_rate'] + readback_echo = port_config['readback_echo'] if 'readback_echo' in port_config else False + min_motor_id = port_config['min_motor_id'] if 'min_motor_id' in port_config else 0 + max_motor_id = port_config['max_motor_id'] if 'max_motor_id' in port_config else 253 + update_rate = port_config['update_rate'] if 'update_rate' in port_config else 5 + error_level_temp = 75 + warn_level_temp = 70 + + if 'diagnostics' in port_config: + if 'error_level_temp' in port_config['diagnostics']: + error_level_temp = port_config['diagnostics']['error_level_temp'] + if 'warn_level_temp' in port_config['diagnostics']: + warn_level_temp = port_config['diagnostics']['warn_level_temp'] + + serial_proxy = SerialProxy(port_name=port_name, + port_namespace=port_namespace, + baud_rate=baud_rate, + min_motor_id=min_motor_id, + max_motor_id=max_motor_id, + update_rate=update_rate, + diagnostics_rate=self.diagnostics_rate, + error_level_temp=error_level_temp, + warn_level_temp=warn_level_temp) + # readback_echo=readback_echo ## this will not work on deb version + serial_proxy.connect() + + # will create a set of services for each serial port under common manager namesapce + # e.g. /dynamixel_manager/robot_arm_port/start_controller + # /dynamixel_manager/robot_head_port/start_controller + # where 'dynamixel_manager' is manager's namespace + # 'robot_arm_port' and 'robot_head_port' are human readable names for serial ports + rospy.Service('%s/%s/start_controller' % (manager_namespace, port_namespace), StartController, self.start_controller) + rospy.Service('%s/%s/stop_controller' % (manager_namespace, port_namespace), StopController, self.stop_controller) + rospy.Service('%s/%s/restart_controller' % (manager_namespace, port_namespace), RestartController, self.restart_controller) + + self.serial_proxies[port_namespace] = serial_proxy + + # services for 'meta' controllers, e.g. joint trajectory controller + # these controllers don't have their own serial port, instead they rely + # on regular controllers for serial connection. The advantage of meta + # controller is that it can pack commands for multiple motors on multiple + # serial ports. + # NOTE: all serial ports that meta controller needs should be managed by + # the same controler manager. + rospy.Service('%s/meta/start_controller' % manager_namespace, StartController, self.start_controller) + rospy.Service('%s/meta/stop_controller' % manager_namespace, StopController, self.stop_controller) + rospy.Service('%s/meta/restart_controller' % manager_namespace, RestartController, self.restart_controller) + + self.diagnostics_pub = rospy.Publisher('/diagnostics', DiagnosticArray) + if self.diagnostics_rate > 0: Thread(target=self.diagnostics_processor).start() + + def on_shutdown(self): + for serial_proxy in self.serial_proxies.values(): + serial_proxy.disconnect() + + def diagnostics_processor(self): + diag_msg = DiagnosticArray() + + rate = rospy.Rate(self.diagnostics_rate) + while not rospy.is_shutdown(): + diag_msg.status = [] + diag_msg.header.stamp = rospy.Time.now() + + for controller in self.controllers.values(): + try: + joint_state = controller.joint_state + temps = joint_state.motor_temps + max_temp = max(temps) + + status = DiagnosticStatus() + status.name = 'Joint Controller (%s)' % controller.joint_name + status.hardware_id = 'Robotis Dynamixel %s on port %s' % (str(joint_state.motor_ids), controller.port_namespace) + status.values.append(KeyValue('Goal', str(joint_state.goal_pos))) + status.values.append(KeyValue('Position', str(joint_state.current_pos))) + status.values.append(KeyValue('Error', str(joint_state.error))) + status.values.append(KeyValue('Velocity', str(joint_state.velocity))) + status.values.append(KeyValue('Load', str(joint_state.load))) + status.values.append(KeyValue('Moving', str(joint_state.is_moving))) + status.values.append(KeyValue('Temperature', str(max_temp))) + status.level = DiagnosticStatus.OK + status.message = 'OK' + + diag_msg.status.append(status) + except: + pass + + self.diagnostics_pub.publish(diag_msg) + rate.sleep() + + def check_deps(self): + controllers_still_waiting = [] + + for i,(controller_name,deps,kls) in enumerate(self.waiting_meta_controllers): + if not set(deps).issubset(self.controllers.keys()): + controllers_still_waiting.append(self.waiting_meta_controllers[i]) + rospy.logwarn('[%s] not all dependencies started, still waiting for %s...' % (controller_name, str(list(set(deps).difference(self.controllers.keys()))))) + else: + dependencies = [self.controllers[dep_name] for dep_name in deps] + controller = kls(controller_name, dependencies) + + if controller.initialize(): + controller.start() + self.controllers[controller_name] = controller + + self.waiting_meta_controllers = controllers_still_waiting[:] + + def start_controller(self, req): + port_name = req.port_name + package_path = req.package_path + module_name = req.module_name + class_name = req.class_name + controller_name = req.controller_name + + self.start_controller_lock.acquire() + + if controller_name in self.controllers: + self.start_controller_lock.release() + return StartControllerResponse(False, 'Controller [%s] already started. If you want to restart it, call restart.' % controller_name) + + try: + if module_name not in sys.modules: + # import if module not previously imported + package_module = __import__(package_path, globals(), locals(), [module_name], -1) + else: + # reload module if previously imported + package_module = reload(sys.modules[package_path]) + controller_module = getattr(package_module, module_name) + except ImportError, ie: + self.start_controller_lock.release() + return StartControllerResponse(False, 'Cannot find controller module. Unable to start controller %s\n%s' % (module_name, str(ie))) + except SyntaxError, se: + self.start_controller_lock.release() + return StartControllerResponse(False, 'Syntax error in controller module. Unable to start controller %s\n%s' % (module_name, str(se))) + except Exception, e: + self.start_controller_lock.release() + return StartControllerResponse(False, 'Unknown error has occured. Unable to start controller %s\n%s' % (module_name, str(e))) + + kls = getattr(controller_module, class_name) + + if port_name == 'meta': + self.waiting_meta_controllers.append((controller_name,req.dependencies,kls)) + self.check_deps() + self.start_controller_lock.release() + return StartControllerResponse(True, '') + + if port_name != 'meta' and (port_name not in self.serial_proxies): + self.start_controller_lock.release() + return StartControllerResponse(False, 'Specified port [%s] not found, available ports are %s. Unable to start controller %s' % (port_name, str(self.serial_proxies.keys()), controller_name)) + + controller = kls(self.serial_proxies[port_name].dxl_io, controller_name, port_name) + + if controller.initialize(): + controller.start() + self.controllers[controller_name] = controller + + self.check_deps() + self.start_controller_lock.release() + + return StartControllerResponse(True, 'Controller %s successfully started.' % controller_name) + else: + self.start_controller_lock.release() + return StartControllerResponse(False, 'Initialization failed. Unable to start controller %s' % controller_name) + + def stop_controller(self, req): + controller_name = req.controller_name + self.stop_controller_lock.acquire() + + if controller_name in self.controllers: + self.controllers[controller_name].stop() + del self.controllers[controller_name] + self.stop_controller_lock.release() + return StopControllerResponse(True, 'controller %s successfully stopped.' % controller_name) + else: + self.self.stop_controller_lock.release() + return StopControllerResponse(False, 'controller %s was not running.' % controller_name) + + def restart_controller(self, req): + response1 = self.stop_controller(StopController(req.controller_name)) + response2 = self.start_controller(req) + return RestartControllerResponse(response1.success and response2.success, '%s\n%s' % (response1.reason, response2.reason)) + +if __name__ == '__main__': + try: + usage_msg = 'Usage: %prog [options]' + parser = OptionParser(usage=usage_msg) + parser.add_option('-d', '--dummy', default=False, + help='running manager in dummy mode, which did not connect to actual haradware device') + (options, args) = parser.parse_args(rospy.myargv()[1:]) + if options.dummy: + SerialProxy = DummyProxy + manager = ControllerManager() + rospy.spin() + except rospy.ROSInterruptException: pass + diff --git a/spur_controller/setup.py b/spur_controller/setup.py new file mode 100644 index 0000000..ccfc659 --- /dev/null +++ b/spur_controller/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=['spur_controller'], + package_dir={'': 'src'}, + ) + +setup(**d) diff --git a/spur_controller/src/spur_controller/__init__.py b/spur_controller/src/spur_controller/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/spur_controller/src/spur_controller/joint_state_publisher.py b/spur_controller/src/spur_controller/joint_state_publisher.py new file mode 100755 index 0000000..a884df7 --- /dev/null +++ b/spur_controller/src/spur_controller/joint_state_publisher.py @@ -0,0 +1,112 @@ +#!/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 = [] + 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] + + self.msg.name.append(joint) + self.msg.position.append(self.raw_to_rad(io.get_position(motor_id),co)) + self.msg.velocity.append(self.raw_to_rad(io.get_speed(motor_id),co)) + self.msg.effort.append(io.get_current(motor_id)) + + self.state_pub.publish(self.msg) + rate.sleep() + + def raw_to_rad(self, raw, c): + return (c.initial_position_raw - raw if c.flipped else raw - c.initial_position_raw) * c.RADIANS_PER_ENCODER_TICK