Finish waypoint ctrl

.gitignore

@@ -0,0 +1,2 @@
+__pycache__/
+.ipynb_checkpoints/

Simsim.py

@@ -2,6 +2,7 @@ import matplotlib.pyplot as plt
 import numpy as np
 from target import Target
 from tracker import Tracker
+import logging
 
 
 class Simsim:
@@ -16,27 +17,53 @@ class Simsim:
         plt.ion()
         plt.axis('equal')
 
-    def add_tracker(self, name, position):
-        tracker = Tracker(name, len(self.trackers), position)
+    def add_tracker(self, name, position, sensor_rad):
+        tracker = Tracker(name, len(self.trackers),
+                          position, sensor_rad, self.rate)
         self.trackers.append(tracker)
 
     def add_edges(self, edges):
         self.edges.extend(edges)
         for e in edges:
-            self.trackers[e[0]].neighbor.append(e[1])
-            self.trackers[e[1]].neighbor.append(e[0])
+            self.trackers[e[0]].neighbor.add(e[1])
+            self.trackers[e[1]].neighbor.add(e[0])
 
     def add_target(self, name, position):
-        target = Target(name, len(self.targets), position)
+        target = Target(name, len(self.targets), position, self.rate)
         self.targets.append(target)
 
-    def run(self):
+    def _update_all(self):
+        """Simulate once, update all trackers and targets
+        """
+        for tracker in self.trackers:
+            tracker.job()
+        for target in self.targets:
+            target.job()
+        pass
+
+    def run(self, log_lvl=logging.WARN, ground_truth=False):
+        logging.basicConfig(format='%(asctime)s.%(msecs)03d %(levelname)s: %(message)s',
+                            datefmt='%m/%d/%Y %H:%M:%S', level=log_lvl)
+        logging.warning("The lvl is WARN")
         while 1:
+            self._update_all()
             plt.cla()
             plt.xlim(0, self.map_size[0])
             plt.ylim(0, self.map_size[1])
 
+            if ground_truth:
+                for target in self.targets:
+                    plt.scatter(
+                        target.position[0], target.position[1], marker='d', s=10, color='r')
+            for e in self.edges:
+                t0 = self.trackers[e[0]]
+                t1 = self.trackers[e[1]]
+                plt.plot([t0.position[0], t1.position[0]],
+                         [t0.position[1], t1.position[1]],
+                         linewidth=1, color='g', alpha=0.5)
+
             for t in self.trackers:
+                # draw the trackers
                 plt.scatter(t.position[0], t.position[1],
                             marker='s', s=20, c='b')
                 plt.annotate(t.id, (t.position[0], t.position[1]+10))
@@ -45,32 +72,26 @@ class Simsim:
                     (t.position), t.sensor.coverage_radius, fill=False, color='grey', alpha=0.3)
                 plt.gcf().gca().add_artist(circle)
                 # draw the detections
-                dets = t.sensor.get_detection(self.targets)
-                for det in dets:
-                    plt.gcf().gca().scatter(
-                        det[0], det[1], marker='^', s=8)  # , alpha=det[2]
-                t.run(self.rate)
-
-            for e in self.edges:
-                t0 = self.trackers[e[0]]
-                t1 = self.trackers[e[1]]
-                plt.plot([t0.position[0], t1.position[0]],
-                         [t0.position[1], t1.position[1]],
-                         linewidth=1, color='g', alpha=0.5)
-
+                dets_relative = t.sensor.get_detection(self.targets)
+                for det in dets_relative:
+                    det_abs = det+t.position
+                    plt.scatter(det_abs[0], det_abs[1],
+                                marker='^', s=8)  # , alpha=det[2]
             plt.draw()
             plt.pause(1/self.rate)
 
 
 s = Simsim()
-s.add_tracker('t', np.array([120.0, 180.0]))
-s.trackers[0].speed = np.array([10,10])
-s.add_tracker('t', np.array([220, 380]))
-s.trackers[1].speed = np.array([5,10])
-s.add_tracker('t', np.array([500, 380]))
-s.trackers[2].speed = np.array([-22,10])
+s.add_tracker('tracker', np.array([120.0, 180.0]), sensor_rad=100)
+# s.trackers[0].speed = np.array([10, 10])
+s.add_tracker('tracker', np.array([220, 380]), sensor_rad=150)
+# s.trackers[1].speed = np.array([5, 10])
+s.add_tracker('tracker', np.array([500, 380]), sensor_rad=200)
+s.add_tracker('tracker', np.array([700, 580]), sensor_rad=120)
+# s.trackers[2].speed = np.array([-22, 10])
 s.add_target('tgt', np.array([200, 300]))
 s.add_target('tgt', np.array([100, 300]))
 s.add_target('tgt', np.array([500, 300]))
-s.add_edges([[0, 1], [0, 2]])
-s.run()
+s.add_target('tgt', np.array([300, 500]))
+s.add_edges([[0, 1], [0, 2], [1,2], [0,3]])
+s.run(logging.INFO, True)

target.py

@@ -3,5 +3,5 @@ import numpy as np
 
 
 class Target(Robot):
-    def __init__(self, name: str, id: int, position: np.array) -> None:
-        super().__init__(name, id, position)
+    def __init__(self, name: str, id: int, position: np.array, rate) -> None:
+        super().__init__(name, id, position, rate)