From 7cbd2a2354704c5cd6b4fbe9d4f4d7ea89b6d72e Mon Sep 17 00:00:00 2001 From: Zbynek Winkler Date: Thu, 9 Apr 2020 15:02:00 +0200 Subject: [PATCH] osgar.explore: add debug_callback to follow_wall_angle --- osgar/explore.py | 16 +++++++++++++--- osgar/lib/drawscan.py | 42 ++++++++++++++++++++++++++++++++++++++++++ subt/main.py | 40 ++++++++++++++++++++++++++++++++++++++-- 3 files changed, 93 insertions(+), 5 deletions(-) create mode 100644 osgar/lib/drawscan.py diff --git a/osgar/explore.py b/osgar/explore.py index 7bf934817..e2e9bec00 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_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 diff --git a/osgar/lib/drawscan.py b/osgar/lib/drawscan.py new file mode 100644 index 000000000..ecaf94ba5 --- /dev/null +++ b/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 diff --git a/subt/main.py b/subt/main.py index 43f65d2ce..3b65eebf6 100644 --- a/subt/main.py +++ b/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