scripts/umbmark.py

   1 #!/usr/bin/env python
   2 # -*- coding: iso-8859-15 -*-
   3 #
   4 # Gazebo Position: rostopic echo -n 1 /gazebo/model_states
   5 #
   6 # x_err = x_real - x_odom
   7 # y_err = y_real - y_odom
   8 # phi_err = phi_real - phi_odom
   9 #
  10 # x_cg_cw/ccw: avg x_err
  11 # y_cg_cw/ccw: avg y_err
  12 #
  13 # r_cg_cw = sqrt(x_cg_cw**2 + y_cg_cw**2)
  14 # r_cg_ccw = sqrt(x_cg_ccw**2 + y_cg_ccw**2)
  15 #
  16 # L: length (2)
  17 # D(l/r): Wheel diameter left/right
  18 # b: wheelbase
  19 #
  20 # Wheel diameter correction:
  21 # beta = (y_cg_cw + y_cg_ccw)/(-4*L)
  22 # R = (L/2)/sin(beta/2)
  23 # Ed = Dr/Dl*(R+b/2)/(R-b/2)
  24 # Da = (Dr + Dl)/2
  25 # Dl = 2/(Ed + 1) * Da
  26 # Dr = 2/((1/Ed) + 1) * Da
  27 #
  28 # Wheelbase correction:
  29 # alpha = (y_cg_cw - y_cg_ccw)/(-4*L) * 180/pi
  30 # Eb = (90)/(90-alpha)
  31 # b_new = Eb*b
  32
  33 import sys
  34 import rospy
  35 import tf
  36 import actionlib
  37 import operator
  38 from time import sleep
  39 from math import *
  40 from nav_msgs.msg import Odometry
  41 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
  42 from actionlib_msgs.msg import GoalStatus
  43 from optparse import OptionParser
  44
  45
  46 class UMBMark:
  47         def __init__(self):
  48                 rospy.init_node('umbmark')
  49                 self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
  50                 rospy.Subscriber("odom", Odometry, self.odom_received)
  51                 self.odom_pose = None
  52                 while not self.move_base.wait_for_server(rospy.Duration(5)):
  53                         rospy.loginfo("Waiting for the move_base action server to come up")
  54
  55         def odom_received(self, msg):
  56                 orientation = tf.transformations.euler_from_quaternion(msg.pose.pose.orientation.__getstate__())
  57                 self.odom_pose = (msg.pose.pose.position.x, msg.pose.pose.position.y, orientation[2])
  58
  59         def next_pos(self, x, y, angle):
  60                 odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle)
  61
  62                 rospy.loginfo("Moving to (%.2f, %2f), %d°.." % (x, y, angle*180/pi))
  63
  64                 goal = MoveBaseGoal()
  65                 goal.target_pose.header.frame_id = "odom"
  66                 goal.target_pose.header.stamp = rospy.Time.now()
  67                 goal.target_pose.pose.position.x = x
  68                 goal.target_pose.pose.position.y = y
  69                 goal.target_pose.pose.orientation.x = odom_quat[0]
  70                 goal.target_pose.pose.orientation.y = odom_quat[1]
  71                 goal.target_pose.pose.orientation.z = odom_quat[2]
  72                 goal.target_pose.pose.orientation.w = odom_quat[3]
  73                 self.move_base.send_goal(goal)
  74
  75                 self.move_base.wait_for_result()
  76
  77                 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
  78                         rospy.loginfo("The base moved to (%.2f, %2f), %d°" % (x, y, angle*180/pi))
  79                 else:
  80                         rospy.logerr("The base failed to (%.2f, %2f), %d°" % (x, y, angle*180/pi))
  81                         raise
  82
  83         def run(self, direction=-1, size=2):
  84                 while self.odom_pose is None:
  85                         sleep(0.1)
  86                 init_pose = self.odom_pose
  87
  88                 x = size
  89                 y = 0
  90                 angle = 0
  91                 self.next_pos(x, y, angle)
  92                 angle = direction*90*pi/180
  93                 self.next_pos(x, y, angle)
  94
  95                 y = size*direction
  96                 self.next_pos(x, y, angle)
  97                 angle = direction*180*pi/180
  98                 self.next_pos(x, y, direction*-180*pi/180)
  99
 100                 x = 0
 101                 self.next_pos(x, y, angle)
 102                 angle = direction*270*pi/180
 103                 self.next_pos(x, y, angle)
 104
 105                 y = 0
 106                 self.next_pos(x, y, angle)
 107                 angle = 0
 108                 self.next_pos(x, y, angle)
 109
 110                 final_pose = map(operator.sub, self.odom_pose, init_pose)
 111                 print "Odom Pose: x=%.3f, y=%.3f, angle=%.3f°" % (final_pose[0], final_pose[1], final_pose[2]*180/pi)
 112
 113         def run_cw(self, size):
 114                 self.run(-1, size)
 115
 116         def run_ccw(self, size):
 117                 self.run(1, size)
 118
 119 if __name__ == "__main__":
 120         parser = OptionParser()
 121         parser.add_option("-l", "--length", dest="length", default=2, help="Square size")
 122         (options, args) = parser.parse_args()
 123
 124         p = UMBMark()
 125         if len(sys.argv) > 1 and sys.argv[1] == "ccw":
 126                 p.run_ccw(float(options.length))
 127         else:
 128                 p.run_cw(float(options.length))