diff --git a/osgar/explore.py b/osgar/explore.py index 7bf934817..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): +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): diff --git a/osgar/lib/drawscan.py b/osgar/lib/drawscan.py new file mode 100644 index 000000000..1cb5670e1 --- /dev/null +++ b/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 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 d2801cb05..8e83171df 100644 --- a/subt/main.py +++ b/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