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, 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.
@@ -107,12 +107,48 @@ def follow_wall_angle(laser_data, radius, right_wall=False):
     else:
         tangent_angle = tangent_circle(last_wall_distance, radius)
 
+    def deg(index):
+        ret = -135 + index * deg_resolution
+        return ret if right_wall else -ret
+
+    def rad(index):
+        return math.radians(deg(index))
 
     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:
+        from osgar.lib.drawscan import draw_scan
+        import cv2
+        img = draw_scan(laser_data, max_obstacle_distance=4.0)
+        width_px, height_px = img.shape[1], img.shape[0]
+        scale = 500
+        dest_x = math.cos(rad(wall_start_idx))
+        dest_y = math.sin(rad(wall_start_idx))
+        point = (width_px//2 - int(dest_y*scale), height_px//2 - int(dest_x*scale))
+        #print(point, f"start {deg(wall_start_idx)})")
+        cv2.line(img, (width_px//2, height_px//2), point, color=(255,255,255))
+
+        dest_x = math.cos(rad(last_wall_idx))
+        dest_y = math.sin(rad(last_wall_idx))
+        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_angle)
+        dest_y = math.sin(total_angle)
+        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))
+
+        cv2.imshow("follow_wall_angle", img)
+        cv2.waitKey()
+
+        #print("start wall", wall_start_idx, f"deg({deg(wall_start_idx)})", "last wall", last_wall_idx, f"deg({deg(last_wall_idx)})", math.degrees(tangent_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

@@ -257,7 +257,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 +330,7 @@ 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)
+                            desired_direction = follow_wall_angle(self.scan, radius=radius, right_wall=right_wall, debug=self.verbose)
                         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()