osgar.explore: add debug_callback 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_callback=None):
     """
         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
@@ -107,12 +107,22 @@ 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
 
+    if debug_callback is not None:
+
+        def deg(index):
+            ret = -135 + index * deg_resolution
+            return ret if right_wall else -ret
+
+        def rad(index):
+            return math.radians(deg(index))
+
+        debug_callback(laser_data, max_obstacle_distance, rad(wall_start_idx), rad(last_wall_idx), total_angle)
+
     return total_angle
 
 

osgar/lib/drawscan.py

@@ -0,0 +1,42 @@
+import cv2
+import numpy as np
+import math
+import osgar.explore
+
+NO_MEASUREMENT = 0
+MAX_OBSTACLE_DISTANCE = 20
+
+
+def draw_scan(scan, max_obstacle_distance=None, scan_left=135, scan_right=-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_right), math.radians(scan_left), 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

subt/main.py

@@ -146,6 +146,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.start_pose = None
         self.traveled_dist = 0.0
         self.time = None
@@ -257,7 +258,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:.2f}°  safe_direction: {safe_deg:.2f}°")
         #desired_angular_speed = 1.2 * safe_direction
         desired_angular_speed = 0.9 * safe_direction
         size = len(self.scan)
@@ -326,7 +331,8 @@ 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)
+                            debug_callback = debug_follow_wall if self.verbose else None
+                            desired_direction = follow_wall_angle(self.scan, radius=radius, right_wall=right_wall, debug_callback=debug_callback)
                         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()
@@ -1006,6 +1012,36 @@ def join(self, timeout=None):
         self.thread.join(timeout)
 
 
+def debug_follow_wall(laser_data, max_obstacle_distance_m, start_rad, last_rad, total_rad):
+    from osgar.lib.drawscan import draw_scan
+    import cv2
+
+    img = draw_scan(laser_data, max_obstacle_distance=max_obstacle_distance_m)
+    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)
+    cv2.waitKey()
+
+
 def main():
     import argparse
     from osgar.lib.config import config_load