2 # -*- coding: iso-8859-15 -*-
4 # Gazebo Position: rostopic echo -n 1 /gazebo/model_states
6 # x_err = x_real - x_odom
7 # y_err = y_real - y_odom
8 # phi_err = phi_real - phi_odom
10 # x_cg_cw/ccw: avg x_err
11 # y_cg_cw/ccw: avg y_err
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)
17 # D(l/r): Wheel diameter left/right
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)
25 # Dl = 2/(Ed + 1) * Da
26 # Dr = 2/((1/Ed) + 1) * Da
28 # Wheelbase correction:
29 # alpha = (y_cg_cw - y_cg_ccw)/(-4*L) * 180/pi
30 # Eb = (90)/(90-alpha)
38 from time import sleep
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
48 rospy.init_node('umbmark')
49 self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
50 rospy.Subscriber("odom", Odometry, self.odom_received)
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")
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])
59 def next_pos(self, x, y, angle):
60 odom_quat = tf.transformations.quaternion_from_euler(0, 0, angle)
62 rospy.loginfo("Moving to (%.2f, %2f), %d°.." % (x, y, angle*180/pi))
65 goal.target_pose.header.frame_id = "base_footprint"
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)
75 self.move_base.wait_for_result()
77 if self.move_base.get_state() == GoalStatus.SUCCEEDED:
78 rospy.loginfo("The base moved to (%.2f, %2f), %d°" % (x, y, angle*180/pi))
80 rospy.logerr("The base failed to (%.2f, %2f), %d°" % (x, y, angle*180/pi))
83 def run(self, direction=-1, size=2):
84 while self.odom_pose is None:
86 init_pose = self.odom_pose
88 self.next_pos(size, 0, 0)
89 self.next_pos(0, 0, direction*90*pi/180)
90 final_pose = map(operator.sub, self.odom_pose, init_pose)
91 print "Odom Pose: x=%.3f, y=%.3f, angle=%.3f°" % (final_pose[0], final_pose[1], final_pose[2]*180/pi)
93 def run_cw(self, size):
96 def run_ccw(self, size):
99 if __name__ == "__main__":
100 parser = OptionParser()
101 parser.add_option("-l", "--length", dest="length", default=2, help="Square size")
102 (options, args) = parser.parse_args()
105 if len(sys.argv) > 1 and sys.argv[1] == "ccw":
106 p.run_ccw(float(options.length))
108 p.run_cw(float(options.length))