external_camera_localizer: publish

Go to the documentation of this file.
00001 #!/usr/bin/env rosh
00002 import roslib; roslib.load_manifest('tf')
00003 import sys
00004 import tf
00005 
00006 load('rosh_robot', globals())
00007 
00008 # if __name__ == '__main__':
00009     # if len(sys.argv) != 3:
00010         # logerr('Usage: %s topic1 topic2' % sys.argv[0])
00011         # exit(1)
00012     
00013 topic1 = '/laser_cloud_converted'
00014 topic2 = '/camera1/transformed'
00015 
00016 print 'Waiting for', topic1
00017 cloud1 = topics[topic1][0]
00018 print 'Waiting for', topic2
00019 cloud2 = topics[topic2][0]
00020 
00021 print 'Waiting for service...'
00022 wait_for_service('matchClouds')
00023 
00024 print 'Calling service...'
00025 transform = services.matchClouds(cloud1, cloud2)
00026 
00027 print 'Got result...'
00028 trans = transform.transform.translation
00029 rot   = transform.transform.rotation
00030 
00031 #t = tf.Transformer(True, Duration(10))
00032 
00033 
00034 #listener = tf.TransformListener()
00035 #cam2_rgb_offset = listener.lookupTransform('/camera2_link', cloud2.header.frame_id)
00036 
00037 #trans, rot = listener.lookupTransform(
00038 
00039 
00040 print '<node pkg="tf" type="static_transform_publisher" name="external_cam" args="%s %s %s %s %s %s %s %s %s 100" />' % (trans.x, trans.y, trans.z, rot.x, rot.y, rot.z, rot.w, cloud1.header.frame_id, cloud2.header.frame_id)
00041 print '\n'
00042 print 'rosrun tf static_transform_publisher %s %s %s %s %s %s %s %s %s 100' % (trans.x, trans.y, trans.z, rot.x, rot.y, rot.z, rot.w, cloud1.header.frame_id, cloud2.header.frame_id)