Improve follow me (used on robot Pat) (#999)

* osgar/drivers/vanjee.py - disable asserts
* osgar/followme.py - parametrize max_speed, max_dist_limit and desired_dist
* osgar/followme.py - simplify rules calculation
* add draw option into followme.py replay

osgar/drivers/vanjee.py

@@ -28,11 +28,11 @@ def on_raw(self, data):
         if self.last_frame is not None:
             assert (self.last_frame + 1) & 0xFFFF == frame, (self.last_frame, frame)
         self.last_frame = frame
-        assert data[6:8] == bytes.fromhex('0000'), data[6:10].hex()  # reserved
+#        assert data[6:8] == bytes.fromhex('0000'), data[6:10].hex()  # reserved
         # reserved [8:10]  - variable
-        assert data[11:14] == bytes.fromhex('02 190C'), data[11:14].hex()
+#        assert data[11:14] == bytes.fromhex('02 190C'), data[11:14].hex()
         # reserved [14:16]
-        assert data[16:18] == bytes.fromhex('01 04'), data[16:18].hex()
+#        assert data[16:18] == bytes.fromhex('01 04'), data[16:18].hex()
         bank, motor_speed = struct.unpack_from('>BH', data, 18)
         assert 1 <= bank <= 16, bank
 #        assert 38000 <= motor_speed <= 39300, motor_speed  # disabled as it can be easily "of of range"

osgar/followme.py

@@ -33,13 +33,17 @@ def __init__(self, config, bus):
         self.last_scan = None
         self.scan_size = config.get('scan_size', 271)
         self.scan_fov_deg = config.get('scan_fov_deg', 270)
+        self.max_speed = config.get('max_speed', 0.5)  # m/s
+        self.max_dist_limit = config.get('max_dist_limit', 1.3)  # m
+        self.desired_dist = config.get('desired_dist', 0.4)  # m
+        self.debug_arr = []
 
     def on_pose2d(self, data):
         self.last_position = data
 
     def on_scan(self, data):
         if self.verbose:
-            print(min_dist(data) / 1000.0)
+            print(self.time, 'min_dist', min_dist(data) / 1000.0)
         self.last_scan = data
 
     def on_emergency_stop(self, data):
@@ -100,19 +104,26 @@ def followme(self):
                 maxnear = min( (x for x in self.last_scan if x > CLOSE_REFLECTIONS) ) / 1000.0
 
                 if self.verbose:
-                    print(near, maxnear, index)
+                    print(self.time, 'near', near, maxnear, index)
 
-                if near > 1.3 or any(x < thresh for (x, thresh) in zip(self.last_scan, thresholds) if x > CLOSE_REFLECTIONS):
-                    self.send_speed_cmd(0.0, 0.0)
+                if near > self.max_dist_limit or any(x < thresh for (x, thresh) in zip(self.last_scan, thresholds) if x > CLOSE_REFLECTIONS):
+                    speed, rot, angle = 0, 0, None
                 else:
                     angle = math.radians(self.scan_fov_deg * index/SCAN_SIZE - self.scan_fov_deg/2) + masterAngleOffset
                     desiredAngle = 0
-                    desiredDistance = 0.4
-                    speed = 0.2 + 2 * (near - desiredDistance)
-                    rot = 1.5 * (angle - desiredAngle)
+                    desiredDistance = self.desired_dist
+#                    speed = 0.2 + 2 * (near - desiredDistance)
+                    speed = near - desiredDistance
+#                    rot = 1.5 * (angle - desiredAngle)
+                    rot = angle - desiredAngle
                     if speed < 0:
                         speed = 0
-                    self.send_speed_cmd(speed, rot)
+                    if speed > self.max_speed:
+                        speed = self.max_speed
+                if self.verbose:
+                    print(self.time, 'speed', speed, angle, rot)
+                    self.debug_arr.append((self.time.total_seconds(), angle, near, speed))
+                self.send_speed_cmd(speed, rot)
 
                 self.last_scan = None
             self.update()
@@ -127,6 +138,32 @@ def run(self):
             self.send_speed_cmd(0.0, 0.0)
             self.wait(timedelta(seconds=1))
 
+    def draw(self):
+        # https://www.perplexity.ai/search/how-to-draw-with-matplotlib-in-SiM5q62xQIGSni3t0OUpYw
+        import matplotlib.pyplot as plt
+
+        x, y1, y2, y3 = map(list, zip(*self.debug_arr))
+
+        # Create a figure with 3 subplots stacked vertically
+        fig, (ax1, ax2, ax3) = plt.subplots(3, 1, figsize=(8, 8))
+
+        # Plot the first graph
+        ax1.plot(x, [math.degrees(tmp) if tmp is not None else None for tmp in y1])
+        ax1.set_title('Angle (deg)')
+
+        # Plot the second graph
+        ax2.plot(x, y2)
+        ax2.set_title('Distance (m)')
+
+        # Plot the third graph
+        ax3.plot(x, y3)
+        ax3.set_title('Speed (m/s)')
+
+        # Adjust the spacing between subplots
+        plt.subplots_adjust(hspace=0.5)
+
+        plt.show()
+
 
 if __name__ == "__main__":
     from osgar.launcher import launch

osgar/followpath.py

@@ -4,8 +4,10 @@
 import math
 
 from osgar.lib.route import Route as GPSRoute, DummyConvertor
+from osgar.lib.line import Line
 from osgar.lib.mathex import normalizeAnglePIPI
 from osgar.node import Node
+from osgar.bus import BusShutdownException
 from osgar.followme import EmergencyStopException
 
 
@@ -26,6 +28,7 @@ def __init__(self, config, bus):
         self.max_speed = config.get('max_speed', 0.2)
         self.raise_exception_on_stop = False
         self.verbose = False
+        self.finished = False
 
     def control(self, pose):
         """
@@ -39,16 +42,28 @@ def control(self, pose):
         Note, that there is no correction based on signed distance from route.
         """
         first, second = self.route.routeSplit(pose[:2])
-        if len(second) <= 1:
+        if len(second) <= 1 or self.finished:
+            self.finished = True
             return 0, 0
         pt = Route(second).pointAtDist(dist=0.2)  # maybe speed dependent
         pt2 = Route(second).pointAtDist(dist=0.2+0.1)
         if math.hypot(pt2[1]-pt[1], pt2[0]-pt[0]) < 0.001:
             # at the very end, or not defined angle
+            self.finished = True
             return 0, 0
         angle = math.atan2(pt2[1]-pt[1], pt2[0]-pt[0])
+        # force robot to move towards original path
+        line = Line(pt, pt2)
+        signed_dist = line.signedDistance(pose)
+        if signed_dist > 1.0:
+           signed_dist = 1.0
+        elif signed_dist < -1.0:
+            signed_dist = -1.0
+        if abs(normalizeAnglePIPI(angle - pose[2])) < math.radians(45):
+            # force correction only if the robot is more-or-less pointing into right direction
+            angle -= math.radians(45) * signed_dist
         if self.verbose:
-            print(self.time, second, pt, angle)
+            print(self.time, second[:5], pt, angle, signed_dist, normalizeAnglePIPI(angle - pose[2]))
         return self.max_speed, normalizeAnglePIPI(angle - pose[2])
 
     def on_pose2d(self, data):