amcl: set_pose.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 
00005 import math
00006 from tf import transformations
00007 from geometry_msgs.msg import PoseWithCovarianceStamped
00008 
00009 
00010 class PoseSetter(rospy.SubscribeListener):
00011     def __init__(self, pose):
00012         self.pose = pose
00013 
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()