wip

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, max_obstacle_distance=4.0, debug_callback=None):
+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.
@@ -77,7 +77,7 @@ def follow_wall_angle(laser_data, radius, right_wall=False, max_obstacle_distanc
     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:
@@ -112,7 +112,7 @@ def follow_wall_angle(laser_data, radius, right_wall=False, max_obstacle_distanc
     if not right_wall:
         total_angle = -total_angle
 
-    if debug_callback is not None:
+    if debug:
 
         def deg(index):
             ret = -135 + index * deg_resolution
@@ -121,9 +121,9 @@ def deg(index):
         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, rad(wall_start_idx), rad(last_wall_idx)
 
-    return total_angle
+    return total_angle,
 
 
 class FollowWall(Node):

osgar/lib/drawscan.py

@@ -30,7 +30,7 @@ def draw_scan(scan, max_obstacle_distance=None, scan_begin=-135, scan_end=135):
 
     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))
+        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)
 

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

@@ -281,8 +281,11 @@ def follow_wall(self, radius, right_wall=False, timeout=timedelta(hours=3), dist
                         if self.use_center:
                             desired_direction = 0
                         else:
-                            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)
+                            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()
@@ -901,12 +904,16 @@ def request_stop(self):
     def join(self, timeout=None):
         self.thread.join(timeout)
 
+g_timeout = 0
 
-def debug_follow_wall(laser_data, max_obstacle_distance_m, start_rad, last_rad, total_rad):
+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)
@@ -929,7 +936,11 @@ def debug_follow_wall(laser_data, max_obstacle_distance_m, start_rad, last_rad,
 
     #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()
+    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():