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)}")