Add debug tool to follow_wall_angle.

osgar/explore.py

@@ -35,7 +35,7 @@ def tangent_circle(dist, radius):
     return math.radians(100)
 
 
-def follow_wall_angle(laser_data, radius, right_wall=False):
+def follow_wall_angle(laser_data, radius, right_wall=False, max_obstacle_distance=4.0, *, debug=False):
     """
         Find the angle to the closest point in laser scan (either on the left or right side).
         Then calculate an angle to a free space as tangent to circle of given radius.
@@ -47,7 +47,7 @@ def follow_wall_angle(laser_data, radius, right_wall=False):
     mask = (data <= 300)  # ignore internal reflections
     data[mask] = 20000
 
-    mask = (data >= 4000)  # ignore obstacles beyond 4m
+    mask = (data >= max_obstacle_distance * 1000)  # ignore obstacles beyond 4m
     data[mask] = 20000
 
     # To make the code simpler, let's pretend we follow the right wall and flip
@@ -77,7 +77,7 @@ def follow_wall_angle(laser_data, radius, right_wall=False):
     if not found_wall:
         # No wall found. Let's slowly circle.
         # TODO: ideally, this would be stateful and we would spiral.
-        return math.radians(-20 if right_wall else 20)
+        return math.radians(-20 if right_wall else 20),
 
     last_wall_idx = wall_start_idx
     while True:
@@ -107,13 +107,23 @@ def follow_wall_angle(laser_data, radius, right_wall=False):
     else:
         tangent_angle = tangent_circle(last_wall_distance, radius)
 
-
     laser_angle = math.radians(-135 + last_wall_idx * deg_resolution)
     total_angle = laser_angle + tangent_angle
     if not right_wall:
         total_angle = -total_angle
 
-    return total_angle
+    if debug:
+
+        def deg(index):
+            ret = -135 + index * deg_resolution
+            return ret if right_wall else -ret
+
+        def rad(index):
+            return math.radians(deg(index))
+
+        return total_angle, rad(wall_start_idx), rad(last_wall_idx)
+
+    return total_angle,
 
 
 class FollowWall(Node):

osgar/lib/drawscan.py

@@ -0,0 +1,41 @@
+import cv2
+import numpy as np
+import math
+
+NO_MEASUREMENT = 0
+MAX_OBSTACLE_DISTANCE = 20
+
+
+def draw_scan(scan, max_obstacle_distance=None, scan_begin=-135, scan_end=135):
+    if max_obstacle_distance is None:
+        max_obstacle_distance = MAX_OBSTACLE_DISTANCE
+    n = len(scan)
+    scan = np.asarray(scan) / 1000
+
+    angles = np.linspace(math.radians(scan_begin), math.radians(scan_end), n).reshape((1, -1))
+    angles_cos = np.cos(angles)
+    angles_sin = np.sin(angles)
+    is_valid = scan != NO_MEASUREMENT
+    valid_scan = scan[is_valid]
+    is_valid = is_valid.reshape((1, -1))
+    acoss = angles_cos[is_valid]
+    asins = angles_sin[is_valid]
+    x = acoss * valid_scan
+    y = asins * valid_scan
+    far_map = valid_scan > max_obstacle_distance
+
+    height_px = 768
+    width_px = 1024
+    img = np.zeros((height_px, width_px, 3), dtype=np.uint8)
+
+    scale = 50
+    for ix, iy, is_far in zip(x, y, far_map):
+        point = (width_px//2 - int(iy*scale), height_px//2 - int(ix*scale))
+        color = (0, 255, 0) if not is_far else (120, 120, 120)
+        cv2.circle(img, point, radius=3, color=color, thickness=-1)
+
+    point = (width_px//2, height_px//2)
+    point2 = (width_px//2, height_px//2-20)
+    cv2.drawMarker(img, point, color=(0, 0, 255), markerType=cv2.MARKER_DIAMOND, thickness=3, markerSize=10)
+    cv2.line(img, point, point2, thickness=3, color=(0, 0, 255))
+    return img

osgar/test_explore.py

@@ -21,7 +21,7 @@ def test_tangent_circle(self):
     def test_follow_wall_angle(self):
         # nothing close -> no command
         scan = [0] * 271  # 1deg resolution
-        cmd = follow_wall_angle(scan, radius=1.0, right_wall=True)
+        cmd, *angles = follow_wall_angle(scan, radius=1.0, right_wall=True)
         self.assertAlmostEqual(cmd, math.radians(-20))
 
         # parallel wall at 1m on the right
@@ -37,18 +37,18 @@ def test_follow_wall_angle(self):
             else:
                 dist_mm = 0  # no reflection
             scan.append(dist_mm)
-        cmd = follow_wall_angle(scan, radius=1.0, right_wall=True)
+        cmd, *angles = follow_wall_angle(scan, radius=1.0, right_wall=True)
         self.assertIsNotNone(cmd)
         # angle should be 0.0, but due to resolution it corresponds to 1deg
         self.assertLess(abs(math.degrees(cmd)), 1.5)
 
         # move towards wall
-        cmd = follow_wall_angle(scan, radius=0.7, right_wall=True)
+        cmd, *angles = follow_wall_angle(scan, radius=0.7, right_wall=True)
         self.assertIsNotNone(cmd)
         self.assertLess(math.degrees(cmd), 0)  # used to be -45, now only slightly move right
 
         # move from wall
-        cmd = follow_wall_angle(scan, radius=1.3, right_wall=True)
+        cmd, *angles = follow_wall_angle(scan, radius=1.3, right_wall=True)
         self.assertIsNotNone(cmd)
         self.assertGreater(math.degrees(cmd), 0)  # just move from the wall (used to be 8deg)
 

subt/main.py

@@ -97,6 +97,7 @@ class SubTChallenge:
     def __init__(self, config, bus):
         self.bus = bus
         bus.register("desired_speed", "pose2d", "artf_xyz", "pose3d", "stdout", "request_origin")
+        self.verbose = False
         self.traveled_dist = 0.0
         self.time = None
         self.max_speed = config['max_speed']
@@ -207,7 +208,11 @@ def go_safely(self, desired_direction):
             safety, safe_direction = 1.0, desired_direction
         else:
             safety, safe_direction = self.local_planner.recommend(desired_direction)
-        #print(self.time,"safety:%f    desired:%f  safe_direction:%f"%(safety, desired_direction, safe_direction))
+        if self.verbose:
+            heading = math.degrees(quaternion.heading(self.orientation))
+            desired_deg = math.degrees(desired_direction)
+            safe_deg = math.degrees(safe_direction)
+            print(self.time, self.sim_time_sec, f"safety:{safety:.2f}    desired:{desired_deg: 7.2f}°  safe_direction:{safe_deg: 7.2f}°")
         #desired_angular_speed = 1.2 * safe_direction
         desired_angular_speed = 0.9 * safe_direction
         size = len(self.scan)
@@ -276,7 +281,11 @@ def follow_wall(self, radius, right_wall=False, timeout=timedelta(hours=3), dist
                         if self.use_center:
                             desired_direction = 0
                         else:
-                            desired_direction = follow_wall_angle(self.scan, radius=radius, right_wall=right_wall)
+                            max_obstacle_distance = 4.0
+                            desired_direction, *angles = follow_wall_angle(self.scan, radius, right_wall, max_obstacle_distance, debug=self.verbose)
+                            #print(angles)
+                            if len(angles):
+                                debug_follow_wall(self.sim_time_sec, self.scan, max_obstacle_distance, angles[0], angles[1], desired_direction)
                         self.go_safely(desired_direction)
                 if dist_limit is not None:
                     if dist_limit < abs(self.traveled_dist - start_dist):  # robot can return backward -> abs()
@@ -895,6 +904,44 @@ def request_stop(self):
     def join(self, timeout=None):
         self.thread.join(timeout)
 
+g_timeout = 0
+
+def debug_follow_wall(sim_time, laser_data, max_obstacle_distance_m, start_rad, last_rad, total_rad):
+    global g_timeout
+    from osgar.lib.drawscan import draw_scan
+    import cv2
+
+    img = draw_scan(laser_data, max_obstacle_distance=max_obstacle_distance_m)
+    cv2.putText(img, f"{sim_time} sec", (50,50), cv2.FONT_HERSHEY_PLAIN, 1, (200,200,200))
+
+    width_px, height_px = img.shape[1], img.shape[0]
+    scale = 300
+    dest_x = math.cos(start_rad)
+    dest_y = math.sin(start_rad)
+    point = (width_px//2 - int(dest_y*scale), height_px//2 - int(dest_x*scale))
+    cv2.line(img, (width_px//2, height_px//2), point, color=(255,255,255))
+
+    dest_x = math.cos(last_rad)
+    dest_y = math.sin(last_rad)
+    point = (width_px//2 - int(dest_y*scale), height_px//2 - int(dest_x*scale))
+    cv2.line(img, (width_px//2, height_px//2), point, color=(120,255,255))
+
+    dest_x = math.cos(total_rad)
+    dest_y = math.sin(total_rad)
+    point = (width_px//2 - int(dest_y*scale), height_px//2 - int(dest_x*scale))
+    cv2.line(img, (width_px//2, height_px//2), point, color=(120,120,255))
+
+    point = (width_px//2, height_px//4)
+    cv2.line(img, (width_px//2, height_px//2), point, color=(120,120,120))
+
+    #print(f"start {math.degrees(start_rad):.2f}°", f"last wall {math.degrees(last_rad):.2f}°" , f"desired {math.degrees(total_rad):.2f}°")
+    cv2.imshow("follow_wall_angle", img)
+    key = cv2.waitKey(g_timeout) & 0xFF
+    if key == ord('q'):
+        raise SystemExit()
+    if key == ord(' '):
+        g_timeout = 1 if g_timeout == 0 else 0
+
 
 def main():
     import argparse