collada_joint_publisher.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 # -*- coding: utf-8 -*-
00003 # 
00004 # Licensed under the Apache License, Version 2.0 (the "License");
00005 # you may not use this file except in compliance with the License.
00006 # You may obtain a copy of the License at
00007 #     http://www.apache.org/licenses/LICENSE-2.0
00008 # 
00009 # Unless required by applicable law or agreed to in writing, software
00010 # distributed under the License is distributed on an "AS IS" BASIS,
00011 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00012 # See the License for the specific language governing permissions and
00013 # limitations under the License. 
00014 import roslib; roslib.load_manifest('geometry_msgs')
00015 import roslib; roslib.load_manifest('sensor_msgs')
00016 import roslib; roslib.load_manifest('openrave')
00017 import roslib; roslib.load_manifest('tf')
00018 import rospy
00019 from openravepy import *
00020 import sensor_msgs.msg
00021 import geometry_msgs.msg
00022 import tf
00023 import tf.msg
00024 import numpy
00025 
00026 class ColladaJointPublisher():
00027     def __init__(self):
00028         description = rospy.get_param("robot_description")
00029         self.env=Environment()
00030         self.robot=self.env.ReadRobotXMLData(description)
00031         self.env.AddRobot(self.robot)
00032         self.env.SetViewer('qtcoin')
00033         # can use to set random values
00034         #self.robot.SetJointValues([1,1,1,1],[0,1,2,3])
00035         self.pub = rospy.Publisher('joint_states', sensor_msgs.msg.JointState)
00036         self.pub_tf = rospy.Publisher("/tf", tf.msg.tfMessage)
00037 
00038     def loop(self):
00039         hz = rospy.get_param("rate", 10) # 10hz
00040         r = rospy.Rate(hz) 
00041 
00042         # Publish Joint States
00043         while not rospy.is_shutdown():
00044             msg = sensor_msgs.msg.JointState()
00045             msg.header.stamp = rospy.Time.now()
00046             for j in self.robot.GetJoints():
00047                 msg.name.append(str(j.GetName()))
00048                 msg.position.append(j.GetValues()[0])
00049             self.pub.publish(msg)
00050             transforms = []
00051             for link in self.robot.GetLinks():
00052                 t = geometry_msgs.msg.TransformStamped()
00053                 t.header.stamp = msg.header.stamp
00054                 t.child_frame_id = str(link.GetName())
00055                 T = link.GetTransform()
00056                 if len(link.GetParentLinks()) == 0:
00057                     t.header.frame_id = '/map'
00058                     Tparent = numpy.eye(4)
00059                 else:
00060                     t.header.frame_id = str(link.GetParentLinks()[0].GetName())
00061                     Tparent = link.GetParentLinks()[0].GetTransform()
00062                 T = numpy.dot(numpy.linalg.inv(Tparent),T)
00063                 q = quatFromRotationMatrix(T[0:3,0:3])
00064                 t.transform.translation.x = T[0,3]
00065                 t.transform.translation.y = T[1,3]
00066                 t.transform.translation.z = T[2,3]
00067                 t.transform.rotation.x = q[1]
00068                 t.transform.rotation.y = q[2]
00069                 t.transform.rotation.z = q[3]
00070                 t.transform.rotation.w = q[0]
00071 
00072                 transforms.append(t)
00073             self.pub_tf.publish(tf.msg.tfMessage(transforms))
00074             r.sleep()
00075 
00076 if __name__ == '__main__':
00077     try:
00078         RaveInitialize()
00079         rospy.init_node('collada_joint_publisher')
00080         jsp = ColladaJointPublisher()
00081         jsp.loop()
00082     except rospy.ROSInterruptException:
00083         pass
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


orrosplanning
Author(s): Rosen Diankov (rosen.diankov@gmail.com)
autogenerated on Sat Mar 23 2013 22:32:56