set_pose.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy
4 
5 import math
6 from tf import transformations
7 from geometry_msgs.msg import PoseWithCovarianceStamped
8 
9 
10 class PoseSetter(rospy.SubscribeListener):
11  def __init__(self, pose, stamp, publish_time):
12  self.pose = pose
13  self.stamp = stamp
14  self.publish_time = publish_time
15 
16  def peer_subscribe(self, topic_name, topic_publish, peer_publish):
17  p = PoseWithCovarianceStamped()
18  p.header.frame_id = "map"
19  p.header.stamp = self.stamp
20  p.pose.pose.position.x = self.pose[0]
21  p.pose.pose.position.y = self.pose[1]
22  (p.pose.pose.orientation.x,
23  p.pose.pose.orientation.y,
24  p.pose.pose.orientation.z,
25  p.pose.pose.orientation.w) = transformations.quaternion_from_euler(0, 0, self.pose[2])
26  p.pose.covariance[6*0+0] = 0.5 * 0.5
27  p.pose.covariance[6*1+1] = 0.5 * 0.5
28  p.pose.covariance[6*3+3] = math.pi/12.0 * math.pi/12.0
29  # wait for the desired publish time
30  while rospy.get_rostime() < self.publish_time:
31  rospy.sleep(0.01)
32  peer_publish(p)
33 
34 
35 if __name__ == '__main__':
36  pose = map(float, rospy.myargv()[1:4])
37  t_stamp = rospy.Time()
38  t_publish = rospy.Time()
39  if len(rospy.myargv()) > 4:
40  t_stamp = rospy.Time.from_sec(float(rospy.myargv()[4]))
41  if len(rospy.myargv()) > 5:
42  t_publish = rospy.Time.from_sec(float(rospy.myargv()[5]))
43  rospy.init_node('pose_setter', anonymous=True)
44  rospy.loginfo("Going to publish pose {} with stamp {} at {}".format(pose, t_stamp.to_sec(), t_publish.to_sec()))
45  pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped, PoseSetter(pose, stamp=t_stamp, publish_time=t_publish), queue_size=1)
46  rospy.spin()
def peer_subscribe(self, topic_name, topic_publish, peer_publish)
Definition: set_pose.py:16
def __init__(self, pose, stamp, publish_time)
Definition: set_pose.py:11


amcl
Author(s): Brian P. Gerkey, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:36