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