def test_compound6(self):
location = easy_Odom(x=0, y=0, v=0.0, heading=0.0, frame='map')
goal = easy_Odom(x=-2.0, y=-1.0, v=0.0, heading=math.pi/4.0, frame='map')
along, off, heading = self.driver_obj.calc_errors(location=location, goal=goal)
self.assertTrue(is_close(along, 1.5*math.sqrt(2.0)))
self.assertTrue(is_close(off, -0.5*math.sqrt(2.0)))
self.assertTrue(is_close(heading, -math.pi/4.0))
def test_compound1(self):
location = easy_Odom(x=0, y=0, v=0.0, heading=0.0, frame='map')
goal = easy_Odom(x=1.0, y=1.0, v=0.0, heading=0.0, frame='map')
along, off, heading = self.driver_obj.calc_errors(location=location, goal=goal)
self.assertTrue(is_close(along, -1.0))
self.assertTrue(is_close(off, -1.0))
self.assertTrue(is_close(heading, 0.0))
def test_aheady_target1(self):
# print('test_ahead_target')
location = easy_Odom(x=0, y=0, v=0.0, heading=math.pi/2.0, frame='map')
goal = easy_Odom(x=0.0, y=1.0, v=0.0, heading=math.pi/2.0, frame='map')
along, off, heading = self.driver_obj.calc_errors(location=location, goal=goal)
self.assertTrue(is_close(along, -1.0))
self.assertTrue(is_close(off, 0))
self.assertTrue(is_close(heading, 0))
def test_behindx_target3(self):
# print('test_behind_target')
location = easy_Odom(x=0, y=0, v=0.0, heading=0.0, frame='map')
goal = easy_Odom(x=-3.0, y=0, v=0.0, heading=0.0, frame='map')
along, off, heading = self.driver_obj.calc_errors(location=location, goal=goal)
self.assertTrue(is_close(along, 3.0))
self.assertTrue(is_close(off, 0))
self.assertTrue(is_close(heading, 0))
def __init__(self, triple='simple'):
self.path = []
if triple == 'simple':
# start
self.path.append(easy_Odom(x=0, y=0, v=0.0, heading=0.0, frame='map'))
# out
self.path.append(easy_Odom(x=0.5, y=0, v=0.0, heading=0.0, frame='map'))
# Figure-8 for x:8m x y:6m box. (forms a 6m-8m-10m right triangle)
elif triple == 'f8_8x6':
# Start in middle facing top left beacon (8mx6m point)
self.path.append(easy_Odom(x=4.0000, y=3.0000, v=0.5, heading= 0.6458, frame='map'))
# Draw partial CW circle with 1.5m radius on right
self.path.append(easy_Odom(x=5.6000, y=4.2000, v=0.5, heading= 0.6458, frame='map'))
self.path.append(easy_Odom(x=6.3653, y=4.4939, v=0.5, heading= 0.0873, frame='map'))
self.path.append(easy_Odom(x=7.1708, y=4.3416, v=0.5, heading=-0.4712, frame='map'))
self.path.append(easy_Odom(x=7.7760, y=3.7886, v=0.5, heading=-1.0123, frame='map'))
self.path.append(easy_Odom(x=8.0000, y=3.0000, v=0.5, heading=-1.5708, frame='map'))
self.path.append(easy_Odom(x=7.7760, y=2.2114, v=0.5, heading=-2.1293, frame='map'))
self.path.append(easy_Odom(x=7.1708, y=1.6584, v=0.5, heading=-2.6704, frame='map'))
self.path.append(easy_Odom(x=6.3653, y=1.5061, v=0.5, heading= 3.0543, frame='map'))
self.path.append(easy_Odom(x=5.6000, y=1.8000, v=0.5, heading= 2.4958, frame='map'))
# Done with partial CW circle on right -> go back to middle
self.path.append(easy_Odom(x=4.0000, y=3.0000, v=0.5, heading= 2.4958, frame='map'))
# Draw partial CCW circle with 1.5m radius on left
self.path.append(easy_Odom(x=2.4000, y=4.2000, v=0.5, heading= 2.4958, frame='map'))
self.path.append(easy_Odom(x=1.6347, y=4.4939, v=0.5, heading= 3.0543, frame='map'))
self.path.append(easy_Odom(x=0.8292, y=4.3416, v=0.5, heading=-2.6704, frame='map'))
self.path.append(easy_Odom(x=0.2240, y=3.7886, v=0.5, heading=-2.1293, frame='map'))
self.path.append(easy_Odom(x=0.0000, y=3.0000, v=0.5, heading=-1.5708, frame='map'))
self.path.append(easy_Odom(x=0.2240, y=2.2114, v=0.5, heading=-1.0123, frame='map'))
self.path.append(easy_Odom(x=0.8292, y=1.6584, v=0.5, heading=-0.4712, frame='map'))
self.path.append(easy_Odom(x=1.6347, y=1.5061, v=0.5, heading= 0.0873, frame='map'))
self.path.append(easy_Odom(x=2.4000, y=1.8000, v=0.5, heading= 0.6458, frame='map'))
# Done with partial CCW circle on left -> go back to middle
self.path.append(easy_Odom(x=4.0000, y=3.0000, v=0.5, heading= 0.6458, frame='map'))
# Stop
# Figure-8 for ShowCASE Demo
elif triple == 'figure_eight':
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
# CW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=0.25, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.5, heading=0.0, frame='map'))
# CCW Circle
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=0.25, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=6.5, y=1.5, v=0.5, heading=pi/2.0, frame='map'))
self.path.append(easy_Odom(x=5.25, y=2.75, v=0.5, heading=pi, frame='map'))
#back to start
self.path.append(easy_Odom(x=4.0, y=1.5, v=0.5, heading=-pi/2.0, frame='map'))
elif triple == 'roltatedI':
# start
self.path.append(easy_Odom(x=3.0, y=2.0, v=0.65, heading=pi*3/4.0, frame='map'))
self.path.append(easy_Odom(x=0.5, y=4.5, v=0.35, heading=pi*3/4.0, frame='map'))
self.path.append(easy_Odom(x=0.5, y=5.0, v=0.35, heading=pi*1/4.0, frame='map'))
self.path.append(easy_Odom(x=1.0, y=5.0, v=0.55, heading=pi*-1/4.0, frame='map'))
self.path.append(easy_Odom(x=2.5, y=3.5, v=0.55, heading=pi*-1/4.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.35, heading=pi*-1/4.0, frame='map'))
self.path.append(easy_Odom(x=3.25, y=2.7, v=0.35, heading=pi*1/4.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=3.5, v=0.55, heading=pi*3/4.0, frame='map'))
self.path.append(easy_Odom(x=0.5, y=6.5, v=0.55, heading=pi*3/4.0, frame='map'))
elif triple == 'halfI':
# start
self.path.append(easy_Odom(x=2, y=2, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=1.75, y=3, v=0.5, heading=pi/2, frame='map'))
# out 1
self.path.append(easy_Odom(x=1.75, y=7, v=0.5, heading=pi/2, frame='map'))
# out 2
self.path.append(easy_Odom(x=1.5, y=7.5, v=0.5, heading=pi/2, frame='map'))
# over
self.path.append(easy_Odom(x=2, y=8, v=0.5, heading=0.0, frame='map'))
# turned around
self.path.append(easy_Odom(x=2.5, y=7.5, v=0.5, heading=-pi/2, frame='map'))
# back 1
self.path.append(easy_Odom(x=2.25, y=7, v=0.5, heading=-pi/2, frame='map'))
# back 2
self.path.append(easy_Odom(x=2.25, y=2, v=0.5, heading=-pi/2, frame='map'))
elif triple == 'I':
# start
self.path.append(easy_Odom(x=1.75, y=2, v=0.5, heading=pi/2, frame='map'))
# offset
self.path.append(easy_Odom(x=1.75, y=3, v=0.5, heading=pi/2, frame='map'))
# out 1
self.path.append(easy_Odom(x=1.75, y=13, v=0.5, heading=pi/2, frame='map'))
# out 2
self.path.append(easy_Odom(x=1.5, y=13.5, v=0.5, heading=pi/2, frame='map'))
# over
self.path.append(easy_Odom(x=2, y=14, v=0.5, heading=0.0, frame='map'))
# turned around
self.path.append(easy_Odom(x=2.5, y=13.5, v=0.5, heading=-pi/2, frame='map'))
# back 1
self.path.append(easy_Odom(x=2.25, y=13, v=0.5, heading=-pi/2, frame='map'))
# back 2
self.path.append(easy_Odom(x=2.25, y=2, v=0.5, heading=-pi/2, frame='map'))
# turn 1
self.path.append(easy_Odom(x=2.5, y=1.5, v=0.5, heading=-pi/2, frame='map'))
# turn 2
self.path.append(easy_Odom(x=2, y=1, v=0.5, heading=pi, frame='map'))
# turn 3
self.path.append(easy_Odom(x=1.5, y=1.5, v=0.5, heading=pi/2, frame='map'))
# back to start
self.path.append(easy_Odom(x=1.75, y=2, v=0.5, heading=pi/2, frame='map'))
elif triple == 'III' or triple == 'hamburger' or triple == 'zigzag':
# start
self.path.append(easy_Odom(x=3.25, y=1, v=0.5, heading=pi/2, frame='map'))
# start 2
self.path.append(easy_Odom(x=3.25, y=2, v=0.5, heading=pi/2, frame='map'))
# back and forth
path_width = float(.7)
# top left
self.path.append(easy_Odom(x=3.25, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# bubble
self.path.append(easy_Odom(x=3, y = 12.5, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5, y= 13, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=4, y = 12.5, v=0.5, heading=-pi/2, frame='map'))
# top right
self.path.append(easy_Odom(x=3.75, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# inside out spiral
# center back
self.path.append(easy_Odom(x=3.75, y=3+path_width, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.75, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 1
self.path.append(easy_Odom(x=3.25-path_width, y=3+path_width, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.25-path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 1 top
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 1
self.path.append(easy_Odom(x=3.75+path_width, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.75+path_width, y=3+3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# right 1 bottom
self.path.append(easy_Odom(x=3.5, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 2
self.path.append(easy_Odom(x=3.25-path_width, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=3.25-2*path_width, y=3+3*path_width/2, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.25-2*path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 2 top
self.path.append(easy_Odom(x=3.25-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 2
self.path.append(easy_Odom(x=3.75+2*path_width, y=13-5*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.75+2*path_width, y=2.0, v=0.5, heading=-pi/2, frame='map'))
# garage
self.path.append(easy_Odom(x=4.5, y=1, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=3.0, y=1, v=0.5, heading=pi, frame='map'))
elif triple == 'IxI':
# start
self.path.append(easy_Odom(x=1, y=1, v=0.5, heading=pi/2, frame='map'))
# start 2
self.path.append(easy_Odom(x=1, y=2, v=0.5, heading=pi/2, frame='map'))
# back and forth
path_width = float(.75)
# lateral passes
for i in range(3.0, 11.5, 2*path_width):
self.path.append(easy_Odom(x=2.65, y=i+path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=5, y=i+path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=5+path_width/2, y=i+path_width, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=5, y=i+path_width*3/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=2.65, y=i+path_width*3/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=2.65-path_width/2, y=i+2*path_width, v=0.5, heading=pi/2, frame='map'))
# last across
## self.path.append(easy_Odom(x=2, y=12.5, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# inside out spiral
# center
self.path.append(easy_Odom(x=3.5, y=3+path_width, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width/2, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 1
self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 1 top
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 1
self.path.append(easy_Odom(x=3.5+path_width, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5+path_width, y=3+3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# right 1 bottom
self.path.append(easy_Odom(x=3.5, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 2
self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=2.25, y=3+3*path_width/2, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=2.25, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 2 top
self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 2
self.path.append(easy_Odom(x=3.5+2*path_width, y=13-5*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5+2*path_width, y=3.0, v=0.5, heading=-pi/2, frame='map'))
# garage
self.path.append(easy_Odom(x=4.0, y=2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=3.0, y=2, v=0.5, heading=pi, frame='map'))
elif triple == 'snownado':
# start
self.path.append(easy_Odom(x=2, y=1, v=0.5, heading=pi/2, frame='map'))
# start 2
self.path.append(easy_Odom(x=2, y=2, v=0.5, heading=pi/2, frame='map'))
# back and forth
path_width = float(.75)
# lateral passes
for i in range(3.0, 12.1, path_width):
self.path.append(easy_Odom(x=2.25, y=i-path_width/2, v=0.5, heading=pi/2, frame='map')) # 1
self.path.append(easy_Odom(x=2.25+path_width, y=i+path_width/2, v=0.5, heading=0, frame='map')) # 2
self.path.append(easy_Odom(x=4.75-path_width, y=i+path_width/2, v=0.5, heading=0, frame='map')) # 3
self.path.append(easy_Odom(x=4.75, y=i-path_width/2, v=0.5, heading=-pi/2, frame='map')) # 4
self.path.append(easy_Odom(x=4.75-path_width, y=i-3*path_width/2, v=0.5, heading=pi, frame='map')) # 5
self.path.append(easy_Odom(x=2.25+2*path_width, y=i-3*path_width/2, v=0.5, heading=pi, frame='map')) # 6
# last across
self.path.append(easy_Odom(x=2, y=12.5, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# finish without spiral
self.path.append(easy_Odom(x=3.0, y=2.5, v=0.5, heading=-pi/2, frame='map'))
# # inside out spiral
# # center
# self.path.append(easy_Odom(x=3.5, y=3+path_width, v=0.5, heading=-pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5-path_width/2, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# # left 1
# self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width, v=0.5, heading=pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5-path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# # left 1 top
# self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# # right 1
# self.path.append(easy_Odom(x=3.5+path_width, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5+path_width, y=3+3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# # right 1 bottom
# self.path.append(easy_Odom(x=3.5, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# # left 2
# self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# self.path.append(easy_Odom(x=3.5-2*path_width, y=3+3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5-2*path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# # left 2 top
# self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# # right 2
# self.path.append(easy_Odom(x=3.5+2*path_width, y=13-5*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5+2*path_width, y=3.0, v=0.5, heading=-pi/2, frame='map'))
# # garage
# self.path.append(easy_Odom(x=4.0, y=2, v=0.5, heading=pi, frame='map'))
# self.path.append(easy_Odom(x=3.0, y=2, v=0.5, heading=pi, frame='map'))
elif triple == 'demo':
self.path.append(easy_Odom(x=3.5, y=1, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5, y=12.5, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=4.0, y=13, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=4.5, y=12.5, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=4.5, y = 2, v=0.5, heading=-pi/2, frame='map'))
self.index = 0
self.rolling_index = -1
# ROS publishers, etc. defined on start_server
self.goal = None
self.start = None
self.start_pub = None
self.back = None
self.next = None
self.current = None
self.rolling = None
def __init__(self, triple='simple'):
self.path = []
if triple == 'simple':
# start
self.path.append(easy_Odom(x=0, y=0, v=0.0, heading=0.0, frame='map'))
# out
self.path.append(easy_Odom(x=0.5, y=0, v=0.0, heading=0.0, frame='map'))
elif triple == 'roltatedI':
# start
self.path.append(easy_Odom(x=3.0, y=2.0, v=0.65, heading=pi*3/4.0, frame='map'))
self.path.append(easy_Odom(x=0.5, y=4.5, v=0.35, heading=pi*3/4.0, frame='map'))
self.path.append(easy_Odom(x=0.5, y=5.0, v=0.35, heading=pi*1/4.0, frame='map'))
self.path.append(easy_Odom(x=1.0, y=5.0, v=0.55, heading=pi*-1/4.0, frame='map'))
self.path.append(easy_Odom(x=2.5, y=3.5, v=0.55, heading=pi*-1/4.0, frame='map'))
self.path.append(easy_Odom(x=2.75, y=2.75, v=0.35, heading=pi*-1/4.0, frame='map'))
self.path.append(easy_Odom(x=3.25, y=2.7, v=0.35, heading=pi*1/4.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=3.5, v=0.55, heading=pi*3/4.0, frame='map'))
self.path.append(easy_Odom(x=0.5, y=6.5, v=0.55, heading=pi*3/4.0, frame='map'))
elif triple == 'halfI':
# start
self.path.append(easy_Odom(x=2, y=2, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=1.75, y=3, v=0.5, heading=pi/2, frame='map'))
# out 1
self.path.append(easy_Odom(x=1.75, y=7, v=0.5, heading=pi/2, frame='map'))
# out 2
self.path.append(easy_Odom(x=1.5, y=7.5, v=0.5, heading=pi/2, frame='map'))
# over
self.path.append(easy_Odom(x=2, y=8, v=0.5, heading=0.0, frame='map'))
# turned around
self.path.append(easy_Odom(x=2.5, y=7.5, v=0.5, heading=-pi/2, frame='map'))
# back 1
self.path.append(easy_Odom(x=2.25, y=7, v=0.5, heading=-pi/2, frame='map'))
# back 2
self.path.append(easy_Odom(x=2.25, y=2, v=0.5, heading=-pi/2, frame='map'))
elif triple == 'I':
# start
self.path.append(easy_Odom(x=2, y=2, v=0.5, heading=pi/2, frame='map'))
# offset
self.path.append(easy_Odom(x=1.75, y=3, v=0.5, heading=pi/2, frame='map'))
# out 1
self.path.append(easy_Odom(x=1.75, y=10, v=0.5, heading=pi/2, frame='map'))
# out 2
self.path.append(easy_Odom(x=1.5, y=10.5, v=0.5, heading=pi/2, frame='map'))
# over
self.path.append(easy_Odom(x=2, y=11, v=0.5, heading=0.0, frame='map'))
# turned around
self.path.append(easy_Odom(x=2.5, y=10.5, v=0.5, heading=-pi/2, frame='map'))
# back 1
self.path.append(easy_Odom(x=2.25, y=10, v=0.5, heading=-pi/2, frame='map'))
# back 2
self.path.append(easy_Odom(x=2.25, y=2.5, v=0.5, heading=-pi/2, frame='map'))
elif triple == 'III' or triple == 'hamburger' or triple == 'zigzag':
# start
self.path.append(easy_Odom(x=3.25, y=1, v=0.5, heading=pi/2, frame='map'))
# start 2
self.path.append(easy_Odom(x=3.25, y=2, v=0.5, heading=pi/2, frame='map'))
# back and forth
path_width = float(.7)
# top left
self.path.append(easy_Odom(x=3.25, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# bubble
self.path.append(easy_Odom(x=3, y = 12.5, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5, y= 13, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=4, y = 12.5, v=0.5, heading=-pi/2, frame='map'))
# top right
self.path.append(easy_Odom(x=3.75, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# inside out spiral
# center back
self.path.append(easy_Odom(x=3.75, y=3+path_width, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.75, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 1
self.path.append(easy_Odom(x=3.25-path_width, y=3+path_width, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.25-path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 1 top
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 1
self.path.append(easy_Odom(x=3.75+path_width, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.75+path_width, y=3+3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# right 1 bottom
self.path.append(easy_Odom(x=3.5, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 2
self.path.append(easy_Odom(x=3.25-path_width, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=3.25-2*path_width, y=3+3*path_width/2, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.25-2*path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 2 top
self.path.append(easy_Odom(x=3.25-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 2
self.path.append(easy_Odom(x=3.75+2*path_width, y=13-5*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.75+2*path_width, y=2.0, v=0.5, heading=-pi/2, frame='map'))
# garage
self.path.append(easy_Odom(x=4.5, y=1, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=3.0, y=1, v=0.5, heading=pi, frame='map'))
elif triple == 'IxI':
# start
self.path.append(easy_Odom(x=1, y=1, v=0.5, heading=pi/2, frame='map'))
# start 2
self.path.append(easy_Odom(x=1, y=2, v=0.5, heading=pi/2, frame='map'))
# back and forth
path_width = float(.75)
# lateral passes
for i in range(3.0, 11.5, 2*path_width):
self.path.append(easy_Odom(x=2.65, y=i+path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=5, y=i+path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=5+path_width/2, y=i+path_width, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=5, y=i+path_width*3/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=2.65, y=i+path_width*3/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=2.65-path_width/2, y=i+2*path_width, v=0.5, heading=pi/2, frame='map'))
# last across
## self.path.append(easy_Odom(x=2, y=12.5, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# inside out spiral
# center
self.path.append(easy_Odom(x=3.5, y=3+path_width, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width/2, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 1
self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 1 top
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 1
self.path.append(easy_Odom(x=3.5+path_width, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5+path_width, y=3+3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# right 1 bottom
self.path.append(easy_Odom(x=3.5, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# left 2
self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=2.25, y=3+3*path_width/2, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=2.25, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# left 2 top
self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# right 2
self.path.append(easy_Odom(x=3.5+2*path_width, y=13-5*path_width/2, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5+2*path_width, y=3.0, v=0.5, heading=-pi/2, frame='map'))
# garage
self.path.append(easy_Odom(x=4.0, y=2, v=0.5, heading=pi, frame='map'))
self.path.append(easy_Odom(x=3.0, y=2, v=0.5, heading=pi, frame='map'))
elif triple == 'snownado':
# start
self.path.append(easy_Odom(x=2, y=1, v=0.5, heading=pi/2, frame='map'))
# start 2
self.path.append(easy_Odom(x=2, y=2, v=0.5, heading=pi/2, frame='map'))
# back and forth
path_width = float(.75)
# lateral passes
for i in range(3.0, 12.1, path_width):
self.path.append(easy_Odom(x=2.25, y=i-path_width/2, v=0.5, heading=pi/2, frame='map')) # 1
self.path.append(easy_Odom(x=2.25+path_width, y=i+path_width/2, v=0.5, heading=0, frame='map')) # 2
self.path.append(easy_Odom(x=4.75-path_width, y=i+path_width/2, v=0.5, heading=0, frame='map')) # 3
self.path.append(easy_Odom(x=4.75, y=i-path_width/2, v=0.5, heading=-pi/2, frame='map')) # 4
self.path.append(easy_Odom(x=4.75-path_width, y=i-3*path_width/2, v=0.5, heading=pi, frame='map')) # 5
self.path.append(easy_Odom(x=2.25+2*path_width, y=i-3*path_width/2, v=0.5, heading=pi, frame='map')) # 6
# last across
self.path.append(easy_Odom(x=2, y=12.5, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=3.5, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# finish without spiral
self.path.append(easy_Odom(x=3.0, y=2.5, v=0.5, heading=-pi/2, frame='map'))
# # inside out spiral
# # center
# self.path.append(easy_Odom(x=3.5, y=3+path_width, v=0.5, heading=-pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5-path_width/2, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# # left 1
# self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width, v=0.5, heading=pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5-path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# # left 1 top
# self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# # right 1
# self.path.append(easy_Odom(x=3.5+path_width, y=13-3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5+path_width, y=3+3*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# # right 1 bottom
# self.path.append(easy_Odom(x=3.5, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# # left 2
# self.path.append(easy_Odom(x=3.5-path_width, y=3+path_width/2, v=0.5, heading=pi, frame='map'))
# self.path.append(easy_Odom(x=3.5-2*path_width, y=3+3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5-2*path_width, y=13-3*path_width/2, v=0.5, heading=pi/2, frame='map'))
# # left 2 top
# self.path.append(easy_Odom(x=3.5-path_width, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# self.path.append(easy_Odom(x=3.5, y=13-path_width/2, v=0.5, heading=0.0, frame='map'))
# # right 2
# self.path.append(easy_Odom(x=3.5+2*path_width, y=13-5*path_width/2, v=0.5, heading=-pi/2, frame='map'))
# self.path.append(easy_Odom(x=3.5+2*path_width, y=3.0, v=0.5, heading=-pi/2, frame='map'))
# # garage
# self.path.append(easy_Odom(x=4.0, y=2, v=0.5, heading=pi, frame='map'))
# self.path.append(easy_Odom(x=3.0, y=2, v=0.5, heading=pi, frame='map'))
elif triple == 'demo':
self.path.append(easy_Odom(x=3.5, y=1, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=3.5, y=12.5, v=0.5, heading=pi/2, frame='map'))
self.path.append(easy_Odom(x=4.0, y=13, v=0.5, heading=0.0, frame='map'))
self.path.append(easy_Odom(x=4.5, y=12.5, v=0.5, heading=-pi/2, frame='map'))
self.path.append(easy_Odom(x=4.5, y = 2, v=0.5, heading=-pi/2, frame='map'))
self.index = 0
self.rolling_index = -1
# ROS publishers, etc. defined on start_server
self.goal = None
self.start = None
self.start_pub = None
self.back = None
self.next = None
self.current = None
self.rolling = None