Initial commit

CMakeLists.txt

@@ -0,0 +1,33 @@
+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 the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+#uncomment if you have defined messages
+rosbuild_genmsg()
+#uncomment if you have defined services
+rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
+include_directories(/usr/include/openni /usr/include/nite /usr/include/ni)
+rosbuild_add_executable(skeleton_tracker src/skeleton_tracker.cpp src/KinectController.cpp src/KinectDisplay.cpp)
+target_link_libraries(skeleton_tracker glut OpenNI)

Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

README.md

@@ -0,0 +1 @@
+Skeleton Tracker Teleop Package for the Pi Robot Project.

bin/pi_tracker_lib.py

@@ -0,0 +1,97 @@
+#!/usr/bin/env python
+
+"""
+    Common functions shared by nodes in the pi_tracker package.
+    Can be used with the OpenNI tracker package in junction with a
+    Kinect RGB-D camera.
+        
+    Created for the Pi Robot Project: http://www.pirobot.org
+    Copyright (c) 2011 Patrick Goebel.  All rights reserved.
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+    
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details at:
+    
+    http://www.gnu.org/licenses/gpl.html
+"""
+
+import roslib; roslib.load_manifest('pi_tracker')
+import rospy
+from sensor_msgs.msg import JointState
+import xml.dom.minidom
+from math import pi
+import PyKDL as KDL
+
+def get_body_dimension(skeleton, joint1, joint2, default_length):
+    try:
+        # Compute a body dimension to use to scale gesture measurements.
+        if skeleton['confidence'][joint1] > 0.5 and skeleton['confidence'][joint2] > 0.5:
+            # The KDL Normalize() function returns the length of the vector being normalized.
+            length = (skeleton['position'][joint1] - skeleton['position'][joint2]).Normalize()
+            if length == 0:
+                length = default_length
+        else:
+            length = default_length
+    except:
+         length = default_length
+
+    return length
+
+def get_joints():
+    """ This function is take from the joint_state_publisher package written by David Lu!!
+        See http://www.ros.org/wiki/joint_state_publisher
+    """
+    description = rospy.get_param("robot_description")
+    robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0]
+    free_joints = {}
+    joint_list = [] # for maintaining the original order of the joints
+    dependent_joints = rospy.get_param("dependent_joints", {})
+
+    # Find all non-fixed joints.
+    for child in robot.childNodes:
+        if child.nodeType is child.TEXT_NODE:
+            continue
+        if child.localName == 'joint':
+            jtype = child.getAttribute('type')
+            if jtype == 'fixed':
+                continue
+            name = child.getAttribute('name')
+
+            if jtype == 'continuous':
+                minval = -pi
+                maxval = pi
+            else:
+                limit = child.getElementsByTagName('limit')[0]
+                minval = float(limit.getAttribute('lower'))
+                maxval = float(limit.getAttribute('upper'))
+
+            if name in dependent_joints:
+                continue
+            if minval > 0 or maxval < 0:
+                zeroval = (maxval + minval)/2
+            else:
+                zeroval = 0
+
+            joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'value':zeroval }
+            free_joints[name] = joint
+            joint_list.append(name)
+
+    joint_state = JointState()
+    joint_state.header.stamp = rospy.Time.now()
+
+    # Add Free Joints.
+    for (name, joint) in free_joints.items():
+        joint_state.name.append(str(name))
+        joint_state.position.append(joint['value'])
+        joint_state.velocity.append(0)
+
+    return joint_state
+
+
+

bin/relax_joints.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+
+import roslib; roslib.load_manifest('pi_tracker')
+import rospy
+import sys
+
+from ax12_controller_core.srv import SetTorqueLimit
+
+class RelaxJoints():
+    def __init__(self, enable):
+        rospy.init_node('relax_joints')
+        self.controller_namespace = rospy.get_param('controller_namespace', '/ax12_controller')
+
+        self.set_torque_limit = dict()
+        dynamixels = rospy.get_param(self.controller_namespace+'/dynamixels', dict())
+        for name in sorted(dynamixels):
+            rospy.wait_for_service(self.controller_namespace+'/'+name+'_controller/set_torque_limit')  
+            self.set_torque_limit[name] = rospy.ServiceProxy(self.controller_namespace+'/'+name+'_controller/set_torque_limit', SetTorqueLimit)
+            rospy.loginfo(name)
+            self.set_torque_limit[name](0.0)
+
+if __name__ == '__main__':
+    try:
+        RelaxJoints(float(sys.argv[1]))
+    except rospy.ROSInterruptException:
+        pass

