def setUp(self):
"""
[1, 2, 3, 4, 5, 6]
0 1 2 3 4 5
"""
self.data = [1, 2, 3, 4, 5, 6]
self.array = CircularArray(self.data)
def __init__(self, direction):
if direction not in [RIGHT, LEFT]:
rospy.loginfo("incorect %s wall selected. choose left or right")
rospy.signal_shutdown()
self.direction = direction
rospy.loginfo("To stop TurtleBot CTRL + C")
rospy.on_shutdown(self.shutdown)
if SHOW_VIS:
self.viz = DynamicPlot()
self.viz.initialize()
self.sub = rospy.Subscriber("/scan",
LaserScan,
self.lidarCB,
queue_size=1)
self.cmd_vel_pub = rospy.Publisher('/cmd_vel_mux/input/wall_follower',
Twist,
queue_size=10)
if PUBLISH_LINE:
self.line_pub = rospy.Publisher("/viz/line_fit",
PolygonStamped,
queue_size=1)
# computed control instructions
self.control = None
self.steering_hist = CircularArray(HISTORY_SIZE)
# containers for laser scanner related data
self.data = None
self.xs = None
self.ys = None
self.m = 0
self.c = 0
# flag to indicate the first laser scan has been received
self.received_data = False
# flag for start/stop following
self.follow_the_wall = False
# cached constants
self.min_angle = None
self.max_angle = None
self.direction_muliplier = 0
self.laser_angles = None
self.drive_thread = Thread(target=self.apply_control)
self.drive_thread.start()
if SHOW_VIS:
while not rospy.is_shutdown():
self.viz_loop()
rospy.sleep(0.1)
def test_shrink(self):
self.data = []
self.array = CircularArray(self.data)
for i in range(self.array._initialSize + 1):
self.array.append(i)
self.data.append(i)
self.assertEqual(len(self.array._array),
self.array._initialSize * self.array._resizeFactor)
for i in range(self.array._initialSize + 1):
self.array.pop()
self.data.pop()
self._compare_contents()
self.assertEqual(len(self.array._array), self.array._initialSize)
def test_speed(self):
k = 1000
array, list = CircularArray(), []
start_array = time.time()
for i in range(k):
array.insert(0, i)
end_array = time.time()
start_list = time.time()
for i in range(k):
list.insert(0, k)
end_list = time.time()
print("\n")
print(f"array time: {round(end_array-start_array, 4)}")
print(f"list time: {round(end_list-start_list, 4)}")
def testPushPop(self):
cir = CircularArray(10)
# try pop with empty array
self.assertEqual(cir.pop(), None)
# since this is a circular array
# it should support multiple fill out and clear
for j in range(10):
# push ten elements
for i in range(10):
self.assertEqual(cir.push(i), True)
# it is full, it should return false
# means fail to push again
self.assertEqual(cir.push(1), False)
for i in range(10):
self.assertEqual(i, cir.pop())
# it should support push and pop right after each other
for i in range(100):
ran = random.randint(-10000, 10000)
self.assertEqual(cir.push(ran), True)
self.assertEqual(cir.pop(), ran)
def testState(self):
cir = CircularArray(5)
self.assertEqual(cir.isEmpty(), True)
self.assertEqual(cir.isFull(), False)
cir.push('A')
self.assertEqual(cir.isEmpty(), False)
self.assertEqual(cir.isFull(), False)
cir.push('B')
self.assertEqual(cir.isEmpty(), False)
self.assertEqual(cir.isFull(), False)
cir.push('C')
self.assertEqual(cir.isEmpty(), False)
self.assertEqual(cir.isFull(), False)
cir.push('D')
self.assertEqual(cir.isEmpty(), False)
self.assertEqual(cir.isFull(), False)
cir.push('E')
self.assertEqual(cir.isEmpty(), False)
self.assertEqual(cir.isFull(), True)
# pop
self.assertEqual(cir.pop(), 'A')
self.assertEqual(cir.isEmpty(), False)
self.assertEqual(cir.isFull(), False)
self.assertEqual(cir.pop(), 'B')
self.assertEqual(cir.isEmpty(), False)
self.assertEqual(cir.isFull(), False)
self.assertEqual(cir.pop(), 'C')
self.assertEqual(cir.isEmpty(), False)
self.assertEqual(cir.isFull(), False)
self.assertEqual(cir.pop(), 'D')
self.assertEqual(cir.isEmpty(), False)
self.assertEqual(cir.isFull(), False)
self.assertEqual(cir.pop(), 'E')
self.assertEqual(cir.isEmpty(), True)
self.assertEqual(cir.isFull(), False)
self.assertEqual(cir.pop(), None)
self.assertEqual(cir.isEmpty(), True)
self.assertEqual(cir.isFull(), False)
