vato007/picar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Lidar fixes

Browse Source
This commit is contained in:
Piv
2020-04-15 21:41:09 +09:30
parent e09465e33c
commit 65c19e7494
2 changed files with 7 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
tracking/algorithms.py
View File

@@ -83,11 +83,11 @@ def convert_cartesian_to_lidar(x, y):
A tuple (distance, angle) that represents the point. Angle is in degrees.
"""
# Angle depends on x/y position.
# if x is positive and y is positive, then angle = 90 - tan-1(y/x)
# if x is positive and y is negative, then angle = 90 + tan-1(y/x)
# if x is negative and y is positive, then angle = 270 + tan-1(y/x)
# if x is negative and y is negative, then angle = 270 - tan-1(y/x)
return (math.sqrt(x ** 2 + y ** 2), math.degrees(math.atan(y/x) + (180 if ))
# if x is positive and y is positive, then angle = tan-1(y/x)
# if x is positive and y is negative, then angle = 360 + tan-1(y/x)
# if x is negative and y is positive, then angle = 180 + tan-1(y/x)
# if x is negative and y is negative, then angle = 180 + tan-1(y/x)
return (math.sqrt(x ** 2 + y ** 2), math.degrees(math.atan(y/x)) + (180 if x < 0 else 270 if y < 0 else 0))
def calc_groups(scan):

3
tracking/lidar_servicer.py
View File

@@ -4,13 +4,14 @@ from tracking.lidar_cache import LidarCache
from multiprocessing import Process
import messaging.message_factory as mf
from rplidar import RPLidar
from Messaging import messages
from messaging import messages
class LidarServicer(PersonTrackingServicer):
def __init__(self):
# TODO: Put the rplidar creation in a factory or something, to make it possible to test this servicer.
# Also, it would allow creating the service without the lidar being connected.
self.cache = LidarCache(RPLidar('/dev/ttyUSB0'), measurements=100)
self._mFactory = None
self._port = None