amcl: set_pose.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
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()