bin/robotis_joint_controller.py

@@ -0,0 +1,108 @@
+#!/usr/bin/env python
+
+"""
+    robotis_joint_controller.py - Version 1.0 2010-12-28
+    
+    Joint Controller for AX-12 servos on a USB2Dynamixel device using
+    the Robotis Controller Package from the Healthcare Robotics Lab at Georgia Tech
+    
+    Created for the Pi Robot Project: http://www.pirobot.org
+    Copyright (c) 2010 Patrick Goebel.  All rights reserved.
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+    
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details at:
+    
+    http://www.gnu.org/licenses/gpl.html
+"""
+
+import roslib; roslib.load_manifest('robotis')
+import rospy
+import servo_config as sc
+from robotis.lib_robotis import Robotis_Servo, USB2Dynamixel_Device
+from robotis.ros_robotis import ROS_Robotis_Server, ROS_Robotis_Client
+from sensor_msgs.msg import JointState
+
+class joint_controller():
+    def __init__(self):
+        rospy.init_node('joint_controller')
+
+        rospy.on_shutdown(self.shutdown)
+
+        joint_state_pub = rospy.Publisher('/joint_states', JointState)
+
+        self.port = rospy.get_param('~port', '/dev/ttyUSB0')
+        self.rate = rospy.get_param('~rate', 5)
+        r = rospy.Rate(self.rate)
+
+        """ Read in the servo_config.py file. """
+        self.servo_param = sc.servo_param
+
+        self.controllers = dict()
+        self.joints = dict()
+        self.ids = list()
+
+        """ Read servo ids and joint names from the servo config file. """
+        for id in self.servo_param.keys():
+            joint = self.servo_param[id]['name']
+            self.joints[id] = joint
+            self.ids.append(id)
+
+        """ Connect to the USB2Dynamixel controller """
+        usb2dynamixel = USB2Dynamixel_Device(self.port)
+
+        """ Fire up the ROS joint server processes. """
+        servos = [ Robotis_Servo( usb2dynamixel, i ) for i in self.ids ]
+        ros_servers = [ ROS_Robotis_Server( s, str(j) ) for s, j in zip(servos, self.joints.values()) ]
+
+        """ Fire up the ROS services. """
+        for joint in self.joints.values():
+            self.controllers[joint] = ROS_Robotis_Client(joint)
+
+        """ Start the joint command subscriber """
+        rospy.Subscriber('cmd_joints', JointState, self.cmd_joints_handler) 
+
+        while not rospy.is_shutdown():
+            """ Create a JointState object which we use to publish the current state of the joints. """
+            joint_state = JointState()
+            joint_state.name = list()
+            joint_state.position = list()
+            joint_state.velocity = list()
+            joint_state.effort = list()
+            for s in ros_servers:
+                joint_state.name.append(s.name)
+                joint_state.position.append(s.update_server())
+                """ The robotis driver does not currently query the speed and torque of the servos,
+                    so just set them to 0. """
+                joint_state.velocity.append(0)
+                joint_state.effort.append(0)
+            joint_state.header.stamp = rospy.Time.now()
+            joint_state_pub.publish(joint_state)
+            r.sleep()
+
+    def cmd_joints_handler(self, req):
+        for joint in self.joints.values():
+            try:
+                self.controllers[joint].move_angle(req.position[req.name.index(joint)],
+                max(0.01, req.velocity[req.name.index(joint)]), blocking=False)
+            except:
+                pass
+
+    def shutdown(self):
+        rospy.loginfo('Shutting down joint command controller node...')
+
+
+if __name__ == '__main__':
+    try:
+        jc = joint_controller()
+    except rospy.ROSInterruptException:
+        pass
+
+
+

bin/servo_config.py

