static_odom_broadcaster.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 #
00004 # Similar to static_transform_broadcaster, this node constantly publishes
00005 # static odometry information (Odometry msg and tf). This can be used
00006 # with fake_localization to evaluate planning algorithms without running
00007 # an actual robot with odometry or localization
00008 #
00009 # Author: Armin Hornung
00010 # License: BSD
00011 
00012 import rospy
00013 
00014 import tf
00015 from nav_msgs.msg import Odometry
00016 from geometry_msgs.msg import Pose, Quaternion, Point
00017 
00018 
00019 def publishOdom():
00020     rospy.init_node('fake_odom')
00021     base_frame_id = rospy.get_param("~base_frame_id", "base_link")
00022     odom_frame_id = rospy.get_param("~odom_frame_id", "odom")
00023     publish_frequency = rospy.get_param("~publish_frequency", 10.0)
00024     pub = rospy.Publisher('odom', Odometry)
00025     tf_pub = tf.TransformBroadcaster()
00026 
00027     #TODO: static pose could be made configurable (cmd.line or parameters)
00028     quat = tf.transformations.quaternion_from_euler(0, 0, 0)
00029 
00030     odom = Odometry()
00031     odom.header.frame_id = odom_frame_id
00032     odom.pose.pose = Pose(Point(0, 0, 0), Quaternion(*quat))
00033 
00034     rospy.loginfo("Publishing static odometry from \"%s\" to \"%s\"", odom_frame_id, base_frame_id)
00035     r = rospy.Rate(publish_frequency)
00036     while not rospy.is_shutdown():
00037         odom.header.stamp = rospy.Time.now()
00038         pub.publish(odom)
00039         tf_pub.sendTransform((0, 0, 0), quat,
00040                              odom.header.stamp, base_frame_id, odom_frame_id)
00041         r.sleep()
00042 
00043 if __name__ == '__main__':
00044     try:
00045         publishOdom()
00046     except rospy.ROSInterruptException:
00047         pass


fake_localization
Author(s): Ioan A. Sucan, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:00