Add random moving func

.gitignore

@@ -1,3 +1,4 @@
 __pycache__/
 .ipynb_checkpoints/
 .vscode/
+*.ipynb

Robot.py

@@ -24,6 +24,7 @@ class Robot:
         self.id = id
         self.log_head = f"{self.name}_{self.id}"
         self.position = position
+        # facing direction
         self.ang = np.random.random()*360
         self.desierd_position = self.position
         self.in_position = True

Simsim.py

@@ -72,8 +72,8 @@ class Simsim:
             self.plt_pm.cla()
             self.plt_sim.set_xlim(0, self.map_size[0])
             self.plt_sim.set_ylim(0, self.map_size[1])
-            self.plt_pm.set_xlim(2000, 4000)
-            self.plt_pm.set_ylim(2000, 4000)
+            self.plt_pm.set_xlim(10000, 20000)
+            self.plt_pm.set_ylim(10000, 20000)
 
             if ground_truth:
                 for target in self.targets:
@@ -102,8 +102,9 @@ class Simsim:
                     self.plt_sim.scatter(det_pos[0], det_pos[1],
                                          marker='^', s=2)
                     for ind in t.prob_map.prob_map:
-                        new_ind = np.array(ind) # - self.map_size
+                        new_ind = np.array(ind)  # - self.map_size
                         self.plt_pm.scatter(
                             new_ind[0], new_ind[1], marker='s', s=1, c='r', alpha=t.prob_map.prob_map[ind])
-                        self.plt_pm.annotate(f"{t.prob_map.prob_map[ind]:.3e}", (new_ind[0], new_ind[1]+5))
+                        self.plt_pm.annotate(f"{t.prob_map.prob_map[ind]:.3e}", (new_ind[0], new_ind[1]+5),
+                                             fontsize=2)
             plt.pause(1/self.rate)

main.py

@@ -4,18 +4,17 @@ import logging
 
 if __name__ == "__main__":
     s = Simsim()
-    s.rate = 5
+    s.rate = 15
     s.add_tracker('tracker', np.array([500, 300]), sensor_rad=250)
     s.add_tracker('tracker', np.array([300, 500]), sensor_rad=250)
     s.add_tracker('tracker', np.array([700, 500]), sensor_rad=250)
-    # s.add_tracker('tracker', np.array([700, 580]), sensor_rad=200)
-    # s.add_tracker('tracker', np.array([620, 220]), sensor_rad=450)
-    # s.add_target('tgt', np.array([150, 50]))
+    s.add_tracker('tracker', np.array([620, 220]), sensor_rad=150)
+    s.add_target('tgt', np.array([150, 50]))
     s.add_target('tgt', np.array([500, 490]))
-    # s.add_target('tgt', np.array([180, 500]))
+    s.add_target('tgt', np.array([600, 290]))
+    s.add_target('tgt', np.array([180, 500]))
     s.add_target('tgt', np.array([500, 300]))
     s.add_target('tgt', np.array([300, 500]))
-    # s.add_target('tgt', np.array([630, 500]))
-    # s.add_edges([[0, 1], [0, 2], [3, 2], [1, 3], [1, 4], [3, 4]])
-    s.add_edges([[0, 1], [1, 2], [0, 2]])
+    s.add_target('tgt', np.array([630, 500]))
+    s.add_edges([[0, 1], [1, 2], [0, 2], [3, 0]])
     s.run(logging.DEBUG, ground_truth=True)

tracker.py

@@ -14,6 +14,8 @@ class Sensor:
         # host tracker
         self.tracker = tracker
         self.coverage_radius = coverage_radius
+        # the sensor's condidence will multiplied by this factor
+        self.health = 1.0
 
     def get_detection(self):
         all_targets = self.tracker.simulator.targets
@@ -49,10 +51,10 @@ class Tracker(Robot):
 
         self.area_width = 2000  # meter
         self.area_height = 2000  # meter
-        self.resolution = 0.5  # meter
+        self.resolution = 0.1  # meter
         self.prob_map = ProbMap(self.area_width, self.area_height, self.resolution,
                                 center_x=0.0, center_y=0.0, init_val=0.05,
-                                false_alarm_prob=0.01)
+                                false_alarm_prob=0.05)
 
         self.observations = dict()  # type: dict[tuple]
         self.shareable_v = ProbMapData()
@@ -115,8 +117,18 @@ class Tracker(Robot):
             output_detection[id_counter] = transformed_detection
             id_counter += 1
         self.observations = output_detection
+    
+    def random_moving(self):
+        # if already reached the previous waypoint
+        if self.waypoint_ctrl():
+            self.ang = (self.ang + np.deg2rad(np.random.randint(-360, 360))) % 360
+            rot_mat = np.asarray([[np.cos(self.ang), -np.sin(self.ang)],
+                                  [np.sin(self.ang), np.cos(self.ang)]])
+            self.waypoint_ctrl(speed=20, desierd_pos=(self.position
+                                                      - np.dot([0, 10], rot_mat)))
 
     def job(self):
+        self.random_moving()
         self.sensing()
         logging.debug(f"{self.log_head} OBSERVATION: {self.observations}")
         shareable_v = self.prob_map.generate_shareable_v(
@@ -148,7 +160,7 @@ class Tracker(Robot):
         self.prob_map.consensus(neighbors_map)
 
         self.target_estimates = self.prob_map.get_target_est(
-            0.6, normalization=True)
+            0.6, normalization=False)
         logging.debug(
             f"{self.name}_{self.id} ProbMap: {self.prob_map.prob_map}")
         # print(target_estimates)