def test_k_not_met(self):
""" Test that maximum only considers elements within head """
a = CircularArray(3)
a.offer(5)
a.a[1] = 10 # direct assignment
self.assertEqual(a.a, [5, 10, None])
self.assertEqual(a.maximum(), 5)
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 test_full(self):
""" Test a call to maximum() """
a = CircularArray(3)
for x in xrange(1, 4):
a.offer(x)
self.assertEqual(a.a, [1, 2, 3])
self.assertEqual(a.maximum(), 3)
def test_wrapped(self):
""" Test the circular property of the array """
a = CircularArray(3)
for x in xrange(1, 7):
a.offer(x)
self.assertEqual(a.a, [4, 5, 6])
self.assertEqual(a.maximum(), 6)
def test_k_not_met(self):
""" Test that maximum() compares only necessary indices """
a = CircularArray(100000000) # one hundred million
a.offer(1)
start = time.clock()
a.maximum()
end = time.clock()
self.assertTrue(end - start < 1e-04)
def test_maximum_left(self):
"""
Test calling maximum on an empty array
Structured basis: initial condition not met
Boundary: head <= 0
"""
a = CircularArray(3)
self.assertEqual(a.a, [None, None, None])
self.assertEqual(a.maximum(), None)
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_offer_head_left(self):
"""
Test offer to a CircularArray when head is less than the length of a
Data flow: a Used, head Used
Boundary: head < len(a)
"""
a = CircularArray(3)
self.assertEqual(a.a, [None, None, None])
self.assertEqual(a.head, 0)
a.offer(0)
self.assertEqual(a.a, [0, None, None])
self.assertEqual(a.head, 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 test_maximum_middle(self):
"""
Test calling maximum on a partially filled array
Structured basis: initial condition met
Boundary: len(a) > head > 0
"""
a = CircularArray(3)
a.offer(1)
self.assertEqual(a.maximum(), 1)
a.offer(3)
self.assertEqual(a.maximum(), 3)
a.offer(2)
self.assertEqual(a.maximum(), 3)
self.assertEqual(a.a, [1, 3, 2])
def test_offer_head_right(self):
"""
Test offer to a CircularArray when head is equal to the length of a
Data flow: a Used, head Used
Boundary: head >= len(a)
"""
a = CircularArray(3)
a.offer(0)
a.offer(1)
a.offer(2)
self.assertEqual(a.a, [0, 1, 2])
self.assertEqual(a.head, 3)
a.offer(3)
self.assertEqual(a.a, [3, 1, 2])
self.assertEqual(a.head, 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 test_stress(self):
"""
Stress test entire class
Warning: this test has a long runtime
"""
BIG_NUMBER = 1000000 # one million
a = CircularArray(BIG_NUMBER)
# populate first half with random integers
for _ in xrange(BIG_NUMBER / 2):
a.offer(randint(0, 9))
# offer the maximum
a.offer(10)
# populate second half with random integers
for _ in xrange(BIG_NUMBER / 2, BIG_NUMBER):
a.offer(randint(0, 9))
self.assertEqual(a.maximum(), 10)
def test_empty(self):
""" Test a call to maximum() on an empty array """
a = CircularArray(3)
self.assertEqual(a.a, [None, None, None])
self.assertEqual(a.maximum(), None)
def test_maximum_right(self):
"""
Test calling maximum on an overfilled array
Structured basis: initial condition met
Boundary: head >= len(a) > 0
"""
# pre-wrap
a = CircularArray(3)
a.offer(1)
self.assertEqual(a.maximum(), 1)
a.offer(3)
self.assertEqual(a.maximum(), 3)
a.offer(2)
self.assertEqual(a.maximum(), 3)
self.assertEqual(a.a, [1, 3, 2])
# post-wrap
a.offer(0)
self.assertEqual(a.maximum(), 3)
a.offer(0)
self.assertEqual(a.maximum(), 2)
self.assertEqual(a.a, [0, 0, 2])
class WallFollow():
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 stop(self):
self.follow_the_wall = False
def start(self):
self.follow_the_wall = True
def publish_line(self):
# find the two points that intersect between the fan angle lines and the found y=mx+c line
x0 = self.c / (np.tan(FAN_ANGLE) - self.m)
x1 = self.c / (-np.tan(FAN_ANGLE) - self.m)
y0 = self.m * x0 + self.c
y1 = self.m * x1 + self.c
poly = Polygon()
p0 = Point32()
p0.y = x0
p0.x = y0
p1 = Point32()
p1.y = x1
p1.x = y1
poly.points.append(p0)
poly.points.append(p1)
polyStamped = PolygonStamped()
polyStamped.header.frame_id = "base_link"
polyStamped.polygon = poly
self.line_pub.publish(polyStamped)
def drive_straight(self):
while not rospy.is_shutdown():
move_cmd = Twist()
move_cmd.linear.x = 0.1
move_cmd.angular.z = 0
self.cmd_vel_pub.publish(move_cmd)
# don't spin too fast
rospy.sleep(1.0 / PUBLISH_RATE)
def apply_control(self):
while not rospy.is_shutdown():
if self.control is None:
#print "No control data"
rospy.sleep(0.5)
else:
if self.follow_the_wall:
self.steering_hist.append(self.control[0])
# smoothed_steering = self.steering_hist.mean()
smoothed_steering = self.steering_hist.median()
# print smoothed_steering, self.control[0]
move_cmd = Twist()
move_cmd.linear.x = self.control[1]
move_cmd.angular.z = smoothed_steering
self.cmd_vel_pub.publish(move_cmd)
rospy.sleep(1.0 / PUBLISH_RATE)
# given line parameters cached in the self object, compute the pid control
def compute_pd_control(self):
if self.received_data:
# given the computed wall slope, compute theta, avoid divide by zero error
if np.abs(self.m) < EPSILON:
theta = np.pi / 2.0
x_intercept = 0
else:
theta = np.arctan(1.0 / self.m)
# solve for y=0 in y=mx+c
x_intercept = self.c / self.m
# x axis is perp. to robot but not perpindicular to wall
# cosine term solves for minimum distance to wall
wall_dist = np.abs(np.cos(theta) * x_intercept)
# control proportional to angular error and distance from wall
distance_term = self.direction_muliplier * KP * (wall_dist -
TARGET_DISTANCE)
angle_term = KD * theta
control = angle_term + distance_term
# avoid turning too sharply
self.control = (np.clip(control, -0.1, 0.1), SPEED)
def fit_line(self):
if self.received_data and self.xs.shape[0] > 0:
# fit line to euclidean space laser data in the window of interest
slope, intercept, r_val, p_val, std_err = stats.linregress(
self.xs, self.ys)
self.m = slope
self.c = intercept
# window the data, compute the line fit and associated control
def lidarCB(self, msg):
if not self.received_data:
rospy.loginfo("success! first message received")
# populate cached constants
if self.direction == RIGHT:
center_angle = -math.pi / 2
self.direction_muliplier = -1
else:
center_angle = math.pi / 2
self.direction_muliplier = 1
self.min_angle = center_angle - FAN_ANGLE
self.max_angle = center_angle + FAN_ANGLE
self.laser_angles = (np.arange(len(msg.ranges)) *
msg.angle_increment) + msg.angle_min
# filter lidar data to clean it up and remove outlisers
self.received_data = True
if self.follow_the_wall:
self.data = msg.ranges
tmp = np.array(msg.ranges)
# invert lidar(flip mounted)
values = tmp[::-1]
#values = tmp
# remove out of range values
ranges = values[(values > msg.range_min)
& (values < msg.range_max)]
angles = self.laser_angles[(values > msg.range_min)
& (values < msg.range_max)]
# apply median filter to clean outliers
filtered_ranges = signal.medfilt(ranges, MEDIAN_FILTER_SIZE)
# apply a window function to isolate values to the side of the car
window = (angles > self.min_angle) & (angles < self.max_angle)
filtered_ranges = filtered_ranges[window]
filtered_angles = angles[window]
# convert from polar to euclidean coordinate space
self.ys = filtered_ranges * np.cos(filtered_angles)
self.xs = filtered_ranges * np.sin(filtered_angles)
self.fit_line()
self.compute_pd_control()
if PUBLISH_LINE:
self.publish_line()
if SHOW_VIS:
# cache data for development visualization
self.filtered_ranges = filtered_ranges
self.filtered_angles = filtered_angles
def viz_loop(self):
if self.received_data == True:
self.viz.laser_angular.set_data(self.laser_angles, self.data)
self.viz.laser_filtered.set_data(self.filtered_angles,
self.filtered_ranges)
self.viz.laser_euclid.set_data(self.xs, self.ys)
self.viz.laser_regressed.set_data(self.xs,
self.m * self.xs + self.c)
self.viz.redraw()
def shutdown(self):
rospy.loginfo("Stop BigFoot")
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
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)
class TestCircularArray(unittest.TestCase):
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 _compare_contents(self, array=None, data=None):
array, data = array or self.array, data or self.data
self.assertEqual([i for i in self.array], data)
def _call_method(self, method, *args):
getattr(self.array, method)(*args)
getattr(self.data, method)(*args)
def test_setitem(self):
with self.subTest("positive"):
self._call_method("__setitem__", 1, 5)
self._compare_contents()
with self.subTest("negative"):
self._call_method("__setitem__", -1, 5)
self._compare_contents()
def test_insert(self):
with self.subTest("front"):
self._call_method("insert", 1, 1)
self._compare_contents()
with self.subTest("back"):
self._call_method("insert", -2, 1)
self._compare_contents()
def test_delitem(self):
with self.subTest("front"):
self._call_method("__delitem__", 1)
self._compare_contents()
with self.subTest("back"):
self._call_method("__delitem__", -2)
self._compare_contents()
def test_append_right(self):
for i in range(self.array._initialSize + 1):
self._call_method("append", i)
self._compare_contents()
def test_pop_right(self):
self.test_append_right()
for _ in range(self.array._initialSize):
self.assertEqual(self.array.pop(), self.data.pop())
self._compare_contents()
def test_pop_left(self):
self.test_append_right()
for _ in range(self.array._initialSize):
self.assertEqual(self.array.popLeft(), self.data[0])
del self.data[0]
self._compare_contents()
def test_append_left(self):
for i in range(self.array._initialSize):
self.array.appendLeft(i)
self.data.insert(0, i)
self._compare_contents()
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_repr(self):
self.assertEqual(str(self.array), f"<Deque {self.data}>")
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)}")
