00001
00002
00003 import roslib; roslib.load_manifest('amcl')
00004 import rospy
00005
00006 import sys, math
00007 from tf import transformations
00008 from geometry_msgs.msg import PoseWithCovarianceStamped
00009
00010
00011 class PoseSetter(rospy.SubscribeListener):
00012 def __init__(self, pose):
00013 self.pose = pose
00014 def peer_subscribe(self, topic_name, topic_publish, peer_publish):
00015 p = PoseWithCovarianceStamped()
00016 p.header.frame_id = "map"
00017 p.pose.pose.position.x = self.pose[0]
00018 p.pose.pose.position.y = self.pose[1]
00019 (p.pose.pose.orientation.x,
00020 p.pose.pose.orientation.y,
00021 p.pose.pose.orientation.z,
00022 p.pose.pose.orientation.w) = transformations.quaternion_from_euler(0, 0, self.pose[2])
00023 p.pose.covariance[6*0+0] = 0.5 * 0.5
00024 p.pose.covariance[6*1+1] = 0.5 * 0.5
00025 p.pose.covariance[6*3+3] = math.pi/12.0 * math.pi/12.0
00026 peer_publish(p)
00027
00028
00029 if __name__ == '__main__':
00030 pose = map(float, rospy.myargv()[1:])
00031 rospy.init_node('pose_setter', anonymous=True)
00032 pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped, PoseSetter(pose))
00033 rospy.spin()
