From dd66c09940540227704cda7de3bb78503bf18a29 Mon Sep 17 00:00:00 2001 From: Rahul Date: Sat, 6 Apr 2024 16:29:27 -0700 Subject: [PATCH] Classinated lidar.py --- unified_frameworks/sensor_array/LidarClass.py | 4 +- .../sensor_array/actual_lidar.py | 6 +- .../sensor_array/bridge_lidar.py | 4 +- unified_frameworks/sensor_array/fake_lidar.py | 4 +- unified_frameworks/sensor_array/lidar.py | 295 ++++-------------- .../sensor_array/lidar_visualizer.py | 13 +- unified_frameworks/unified_utils.py | 17 +- 7 files changed, 99 insertions(+), 244 deletions(-) diff --git a/unified_frameworks/sensor_array/LidarClass.py b/unified_frameworks/sensor_array/LidarClass.py index 14ea65d..33afbc3 100644 --- a/unified_frameworks/sensor_array/LidarClass.py +++ b/unified_frameworks/sensor_array/LidarClass.py @@ -1,7 +1,7 @@ from abc import ABC, abstractmethod class LidarDisconnectException(Exception): pass -class Lidar(ABC): +class _Lidar(ABC): @abstractmethod def connect(self, max_attempts=3, wait_seconds=1, verbose_attempts=False) -> bool: """Make attempts to connect to the Lidar @@ -22,7 +22,7 @@ def disconnect(self): pass @abstractmethod def get_measures(self): - """Get measures from the lidar as a list of 3-tuples (quality, angle degrees, distance meter)""" + """Get measures from the lidar as a list of 3-tuples (quality, angle degrees, distance millimeter)""" pass def test_Lidar(lidar, connection_attempts=3): import sys, time diff --git a/unified_frameworks/sensor_array/actual_lidar.py b/unified_frameworks/sensor_array/actual_lidar.py index 68115bf..9a8defb 100644 --- a/unified_frameworks/sensor_array/actual_lidar.py +++ b/unified_frameworks/sensor_array/actual_lidar.py @@ -3,13 +3,13 @@ import traceback try: - from sensor_array.LidarClass import Lidar + from sensor_array.LidarClass import _Lidar except ModuleNotFoundError: import sys import re sys.path.append((next(re.finditer(".*unified_frameworks", __file__)).group())) - from sensor_array.LidarClass import Lidar + from sensor_array.LidarClass import _Lidar from rplidar import RPLidar, RPLidarException import serial.tools.list_ports from serial.serialutil import PortNotOpenError @@ -29,7 +29,7 @@ def getDevicePort(): return None -class ActualLidar(Lidar): +class ActualLidar(_Lidar): def __init__(self, port=getDevicePort()) -> None: self.serial_port = port self.measures = [] diff --git a/unified_frameworks/sensor_array/bridge_lidar.py b/unified_frameworks/sensor_array/bridge_lidar.py index f166df3..1776708 100644 --- a/unified_frameworks/sensor_array/bridge_lidar.py +++ b/unified_frameworks/sensor_array/bridge_lidar.py @@ -3,7 +3,7 @@ root = (next(re.finditer(".*unified_frameworks", __file__)).group()) sys.path.append(root) if root not in sys.path else None from sensor_array.actual_lidar import ActualLidar -from sensor_array.LidarClass import Lidar +from sensor_array.LidarClass import _Lidar from bridge import rover_side, client_side from bridge.exceptions import NoOpenBridgeException from unified_utils import Service @@ -13,7 +13,7 @@ config = { "update_frequency": 20, #Hz } -class BridgeLidar(Lidar): +class BridgeLidar(_Lidar): PATH = '/lidar' def __init__(self) -> None: if rover_side.bridge_is_up(): diff --git a/unified_frameworks/sensor_array/fake_lidar.py b/unified_frameworks/sensor_array/fake_lidar.py index f69becf..c6ba2f4 100644 --- a/unified_frameworks/sensor_array/fake_lidar.py +++ b/unified_frameworks/sensor_array/fake_lidar.py @@ -8,13 +8,13 @@ import sys import re sys.path.append((next(re.finditer(".*unified_frameworks", __file__)).group())) -from sensor_array.LidarClass import Lidar +from sensor_array.LidarClass import _Lidar from unified_utils import Service, polar_sum config = { "verbose":False } -class FakeLidar(Lidar): +class FakeLidar(_Lidar): def __init__(self, points=100, angular_rate=1, translational_rate=1, jitter=0, noise=0, empty_scans=False) -> None: self.n = points angles = np.linspace(0, 360, self.n) diff --git a/unified_frameworks/sensor_array/lidar.py b/unified_frameworks/sensor_array/lidar.py index df76e09..839c523 100644 --- a/unified_frameworks/sensor_array/lidar.py +++ b/unified_frameworks/sensor_array/lidar.py @@ -6,14 +6,15 @@ from sensor_array.bridge_lidar import BridgeLidar import importlib from sensor_array.fake_lidar import FakeLidar -from sensor_array.LidarClass import Lidar +from sensor_array.LidarClass import _Lidar import traceback from threading import Thread -from math import pi, cos, sin, sqrt, atan2 +from math import pi, cos, sin, sqrt, atan2, radians import time import json import serial.tools.list_ports import serial +from unified_utils import _Abstract_Service, polar_dis config = { @@ -27,249 +28,89 @@ "service_event_verbose":True, "verbose_lidar_exceptions":True, "lidar_port": "ws://192.168.1.130:8765", #getDevicePort(), - "wireless_uri": "ws://192.168.1.130:8765" + "wireless_uri": "ws://192.168.1.130:8765", + "verbose": True } - -_point_clouds = None # This will be the raw point cloud -_obstacles = None # This will be the points clustered into obstacles -def run_lidar(service_is_active): - # if config["lidar_port"] is None and not config['use_fake_lidar']: - # print("Port not found!") - # return - - if config["service_event_verbose"]: - print("Starting Lidar Service") - #================================== - # Initial connect and set up Lidar - #---------------------------------- - lidar = None - for _lidar in config['lidar_preference']: - print(f"Trying to use {_lidar}") - try: - L:Lidar = _lidar() - if L.connect(verbose_attempts=True): - lidar = L - break - except: - pass - - if lidar is None: - print("None of the Lidars could connect") - sys.exit(1) +class NoLidarException(Exception): + def __init__(self, *args: object) -> None: + super().__init__(*args) + +class Lidar(_Abstract_Service): + def __init__(self, preference=config['lidar_preference']) -> None: + """Attempt to connect to the lidars listed in the preference and hold on + to the lidar that connected. Throws ``NoLidarException`` if all lidars + fail to connect. + + """ + super().__init__() + if config["verbose"]: print("Initializing Lidar") + lidar = None + for _lidar in preference: + print(f"Trying to use {_lidar}") + try: + L:_Lidar = _lidar() + if L.connect(verbose_attempts=True): + lidar = L + break + except: + pass + + if lidar is None: + raise NoLidarException(f"Failed to connect on the following lidars {preference}") + self._lidar: _Lidar = lidar + self._lidar.disconnect() + def start_service(self): + """Connect to the Lidar and start scanning""" + self._lidar.connect() + def stop_service(self): + """Stop scanning and disconnect from the lidar""" + self._lidar.disconnect() + + def get_point_clouds(self): + """ + Get a list of point Clouds of from the previous few Lidar scans. Each + point cloud is a list of ``(radians, meters)`` + + """ + return [[(radians(deg),mm/1000) for q,deg,mm in self._lidar.get_measures()]] - # PORT_NAME = config['lidar_port'] - # lidar = None - # lidar_iter = None - # ts = time.time() - # while service_is_active(): - # if config['service_event_verbose'] and time.time()-ts > 1: - # ts = time.time() - # try: # Try and try again until Lambs become lions and - # lidar = Lidar(config["wireless_uri"]) - # # lidar_iter = iter(lidar.iter_scans(max_buf_meas=10000)) # Large buffer is able to hold on to data for longer before yielding it. This means the data received can older (Therefore laggier) - # lidar_iter = iter(lidar.iter_scans()) # Stick to the default buffer size - # next(lidar_iter) # and until the Lidar is able to yield stuff without errors - # except RPLidarException as e: - # if config['verbose_lidar_exceptions']: - # print("======================RPLidarException================================") - # print(e) - # lidar.disconnect() - # continue - # except: - # print("Lidar Service Failed before lidar could start") - # print(traceback.format_exc()) - # sys.exit(1) - # print() - # break - #==================================== - - #==================================== - # Running the Lidar - #------------------------------------ - scanned_data = list() # buffer to hold distance data # use same ters scan/measures/ - def set_scan(distances): - "Sets and array of (signal quality, angle (rad), distance (m))" - nonlocal scanned_data - scanned_data = distances #[(sq, a, m) for sq, a, m in distances if m > config['rover_radius']] - # def get_buffers(polar_point): - # angles = [i*2*pi/config["point_buffer_count"] for i in range(config['point_buffer_count'])] - # cart_b = [(config['point_buffer_meters']*cos(a), config['point_buffer_meters']*sin(a)) for a in angles] - # point_cart = (polar_point[1]*cos(polar_point[0]), polar_point[1]*sin(polar_point[0])) - # buffs_cart = [(point_cart[0]+b[0], point_cart[1]+b[1]) for b in cart_b] - # buff_polar = [(atan2(b[1],b[0])%(2*pi), sqrt(b[0]**2+b[1]**2)) for b in buffs_cart] - # return buff_polar - - def get_buffers(polar_point): - angles = [i*2*pi/config["point_buffer_count"] for i in range(config['point_buffer_count'])] - cart_b = [(config['point_buffer_meters']*cos(a), config['point_buffer_meters']*sin(a)) for a in angles] - point_cart = (polar_point[1]*cos(polar_point[0]), polar_point[1]*sin(polar_point[0])) - buffs_cart = [(point_cart[0]+b[0], point_cart[1]+b[1]) for b in cart_b] - buff_polar = [(atan2(b[1],b[0])%(2*pi), sqrt(b[0]**2+b[1]**2)) for b in buffs_cart] - return buff_polar - - def in_sector(angle): - a, b = config['open_sector'] - a %= 2*pi - b %= 2*pi - return (angle-a)%(2*pi) < (b-a)%(2*pi) - def get_scan(flipped=True): - "Gets and array of (signal quality, angle (rad), distance (m))" - sign = -1 if flipped else 1 - distances = [(quality, ((sign*angle_deg)%360)*2*pi/360, distance_mm/1000.0) for quality, angle_deg, distance_mm in scanned_data] - # distances = [(q,a,m) for q,a,m in distances if (m > config['rover_radius'] or (a > config['open_sector'][0] and a < config['open_sector'][1]))] - distances = [(q,a,m) for q,a,m in distances if (m>config["rover_radius"] or in_sector(a))] - buffers = sum([ [ (q,ab,mb) for ab, mb in get_buffers((a,m))] for q,a,m in distances], []) - res = [] - while distances and buffers: - ls = distances if distances[-1] > buffers[-1] else buffers - res.append(ls.pop()) - rem = distances if distances else buffers - return res + rem - - def look(): - """Fetch new distance data and update the buffer""" - set_scan(lidar.get_measures()) - def spin(stop_check): - """Keep fetching and updating buffer with distance data""" - while not stop_check(): # You will notice there is no sleep to rest between loops. - look() # Thats cuz next(lidar_iter) is blocking for a while. - time.sleep(1/4) # Also cuz we are literally trying to consume as fast as possible - # to prevent data build up in buffer - _stop_spinning=False - lidar_thread = Thread(target=spin, args=(lambda: _stop_spinning, ), name="Lidar_consumption_thread") - lidar_thread.start() - if config['service_event_verbose']: - print("started Lidar Scanning Thread") - #==================================== - - #====================================== - # Maintaining Measurements with History - #-------------------------------------- - history_size = config['history_size'] - measurement_history = [[]]*history_size # A list of history_size empty lists - def update_measurement(): - nonlocal measurement_history - measurement_history = [get_scan(),*measurement_history[:-1]] - - #------------------------------------------- - # Joining Neighboring points into obstacles - # Maintain Obstacles in a thread (maybe I've created too many threads) - - thresh = config['point_buffer_meters'] # In meters - obstacles = None # List of obstacles - # Each obstacle is a list of points - # None if not initialized - def calc_polar_distance(p1, p2): - d1 = sqrt(abs(p1[1]**2 + p2[1]**2 - 2*p1[1]*p2[1]*cos(p1[0]-p2[0]))) - return d1 - def join_points(points): - piter = iter(points) + def get_obstacles(self, thresh=1): + """ + Get a list of obstacles where each obstacle is a point cloud of the + obstacle + + :param thresh: Points closer than the threshold distance are grouped in + the same obstacle + + """ + measures = sum(self.get_point_clouds(), []) # use [].extend(measurement for clarity) + angle_sorted_measures = sorted(measures, key=lambda i: i[0]) + + piter = iter(angle_sorted_measures) try: groups = [[next(piter)]] except StopIteration: return None for p in piter: - if calc_polar_distance(groups[-1][-1], p) > thresh: # last group's last point + if polar_dis(groups[-1][-1], p) > thresh: # last group's last point groups.append([]) groups[-1].append(p) - if calc_polar_distance(groups[0][0], groups[-1][-1]) < thresh and len(groups) > 1: + if polar_dis(groups[0][0], groups[-1][-1]) < thresh and len(groups) > 1: groups[0] = groups[-1] + groups[0] groups.pop() + return groups - #---------------------------------------------------------- - # Start up the thread to maintain point cloud and obstacles - - def keep_updating_obstacles(stop_updating): - nonlocal obstacles - while not stop_updating(): - time.sleep(1/config["update_frequency"]) - update_measurement() - measures = sum(measurement_history, []) # use [].extend(measurement for clarity) - angle_sorted_measures = sorted(measures, key=lambda i: i[1]) - obstacles = join_points(((a,d) for q,a,d in angle_sorted_measures)) #quality, angle, distance - stop_updating_obstacles = False - obstacle_thread = Thread(target=keep_updating_obstacles, args=(lambda: stop_updating_obstacles, ), name="Obstacle_maintaining_thread") - obstacle_thread.start() - - - # This could probably be put in the same thread as obstacle thread - # i = 0 - global _point_clouds, _obstacles - while service_is_active(): - _point_clouds = [ - [(a,d) for q,a,d in measures] - for measures in reversed(measurement_history) - ] - _obstacles = obstacles - # with(open(config["service_file"], 'w')) as f: - # f.write(json.dumps(obstacles)) - # print(i) - # i+=1 - time.sleep(1/config['update_frequency']) - # os.remove(config["service_file"]) - - - stop_updating_obstacles=True - if config['service_event_verbose']: - print("waiting for obstacle thread to join") - obstacle_thread.join() - _stop_spinning=True - if config['service_event_verbose']: - print("waiting for lidar thread to join") - lidar_thread.join() - # lidar.stop() - # lidar.stop_motor() - # lidar.disconnect() - lidar.disconnect() - if config["service_event_verbose"]: - print("Stopping Lidar Service") - -def get_point_clouds(): - """ - Get None or the past history_size number of point clouds measured by the lidar - where each point is represented by (radians, meter) - The history is returned most recent first - """ - return _point_clouds -def get_obstacles(): - """ - Get None or a list of obstacles recognized by the lidar. - Obstacles will be a list of (radian, meter) points. - """ - return _obstacles - - - -_thread = None -_running = False -def start_lidar_service(): - if config['service_event_verbose']: - print("Called start_lidar_service") - global _thread, _running - if _running: - raise Exception("Tried to Start Lidar Service but it was already running.") - _thread = Thread(target=run_lidar, args=(lambda: _running,), name="Lidar Service Thread") - _running = True - _thread.start() - -def stop_lidar_service(): - if config['service_event_verbose']: - print("Called stop_lidar_service") - global _running, _thread - _running = False - _thread.join() - -def lidar_service_is_running(): - return _running + pass if __name__=='__main__': - start_lidar_service() + l = Lidar() + l.start_service() time.sleep(10) for _ in range(7): print() - print(get_point_clouds()) + print(len(l.get_point_clouds()[0]), len(l.get_obstacles())) time.sleep(1) - stop_lidar_service() + l.stop_service() diff --git a/unified_frameworks/sensor_array/lidar_visualizer.py b/unified_frameworks/sensor_array/lidar_visualizer.py index 1786a21..3fc5df4 100644 --- a/unified_frameworks/sensor_array/lidar_visualizer.py +++ b/unified_frameworks/sensor_array/lidar_visualizer.py @@ -4,12 +4,14 @@ import json import sys # print(sys.path) -import lidar +import lidar as L import time +import traceback # lidar.config[''] if __name__=='__main__': - lidar.start_lidar_service() + lidar = L.Lidar() + lidar.start_service() # time.sleep(20) try: fig = plt.figure() @@ -40,7 +42,8 @@ def update_plot(_): anime = anim.FuncAnimation(fig, update_plot, 1, interval=50, blit=True) plt.show() - except: - pass - lidar.stop_lidar_service() + except Exception as e: + print(e) + traceback.print_exc() + lidar.stop_service() diff --git a/unified_frameworks/unified_utils.py b/unified_frameworks/unified_utils.py index 051d210..c4075e8 100644 --- a/unified_frameworks/unified_utils.py +++ b/unified_frameworks/unified_utils.py @@ -3,27 +3,38 @@ import time, json from math import sqrt, cos, atan2, sin, pi import numpy as np +from abc import ABC, abstractmethod config = { 'time_analysis': True, 'decimal_precision': 5, } -class Service(): - def __init__(self, service_func, name, *args, **kwargs) -> None: +class _Abstract_Service(ABC): + @abstractmethod + def start_service(self): + pass + @abstractmethod + def stop_service(self): + pass + +class Service(_Abstract_Service): + def __init__(self, service_func, name) -> None: """ Create an easy to start/stop service from a function. The function must accept a callable which returns if the service is still running. That callable must be respected to prevent the service from running amok. """ self._running = False - self._thread = Thread(target=service_func, args=(lambda: self._running, *args), kwargs=kwargs, name=name) + self._service_func = service_func + self._thread = Thread(target=service_func, args=(lambda: self._running,), name=name) def start_service(self): self._running=True self._thread.start() def stop_service(self): self._running=False self._thread.join() + self._thread = Thread(target=self._service_func, args=(lambda: self._running,), name=self._thread.name) def is_running(self): return self._running