From c4917ae3eaa064bd6cc4dd0831137a23dad4b782 Mon Sep 17 00:00:00 2001 From: Zbynek Winkler Date: Wed, 24 Jun 2020 16:57:57 +0200 Subject: [PATCH] wip --- osgar/explore.py | 10 +++++----- osgar/lib/drawscan.py | 2 +- osgar/test_explore.py | 8 ++++---- subt/main.py | 19 +++++++++++++++---- 4 files changed, 25 insertions(+), 14 deletions(-) diff --git a/osgar/explore.py b/osgar/explore.py index e2e9bec00..b3c5e446a 100644 --- a/osgar/explore.py +++ b/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): diff --git a/osgar/lib/drawscan.py b/osgar/lib/drawscan.py index e687157ef..1cb5670e1 100644 --- a/osgar/lib/drawscan.py +++ b/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) diff --git a/osgar/test_explore.py b/osgar/test_explore.py index 2de426e6e..fe7ced819 100644 --- a/osgar/test_explore.py +++ b/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) diff --git a/subt/main.py b/subt/main.py index 05e3ab8ab..cd9acae10 100644 --- a/subt/main.py +++ b/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():