Move root car to pycar, put other pycar back to car.

pycar/src/car/tracking/algorithms.py

@@ -0,0 +1,212 @@
+import math
+
+
+class Group:
+
+    def __init__(self, number, points=[]):
+        self._points = points
+        self._number = number
+        self._minX = None
+        self._maxX = None
+        self._minY = None
+        self._maxY = None
+
+    def add_point(self, point):
+        self._points.append(point)
+        self._update_min_max(point)
+
+    def get_points(self):
+        return self._points
+
+    @property
+    def number(self):
+        return self._number
+
+    @number.setter
+    def number(self, number):
+        self._number = number
+
+    def _update_min_max(self, new_point):
+        """
+        Updates the min and max points for this group.
+        This is to determine when assigning groups whether the
+        same group is selected.
+        """
+        converted_point = convert_lidar_to_cartesian(new_point)
+
+        if self._minX is None or self._minX > converted_point[0]:
+            self._minX = converted_point[0]
+
+        if self._maxX is None or self._maxX < converted_point[0]:
+            self._maxX = converted_point[0]
+
+        if self._minY is None or self._minY > converted_point[1]:
+            self._minY = converted_point[1]
+
+        if self._maxY is None or self._maxY < converted_point[1]:
+            self._maxY = converted_point[1]
+
+    def get_minX(self):
+        return self._minY
+
+    def get_maxX(self):
+        return self._maxY
+
+    def get_minY(self):
+        return self._minY
+
+    def get_maxY(self):
+        return self._maxY
+
+
+def convert_lidar_to_cartesian(new_point):
+    x = new_point[2] * math.sin(new_point[1])
+    y = new_point[2] * math.cos(new_point[1])
+    return (x, y)
+
+
+def convert_cartesian_to_lidar(x, y):
+    """
+    Converts a point on the grid (with car as the origin) to a lidar tuple (distance, angle)
+
+    Parameters
+    ----------
+    x
+        Horizontal component of point to convert.
+
+    y
+        Vertical component of point to convert.
+
+    Returns
+    -------
+    converted
+        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 = 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):
+    """
+    Calculates groups of points from a lidar scan. The scan should
+    already be sorted.
+
+    Parameters
+    ----------
+
+    scan: Iterable
+        The lidar scan data to get groups of.
+        Should be of format: (quality, angle, distance)
+
+    Returns
+    -------
+    list
+        List of groups that were found.
+    """
+    prevPoint = None
+    currentGroup = None
+    allGroups = []
+    currentGroupNumber = 0
+
+    # assume the list is already sorted.
+    for point in scan:
+        if prevPoint is None:
+            prevPoint = point
+            continue
+
+        # Distances are in mm.
+        # within 1cm makes a group. Will need to play around with this.
+        if (point[2] - prevPoint[2]) ** 2 < 10 ** 2:
+            if currentGroup is None:
+                currentGroup = Group(currentGroupNumber)
+                allGroups.append(currentGroup)
+            currentGroup.add_point(point)
+        else:
+            if currentGroup is not None:
+                currentGroupNumber += 1
+                currentGroup = None
+
+        prevPoint = point
+
+    return allGroups
+
+
+def find_centre(group):
+    """
+    Gets a tuple (x,y) of the centre of the group.
+
+    Parameters
+    ----------
+    group: Group
+        A group of points to find the centre of.
+
+    Returns
+    -------
+    tuple (x,y)
+        The centre in the form of a tuple (x,y)
+    """
+    return ((group.get_maxX() + group.get_minX()) / 2, (group.get_maxY() + group.get_minY()) / 2)
+
+
+def assign_groups(prev_groups, new_groups):
+    """
+    Assigns group numbers to a new scan based on the groups of an old scan.
+    """
+    for group in prev_groups:
+        old_centre = find_centre(group)
+        for new_group in new_groups:
+            new_centre = find_centre(new_group)
+            # They are considered the same if the new group and old group centres are within 5cm.
+            if ((new_centre[0] - old_centre[0]) ** 2 + (new_centre[1] - old_centre[1]) ** 2) < 50 ** 2:
+                new_group.number = group.number
+
+    return new_groups
+
+
+def updateCarVelocity(oldGroup, newGroup):
+    """
+    Return a tuple (DistanceChange, AngleChange) indicating how the tracked groups have changed, which can
+    be used to then update the steering/throttle of the car (or other vehicle that
+    may be used)
+
+    Parameters
+    ----------
+    oldGroup: Group
+        The positioning of points for the group in the last scan.
+
+    newGroup: Group
+        The positioning of points for the group in the latest scan.
+
+    Returns
+    -------
+    tuple (DistanceChange, AngleChange)
+        A tuple containing how the groups' centres changed in the form (distance,angle)
+    """
+    old_polar = convert_cartesian_to_lidar(*find_centre(oldGroup))
+    new_centre = convert_cartesian_to_lidar(*find_centre(newGroup))
+    return (new_centre[0] - old_polar[0], new_centre[1] - old_polar[1])
+
+
+def dualServoChange(newCentre, changeTuple):
+    """
+    Gets a tuple (throttleChange, steeringChange) indicating the change that should be applied to the current
+    throttle/steering of an rc car that uses dual servos.
+
+    Parameters
+    ---------
+    newCentre
+        Tuple (distance, angle) of the new centre of the tracked group.
+
+    changeTuple
+        Tuple (distanceChange, angleChange) from the old centre to the new centre.
+
+    Returns
+    -------
+    tuple
+        Tuple of (throttleChange, steeringChange) to apply to the 2 servos.
+    """
+    return ((changeTuple[0] / 3) - (newCentre[0] / 4) + 1, 0)