add control manager for dummy/joint_state_publisher option

spur_bringup/launch/spur_dynamixel.launch

@@ -11,7 +11,7 @@
 
   <!-- Start the Dynamixel motor manager to control all spur servos  -->
   <group ns="spur" >
-    <node name="spur_controller_manager" pkg="dynamixel_controllers"
+    <node name="spur_controller_manager" pkg="spur_controller"
           type="controller_manager.py" required="true" output="screen"
           args="$(arg spur_controller_args)" launch-prefix="$(arg launch_prefix)">
       <rosparam file="$(find spur_controller)/config/dynamixel_ports.yaml" command="load"/>

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

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
 

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)

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