set_pose.py
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, stamp, publish_time):
00012         self.pose = pose
00013         self.stamp = stamp
00014         self.publish_time = publish_time
00015 
00016     def peer_subscribe(self, topic_name, topic_publish, peer_publish):
00017         p = PoseWithCovarianceStamped()
00018         p.header.frame_id = "map"
00019         p.header.stamp = self.stamp
00020         p.pose.pose.position.x = self.pose[0]
00021         p.pose.pose.position.y = self.pose[1]
00022         (p.pose.pose.orientation.x,
00023          p.pose.pose.orientation.y,
00024          p.pose.pose.orientation.z,
00025          p.pose.pose.orientation.w) = transformations.quaternion_from_euler(0, 0, self.pose[2])
00026         p.pose.covariance[6*0+0] = 0.5 * 0.5
00027         p.pose.covariance[6*1+1] = 0.5 * 0.5
00028         p.pose.covariance[6*3+3] = math.pi/12.0 * math.pi/12.0
00029         # wait for the desired publish time
00030         while rospy.get_rostime() < self.publish_time:
00031             rospy.sleep(0.01)
00032         peer_publish(p)
00033 
00034 
00035 if __name__ == '__main__':
00036     pose = map(float, rospy.myargv()[1:4])
00037     t_stamp = rospy.Time()
00038     t_publish = rospy.Time()
00039     if len(rospy.myargv()) > 4:
00040         t_stamp = rospy.Time.from_sec(float(rospy.myargv()[4]))
00041     if len(rospy.myargv()) > 5:
00042         t_publish = rospy.Time.from_sec(float(rospy.myargv()[5]))
00043     rospy.init_node('pose_setter', anonymous=True)
00044     rospy.loginfo("Going to publish pose {} with stamp {} at {}".format(pose, t_stamp.to_sec(), t_publish.to_sec()))
00045     pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped, PoseSetter(pose, stamp=t_stamp, publish_time=t_publish), queue_size=1)
00046     rospy.spin()


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:02