1_test_odom.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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 # reset robot pose and restart spur_contrller to start from origin
00011 #
00012 # use laser: roslaunch base_controller.launch use_base_odom:=false
00013 # use odom : roslaunch base_controller.launch use_base_odom:=true
00014 # 
00015 # with laser odom
00016 # roslaunch spur_controller  spur_controller.launch use_base_odom:=false  use_dynamixel_controller:=false
00017 # ground_truth  -0.143   0.556  -1.487, odom(laser)  -0.146   0.553  -1.493, diff   0.003   0.003   0.006
00018 #
00019 # without laser odom
00020 # ground_truth  -0.065   0.179  -1.436, odom(base )   0.496  -0.007   0.097, diff   0.560   0.186   1.532
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     # first thing, init a node!
00051     rospy.init_node('test_odom')
00052 
00053     # publish to cmd_vel
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) # 50*0.1 = 5.0
00062         print_odom()
00063 
00064     # create a twist message, fill in the details
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         # announce move, and publish the message
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) # 50*0.1 = 5.0
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) # 50*0.1 = 5.0
00089             print_odom()
00090 
00091     # create a new message
00092     twist = Twist()
00093 
00094     # note: everything defaults to 0 in twist, if we don't fill it in, we stop!
00095     rospy.loginfo("Stopping!")
00096     p.publish(twist)


spur_controller
Author(s): Tokyo Opensource Robotics Programmer 534o <534o@opensource-robotics.tokyo.jp>, Isaac I. Y. Saito
autogenerated on Sat Jun 8 2019 19:44:12