static_odom_broadcaster.py
Go to the documentation of this file.
1 #!/usr/bin/python
2 
3 #
4 # Similar to static_transform_broadcaster, this node constantly publishes
5 # static odometry information (Odometry msg and tf). This can be used
6 # with fake_localization to evaluate planning algorithms without running
7 # an actual robot with odometry or localization
8 #
9 # Author: Armin Hornung
10 # License: BSD
11 
12 import rospy
13 
14 import tf
15 from nav_msgs.msg import Odometry
16 from geometry_msgs.msg import Pose, Quaternion, Point
17 
18 
20  rospy.init_node('fake_odom')
21  base_frame_id = rospy.get_param("~base_frame_id", "base_link")
22  odom_frame_id = rospy.get_param("~odom_frame_id", "odom")
23  publish_frequency = rospy.get_param("~publish_frequency", 10.0)
24  pub = rospy.Publisher('odom', Odometry)
25  tf_pub = tf.TransformBroadcaster()
26 
27  #TODO: static pose could be made configurable (cmd.line or parameters)
28  quat = tf.transformations.quaternion_from_euler(0, 0, 0)
29 
30  odom = Odometry()
31  odom.header.frame_id = odom_frame_id
32  odom.pose.pose = Pose(Point(0, 0, 0), Quaternion(*quat))
33 
34  rospy.loginfo("Publishing static odometry from \"%s\" to \"%s\"", odom_frame_id, base_frame_id)
35  r = rospy.Rate(publish_frequency)
36  while not rospy.is_shutdown():
37  odom.header.stamp = rospy.Time.now()
38  pub.publish(odom)
39  tf_pub.sendTransform((0, 0, 0), quat,
40  odom.header.stamp, base_frame_id, odom_frame_id)
41  r.sleep()
42 
43 if __name__ == '__main__':
44  try:
45  publishOdom()
46  except rospy.ROSInterruptException:
47  pass


fake_localization
Author(s): Ioan A. Sucan, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:04