diff --git a/dxl_armed_turtlebot/launch/dxl_armed_turtlebot_bringup.launch b/dxl_armed_turtlebot/launch/dxl_armed_turtlebot_bringup.launch
index c03f8da1..d74c11db 100644
--- a/dxl_armed_turtlebot/launch/dxl_armed_turtlebot_bringup.launch
+++ b/dxl_armed_turtlebot/launch/dxl_armed_turtlebot_bringup.launch
@@ -15,6 +15,11 @@
+
+
+
+
+
diff --git a/dxl_armed_turtlebot/urdf/dynamixel_7dof_arm.urdf.xacro b/dxl_armed_turtlebot/urdf/dynamixel_7dof_arm.urdf.xacro
index 9c82b016..b64ab09e 100644
--- a/dxl_armed_turtlebot/urdf/dynamixel_7dof_arm.urdf.xacro
+++ b/dxl_armed_turtlebot/urdf/dynamixel_7dof_arm.urdf.xacro
@@ -4,16 +4,49 @@
-
-
+
+
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -37,16 +70,16 @@
-
-
+
+
-
+
-
+
@@ -111,58 +144,92 @@
-
-
+
+
-
-
-
+
+
+
+
+
+
+
+
+
+
-
-
-
+
+
+
-
+
-
-
-
+
+
+
+
+
-
+
-
-
+
+
+
+
+
+
+
+
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -177,26 +244,34 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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/config/dynamixel_joint_controllers.yaml b/dynamixel_7dof_arm/config/dynamixel_joint_controllers.yaml
index 001c945d..f43c0ad0 100644
--- a/dynamixel_7dof_arm/config/dynamixel_joint_controllers.yaml
+++ b/dynamixel_7dof_arm/config/dynamixel_joint_controllers.yaml
@@ -1,3 +1,9 @@
+joint_state_publisher:
+ controller:
+ package: dynamixel_7dof_arm
+ module: joint_state_publisher
+ type: JointStatePublisher
+
arm_j1_controller:
controller:
package: dynamixel_controllers
diff --git a/dynamixel_7dof_arm/launch/dynamixel_7dof_arm_bringup.launch b/dynamixel_7dof_arm/launch/dynamixel_7dof_arm_bringup.launch
index 3e57e2f8..d4940fed 100644
--- a/dynamixel_7dof_arm/launch/dynamixel_7dof_arm_bringup.launch
+++ b/dynamixel_7dof_arm/launch/dynamixel_7dof_arm_bringup.launch
@@ -45,6 +45,21 @@
arm_j6_controller"
output="screen"/>
+
+
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