Go to the documentation of this file.00001
00002
00003 import rospy
00004 from math import sin, pi
00005 from nav_msgs.msg import Odometry
00006 from geometry_msgs.msg import Twist, Pose2D
00007 from tf.transformations import euler_from_quaternion
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 odom = {}
00024 pose = None
00025 def odom_cb(msg):
00026 global odom
00027 odom[msg.header.frame_id] = msg
00028 def pose_cb(msg):
00029 global pose
00030 pose = msg
00031
00032 def print_odom():
00033 global odom, pose
00034 t1_x = odom['map'].pose.pose.position.x
00035 t1_y = odom['map'].pose.pose.position.y
00036 t1_z = euler_from_quaternion([odom['map'].pose.pose.orientation.x,odom['map'].pose.pose.orientation.y,odom['map'].pose.pose.orientation.z,odom['map'].pose.pose.orientation.w])[2]
00037 if odom.has_key('odom'):
00038 t2_x = odom['odom'].pose.pose.position.x
00039 t2_y = odom['odom'].pose.pose.position.y
00040 t2_z = euler_from_quaternion([odom['odom'].pose.pose.orientation.x,odom['odom'].pose.pose.orientation.y,odom['odom'].pose.pose.orientation.z,odom['odom'].pose.pose.orientation.w])[2]
00041 src = "base "
00042 else:
00043 t2_x = pose.x
00044 t2_y = pose.y
00045 t2_z = pose.theta
00046 src = "laser"
00047 print("ground_truth %7.3f %7.3f %7.3f, odom(%s) %7.3f %7.3f %7.3f, diff %7.3f %7.3f %7.3f" % (t1_x, t1_y, t1_z, src, t2_x, t2_y, t2_z, abs(t1_x-t2_x), abs(t1_y-t2_y), abs(t1_z-t2_z)))
00048
00049 if __name__=="__main__":
00050
00051 rospy.init_node('test_odom')
00052
00053
00054 p = rospy.Publisher('spur/cmd_vel', Twist)
00055 rospy.Subscriber('odom', Odometry, odom_cb, queue_size=10)
00056 rospy.Subscriber('base_pose_ground_truth', Odometry, odom_cb, queue_size=10)
00057 rospy.Subscriber('pose2D', Pose2D, pose_cb, queue_size=10)
00058
00059
00060 for i in range(50):
00061 rospy.sleep(0.1)
00062 print_odom()
00063
00064
00065 twist = Twist()
00066 twist.linear.x = 0
00067 twist.linear.y = 0
00068 twist.linear.z = 0
00069 twist.angular.x = 0
00070 twist.angular.y = 0
00071 twist.angular.z = 0
00072
00073 for i in range(4):
00074
00075 twist.linear.x = 0.1
00076 twist.angular.z = 0
00077 rospy.loginfo("move forward for 5 sec (0.5m)")
00078 for i in range(50):
00079 p.publish(twist)
00080 rospy.sleep(0.1)
00081 print_odom()
00082
00083 twist.linear.x = 0
00084 twist.angular.z = 0.3
00085 rospy.loginfo("rotate for 5 sec (90 deg)")
00086 for i in range(50):
00087 p.publish(twist)
00088 rospy.sleep(0.1)
00089 print_odom()
00090
00091
00092 twist = Twist()
00093
00094
00095 rospy.loginfo("Stopping!")
00096 p.publish(twist)