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