index f031a67..60a7b5d 100755 (executable)
@@ -40,6 +40,8 @@ from math import *
from nav_msgs.msg import Odometry
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from actionlib_msgs.msg import GoalStatus
+from optparse import OptionParser
+

class UMBMark:
def __init__(self):
@@ -60,7 +62,7 @@ class UMBMark:
rospy.loginfo("Moving to (%.2f, %2f), %d°.." % (x, y, angle*180/pi))

goal = MoveBaseGoal()
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
@@ -78,25 +80,49 @@ class UMBMark:
rospy.logerr("The base failed to (%.2f, %2f), %d°" % (x, y, angle*180/pi))
raise

-       def run(self, direction=-1):
+       def run(self, direction=-1, size=2):
while self.odom_pose is None:
sleep(0.1)
init_pose = self.odom_pose
-               for i in range(4):
-                       self.next_pos(2, 0, 0)
-                       self.next_pos(0, 0, direction*90*pi/180)
+
+               x = size
+               y = 0
+               angle = 0
+               self.next_pos(x, y, angle)
+               angle = direction*90*pi/180
+               self.next_pos(x, y, angle)
+
+               y = size*direction
+               self.next_pos(x, y, angle)
+               angle = direction*180*pi/180
+               self.next_pos(x, y, direction*-180*pi/180)
+
+               x = 0
+               self.next_pos(x, y, angle)
+               angle = direction*270*pi/180
+               self.next_pos(x, y, angle)
+
+               y = 0
+               self.next_pos(x, y, angle)
+               angle = 0
+               self.next_pos(x, y, angle)
+
final_pose = map(operator.sub, self.odom_pose, init_pose)
print "Odom Pose: x=%.3f, y=%.3f, angle=%.3f°" % (final_pose[0], final_pose[1], final_pose[2]*180/pi)

-       def run_cw(self):
-               self.run(-1)
+       def run_cw(self, size):
+               self.run(-1, size)

-       def run_ccw(self):
-               self.run(1)
+       def run_ccw(self, size):
+               self.run(1, size)

if __name__ == "__main__":
+       parser = OptionParser()
+       parser.add_option("-l", "--length", dest="length", default=2, help="Square size")
+       (options, args) = parser.parse_args()
+
p = UMBMark()
if len(sys.argv) > 1 and sys.argv[1] == "ccw":
-               p.run_ccw()
+               p.run_ccw(float(options.length))
else:
-               p.run_cw()
+               p.run_cw(float(options.length))