X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Fumbmark.py;h=60a7b5deaeae6cb8ef8b9173ca3ea0f9908a0bce;hp=d103b5c0abf277b6afb0f0a9b544f9ce503af549;hb=6064553e6895205dd17bb5a49914415b24623de7;hpb=6925d3469d5f22b6271ece74540c0a30a51942db

diff --git a/scripts/umbmark.py b/scripts/umbmark.py
index d103b5c..60a7b5d 100755
--- a/scripts/umbmark.py
+++ b/scripts/umbmark.py
@@ -21,10 +21,14 @@
 # beta = (y_cg_cw + y_cg_ccw)/(-4*L)
 # R = (L/2)/sin(beta/2)
 # Ed = Dr/Dl*(R+b/2)/(R-b/2)
+# Da = (Dr + Dl)/2
+# Dl = 2/(Ed + 1) * Da
+# Dr = 2/((1/Ed) + 1) * Da
 # 
 # Wheelbase correction:
 # alpha = (y_cg_cw - y_cg_ccw)/(-4*L) * 180/pi
 # Eb = (90)/(90-alpha)
+# b_new = Eb*b
 
 import sys
 import rospy
@@ -36,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):
@@ -56,7 +62,7 @@ class UMBMark:
 		rospy.loginfo("Moving to (%.2f, %2f), %d°.." % (x, y, angle*180/pi))
 
 		goal = MoveBaseGoal()
-		goal.target_pose.header.frame_id = "base_footprint"
+		goal.target_pose.header.frame_id = "odom"
 		goal.target_pose.header.stamp = rospy.Time.now()
 		goal.target_pose.pose.position.x = x
 		goal.target_pose.pose.position.y = y
@@ -74,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=%.2f, y=%.2f, angle=%.2f°" % (final_pose[0], final_pose[1], final_pose[2]*180/pi)
+		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))