Go to the documentation of this file.00001
00002
00003
00004 from __future__ import with_statement
00005
00006 PKG = 'articulation_closedchain'
00007 NAME = 'pose_assembler'
00008 import roslib; roslib.load_manifest(PKG)
00009
00010 import rospy
00011 from geometry_msgs.msg import PoseStamped
00012 from articulation_closedchain.msg import PoseStampedIdMsg
00013 import tf
00014
00015 def send_marker(pose,id):
00016 global pub,br
00017 msg = PoseStampedIdMsg(pose,id)
00018 rospy.loginfo("received marker %d @ %f"%(id,pose.header.stamp.to_sec()))
00019 pub.publish(msg)
00020 br.sendTransform(pose,"/marker/%d"%id)
00021
00022
00023 def callback1(pose):
00024 send_marker(pose,1)
00025
00026 def callback2(pose):
00027 send_marker(pose,2)
00028
00029 def callback3(pose):
00030 send_marker(pose,3)
00031
00032 def callback4(pose):
00033 send_marker(pose,4)
00034
00035 def main(argv=None):
00036 global pub,br
00037 rospy.init_node(NAME, anonymous=False)
00038 rospy.loginfo("ready to receive /marker/[1-4], republishing to /markers")
00039 rospy.Subscriber("/marker/1", PoseStamped, callback1, queue_size=100)
00040 rospy.Subscriber("/marker/2", PoseStamped, callback2, queue_size=100)
00041 rospy.Subscriber("/marker/3", PoseStamped, callback3, queue_size=100)
00042 rospy.Subscriber("/marker/4", PoseStamped, callback4, queue_size=100)
00043 br = tf.TransformBroadcaster()
00044 pub = rospy.Publisher('/markers', PoseStampedIdMsg)
00045 rospy.spin()
00046
00047 if __name__ == '__main__':
00048 main()
00049