@@ -0,0 +1,87 @@
+from math import radians
+
+servo_param = {
+    1: {'name': 'head_pan_joint', 
+        'home_encoder': 512,
+        'max_speed': radians(180),
+        'max_ang': radians(145.),
+        'min_ang': radians(-145.)
+       }, 
+    2: {'name': 'head_tilt_joint',
+        'home_encoder': 512,
+        'max_speed': radians(180),
+        'max_ang': radians(360.),
+        'min_ang': radians(-360.),
+        'flipped': True
+       }, 
+    3: {'name': 'right_shoulder_pan_joint',
+        'home_encoder': 512,
+        'max_speed': radians(180),
+        'max_ang': radians(360.),
+        'min_ang': radians(-360.)
+       }, 
+    4: {'name': 'right_shoulder_lift_joint',
+        'home_encoder': 512,
+        'max_speed': radians(180),
+        'max_ang': radians(360.),
+        'min_ang': radians(-360.),
+        'flipped': True
+       }, 
+    5: {'name': 'right_arm_roll_joint',
+        'home_encoder': 512,
+        'max_speed': radians(180),
+        'max_ang': radians(360.),
+        'min_ang': radians(-360.)
+       }, 
+    6: {'name': 'right_wrist_joint',
+        'home_encoder': 512,
+        'max_speed': radians(180),
+        'max_ang': radians(360.),
+        'min_ang': radians(-360.)
+       }, 
+    7: {'name': 'left_shoulder_pan_joint',
+        'home_encoder': 512,
+        'max_speed': radians(180),
+        'max_ang': radians(360.),
+        'min_ang': radians(-360.),
+        'flipped': True
+       }, 
+    8: {'name': 'left_shoulder_lift_joint',
+        'home_encoder': 512,
+        'max_speed': radians(180),
+        'max_ang': radians(360.),
+        'min_ang': radians(-360.),
+        'flipped': True
+       }, 
+    9: {'name': 'left_arm_roll_joint',
+        'home_encoder': 512,
+        'max_speed': radians(180),
+        'max_ang': radians(360.),
+        'min_ang': radians(-360.)
+       }, 
+    10: {'name': 'left_wrist_joint',
+        'home_encoder': 512,
+        'max_speed': radians(180),
+        'max_ang': radians(360.),
+        'min_ang': radians(-360.)
+       }, 
+    11: {'name': 'torso_joint',
+        'home_encoder': 512,
+        'max_speed': radians(180),
+        'max_ang': radians(360.),
+        'min_ang': radians(-360.)
+       }, 
+    12: {'name': 'left_elbow_joint',
+        'home_encoder': 512,
+        'max_speed': radians(180),
+        'max_ang': radians(360.),
+        'min_ang': radians(-360.)
+       }, 
+    13: {'name': 'right_elbow_joint',
+        'home_encoder': 512,
+        'max_speed': radians(180),
+        'max_ang': radians(360.),
+        'min_ang': radians(-360.)
+       }
+}
+

bin/set_speed.py

@@ -0,0 +1,34 @@
+#!/usr/bin/env python
+
+import roslib; roslib.load_manifest('pi_tracker')
+import rospy
+import sys
+
+from ax12_controller_core.srv import SetSpeed
+
+class SetArmSpeed():
+    def __init__(self, speed):
+        rospy.init_node('set_arm_speed')
+
+        rospy.loginfo("Setting arm speed to " + str(speed))
+
+        max_speed = 0.7
+        if speed > max_speed:
+            speed = max_speed
+
+        self.servo_speed = dict()
+        ax12_namespace = '/ax12_controller'
+        dynamixels = rospy.get_param(ax12_namespace+'/dynamixels', dict())
+        for name in sorted(dynamixels):
+            rospy.wait_for_service(ax12_namespace+'/'+name+'_controller/set_speed')  
+            self.servo_speed[name] = rospy.ServiceProxy(ax12_namespace+'/'+name+'_controller/set_speed', SetSpeed)
+            try:
+                self.servo_speed[name](speed)
+            except:
+                pass
+
+if __name__ == '__main__':
+    try:
+        SetArmSpeed(float(sys.argv[1]))
+    except rospy.ROSInterruptException:
+        pass