Go to the documentation of this file.00001
00002
00003 """
00004 Common functions shared by nodes in the pi_tracker package.
00005 Can be used with the OpenNI tracker package in junction with a
00006 Kinect RGB-D camera.
00007
00008 Created for the Pi Robot Project: http://www.pirobot.org
00009 Copyright (c) 2011 Patrick Goebel. All rights reserved.
00010
00011 This program is free software; you can redistribute it and/or modify
00012 it under the terms of the GNU General Public License as published by
00013 the Free Software Foundation; either version 2 of the License, or
00014 (at your option) any later version.
00015
00016 This program is distributed in the hope that it will be useful,
00017 but WITHOUT ANY WARRANTY; without even the implied warranty of
00018 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00019 GNU General Public License for more details at:
00020
00021 http://www.gnu.org/licenses/gpl.html
00022 """
00023
00024 import rospy
00025 from sensor_msgs.msg import JointState
00026 import xml.dom.minidom
00027 from math import pi
00028 import PyKDL as KDL
00029
00030 def get_body_dimension(skeleton, joint1, joint2, default_length):
00031 try:
00032
00033 if skeleton['confidence'][joint1] > 0.5 and skeleton['confidence'][joint2] > 0.5:
00034
00035 length = (skeleton['position'][joint1] - skeleton['position'][joint2]).Normalize()
00036 if length == 0:
00037 length = default_length
00038 else:
00039 length = default_length
00040 except:
00041 length = default_length
00042
00043 return length
00044
00045 def get_joints():
00046 """ This function is take from the joint_state_publisher package written by David Lu!!
00047 See http://www.ros.org/wiki/joint_state_publisher
00048 """
00049 description = rospy.get_param("robot_description")
00050 robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0]
00051 free_joints = {}
00052 joint_list = []
00053 dependent_joints = rospy.get_param("dependent_joints", {})
00054
00055
00056 for child in robot.childNodes:
00057 if child.nodeType is child.TEXT_NODE:
00058 continue
00059 if child.localName == 'joint':
00060 jtype = child.getAttribute('type')
00061 if jtype == 'fixed':
00062 continue
00063 name = child.getAttribute('name')
00064
00065 if jtype == 'continuous':
00066 minval = -pi
00067 maxval = pi
00068 else:
00069 limit = child.getElementsByTagName('limit')[0]
00070 minval = float(limit.getAttribute('lower'))
00071 maxval = float(limit.getAttribute('upper'))
00072
00073 if name in dependent_joints:
00074 continue
00075 if minval > 0 or maxval < 0:
00076 zeroval = (maxval + minval)/2
00077 else:
00078 zeroval = 0
00079
00080 joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'value':zeroval }
00081 free_joints[name] = joint
00082 joint_list.append(name)
00083
00084 joint_state = JointState()
00085 joint_state.header.stamp = rospy.Time.now()
00086
00087
00088 for (name, joint) in free_joints.items():
00089 joint_state.name.append(str(name))
00090 joint_state.position.append(joint['value'])
00091 joint_state.velocity.append(0)
00092
00093 return joint_state
00094
00095
00096