From 65c19e7494f82363542d6d1243b0ed174b590021 Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Wed, 15 Apr 2020 21:41:09 +0930
Subject: [PATCH] Lidar fixes

---
 tracking/algorithms.py     | 10 +++++-----
 tracking/lidar_servicer.py |  3 ++-
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/tracking/algorithms.py b/tracking/algorithms.py
index cfdf658..6efeff5 100644
--- a/tracking/algorithms.py
+++ b/tracking/algorithms.py
@@ -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):
diff --git a/tracking/lidar_servicer.py b/tracking/lidar_servicer.py
index 44debbb..441b141 100644
--- a/tracking/lidar_servicer.py
+++ b/tracking/lidar_servicer.py
@@ -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