From af2aec5a5bcdd6cc029c115bf32a928da6d80b69 Mon Sep 17 00:00:00 2001 From: chrehall68 <60240707+chrehall68@users.noreply.github.com> Date: Tue, 16 Apr 2024 14:36:29 -0700 Subject: [PATCH] lidar patch that fixes mirrored lidar points (#132) --- unified_frameworks/pathfinder_visualizer.py | 212 ++++++++++-------- .../sensor_array/lidar/actual_lidar.py | 2 +- .../sensor_array/lidar/lidar_visualizer.py | 6 +- 3 files changed, 121 insertions(+), 99 deletions(-) diff --git a/unified_frameworks/pathfinder_visualizer.py b/unified_frameworks/pathfinder_visualizer.py index 32c4239..f76b297 100644 --- a/unified_frameworks/pathfinder_visualizer.py +++ b/unified_frameworks/pathfinder_visualizer.py @@ -8,113 +8,131 @@ import numpy as np from math import pi -config = { - "blit": False, - "view_radius_meter": 40, - "step_delay": 5 -} -def run_visualizer(get_pathfinder, on_hover_mouse=lambda p:None,): - fig = plt.figure(0) - # fig.subplots - ax = plt.subplot(111, projection='polar') - goal_points = ax.scatter([0],[1], marker="*", color='g') - tree_lines = LineCollection([], color='g',) - tree_lines.set_alpha(0.5) - ax.add_collection(tree_lines) - path_line = LineCollection([], color='g', linewidths=3) - ax.add_collection(path_line) - obstacle_groups = LineCollection([], color='k') - ax.add_collection(obstacle_groups) - body = get_pathfinder().worldview.get_rover_body() - rover_body = PolyCollection([body], closed=True) - ax.add_collection(rover_body) - rover_projection = PolyCollection([body], closed=True) - rover_projection.set_alpha(0.2) - ax.add_collection(rover_projection) - - rmax=config['view_radius_meter'] - rscaler = 1.3 - rlagger = 0.99 - ax.set_rmax(rmax) - pos = 1 - delay = 0 - def update_plot(_): - # print() - modded = [] - # Goal points - goal = get_pathfinder().get_goal() - goal_points.set_offsets([goal]) - modded.append(goal_points) - # Visualize exploration tree - tree_links = get_pathfinder().get_tree_links() - tree_lines.set_segments(tree_links) - modded.append(tree_lines) - #--------------------------- - # Visualize path - path = get_pathfinder().get_path() - if path: - path_line.set_segments([path]) - modded.append(path_line) - #--------------------- - obstacles = get_pathfinder().worldview.get_obstacles() - # Visualizing Obstacles - obstacle_groups.set_segments(obstacles) - modded.append(obstacle_groups) - #--------------------- - # Animate the rover intention - nonlocal pos, delay - delay+=1 - if delay >= config['step_delay']: - delay=0 - pos+=1 - if pos >= len(path): - pos=1 - if len(path)>1: - # print(len(path),pos) - a, b = path[pos-1:pos+1] - rotate_angle = polar_sum(a, (b[0],-b[1]))[0]+pi/2 - rotated_body = [np.array([rotate_angle, 0])+p for p in body] - shifted_body = [polar_sum(rov_p, b) for rov_p in rotated_body] - else: - shifted_body = body - rover_projection.set_verts([shifted_body], closed=True) - modded.append(rover_projection) - - - # Scale for obstacles - points = sum(obstacles, []) if obstacles is not None else [] - points.extend(path) - points.extend(sum(tree_links, [])) - points.append(goal) - nonlocal rmax - if points: - rmax = rlagger*rmax + rscaler*(1-rlagger)*max([d for a,d in points]) - ax.set_rmax(rmax) if not config['blit'] else None - - return modded - - def on_mouse_move(event): - point_polar = (event.xdata, event.ydata) - on_hover_mouse(None if any([i is None for i in point_polar]) else point_polar) - fig.canvas.mpl_connect("motion_notify_event", on_mouse_move) - return fig, update_plot +config = {"blit": False, "view_radius_meter": 40, "step_delay": 5} + + +def run_visualizer( + get_pathfinder, + on_hover_mouse=lambda p: None, +): + fig = plt.figure(0) + # fig.subplots + ax = plt.subplot(111, projection="polar") + goal_points = ax.scatter([0], [1], marker="*", color="g") + tree_lines = LineCollection( + [], + color="g", + ) + tree_lines.set_alpha(0.5) + ax.add_collection(tree_lines) + path_line = LineCollection([], color="g", linewidths=3) + ax.add_collection(path_line) + obstacle_groups = LineCollection([], color="k") + ax.add_collection(obstacle_groups) + body = get_pathfinder().worldview.get_rover_body() + rover_body = PolyCollection([body], closed=True) + ax.add_collection(rover_body) + rover_projection = PolyCollection([body], closed=True) + rover_projection.set_alpha(0.2) + ax.add_collection(rover_projection) + + rmax = config["view_radius_meter"] + rscaler = 1.3 + rlagger = 0.99 + ax.set_rmax(rmax) + pos = 1 + delay = 0 + + def update_plot(_): + # print() + modded = [] + # Goal points + goal = get_pathfinder().get_goal() + goal_points.set_offsets([goal]) + modded.append(goal_points) + # Visualize exploration tree + tree_links = get_pathfinder().get_tree_links() + tree_lines.set_segments(tree_links) + modded.append(tree_lines) + # --------------------------- + # Visualize path + path = get_pathfinder().get_path() + if path: + path_line.set_segments([path]) + modded.append(path_line) + # --------------------- + obstacles = get_pathfinder().worldview.get_obstacles() + # Visualizing Obstacles + obstacle_groups.set_segments(obstacles) + modded.append(obstacle_groups) + # --------------------- + # Animate the rover intention + nonlocal pos, delay + delay += 1 + if delay >= config["step_delay"]: + delay = 0 + pos += 1 + if pos >= len(path): + pos = 1 + if len(path) > 1: + # print(len(path),pos) + a, b = path[pos - 1 : pos + 1] + rotate_angle = polar_sum(a, (b[0], -b[1]))[0] + pi / 2 + rotated_body = [np.array([rotate_angle, 0]) + p for p in body] + shifted_body = [polar_sum(rov_p, b) for rov_p in rotated_body] + else: + shifted_body = body + rover_projection.set_verts([shifted_body], closed=True) + modded.append(rover_projection) + + # Scale for obstacles + points = sum(obstacles, []) if obstacles is not None else [] + points.extend(path) + points.extend(sum(tree_links, [])) + points.append(goal) + nonlocal rmax + if points: + rmax = rlagger * rmax + rscaler * (1 - rlagger) * max( + [d for a, d in points] + ) + ax.set_rmax(rmax) if not config["blit"] else None + + return modded + + def on_mouse_move(event): + point_polar = (event.xdata, event.ydata) + on_hover_mouse(None if any([i is None for i in point_polar]) else point_polar) + + fig.canvas.mpl_connect("motion_notify_event", on_mouse_move) + return fig, update_plot + def show_visual(get_pathfinder): def on_hover_point(point_polar): # return get_pathfinder().set_goal(point_polar) if not point_polar is None else None + fig, update_func = run_visualizer(get_pathfinder, on_hover_point) - anime = anim.FuncAnimation(fig, update_func, 1, interval=50, blit=config['blit']) + anime = anim.FuncAnimation(fig, update_func, 1, interval=50, blit=config["blit"]) plt.show() return anime -if __name__=='__main__': - import unified_frameworks.pathfinders.pathfinder as pathfinder + +if __name__ == "__main__": + import re + import os + + root = next(re.finditer(".*unified_frameworks", __file__)).group() + sys.path.append(root) if root not in sys.path else None + sys.path.append(os.path.realpath(__file__ + os.sep + ".." + os.sep + "..")) + import unified_frameworks.pathfinders.pathfinder as _pathfinder import matplotlib from unified_utils import time_tracking_service + time_tracking_service.start_service() - pathfinder.get_pathfinder().start_pathfinder_service() + pathfinder = _pathfinder.Pathfinder() + pathfinder.start_pathfinder_service() - show_visual(pathfinder.get_pathfinder) - pathfinder.get_pathfinder().stop_pathfinder_service() - time_tracking_service.stop_service() \ No newline at end of file + show_visual(lambda: pathfinder) + pathfinder.stop_pathfinder_service() + time_tracking_service.stop_service() diff --git a/unified_frameworks/sensor_array/lidar/actual_lidar.py b/unified_frameworks/sensor_array/lidar/actual_lidar.py index 22db051..9276e90 100644 --- a/unified_frameworks/sensor_array/lidar/actual_lidar.py +++ b/unified_frameworks/sensor_array/lidar/actual_lidar.py @@ -69,7 +69,7 @@ def connect(self, max_attempts=3, wait_seconds=1, verbose_attempts=False) -> boo def look(): while self.stay_connected: - self.measures = next(self.lidar_iter) + self.measures = [(q, -a % 360, d) for q, a, d in next(self.lidar_iter)] self.thread = Thread(target=look) self.thread.start() diff --git a/unified_frameworks/sensor_array/lidar/lidar_visualizer.py b/unified_frameworks/sensor_array/lidar/lidar_visualizer.py index c155250..9e3b986 100644 --- a/unified_frameworks/sensor_array/lidar/lidar_visualizer.py +++ b/unified_frameworks/sensor_array/lidar/lidar_visualizer.py @@ -5,13 +5,16 @@ import sys # print(sys.path) -import sensor_array.lidar.lidar as L +import lidar as L import time import traceback +import bridge.client_side as client_side # lidar.config[''] if __name__ == "__main__": + client_side.service.start_service() lidar = L.Lidar() + lidar.start_service() # time.sleep(20) try: @@ -48,3 +51,4 @@ def update_plot(_): print(e) traceback.print_exc() lidar.stop_service() + client_side.service.stop_service()