trackers now using prob map but have issues

.gitignore

@@ -1,2 +1,3 @@
 __pycache__/
-.ipynb_checkpoints/
+.ipynb_checkpoints/
+.vscode/

Robot.py

@@ -0,0 +1,60 @@
+import logging
+
+import numpy as np
+
+
+class Robot:
+    def __init__(self, simulator,  name: str, id: int, position: np.array, rate: int) -> None:
+        """The base Robot class
+
+        Parameters
+        ----------
+        name : str
+            Robot's name
+        id : int
+            Robot's ID
+        position : np.array
+            Robot's start position
+        rate : int
+            Simulator's update rate
+        """
+        # host simulator
+        self.simulator = simulator
+        self.name = name
+        self.id = id
+        self.position = position
+        self.desierd_position = self.position
+        self.in_position = True
+        self.speed = 0.0
+        self.rate = rate
+        self.max_speed = 100.0  # m/s
+
+    def waypoint_ctrl(self, speed=None, desierd_pos: np.array = None):
+        # if got new waypoint, update
+        if not np.any(desierd_pos == None):
+            self.desierd_position = desierd_pos
+
+        if speed == None:
+            self.speed = self.max_speed
+        else:
+            if speed > self.max_speed:
+                logging.warning("Given speed is over maximum")
+            self.speed = speed
+
+        if np.any(self.position != self.desierd_position):
+            self.in_position = False
+            one_time_step_length = speed/self.rate
+            difference = self.desierd_position - self.position
+            distance = np.linalg.norm(difference)
+            # if it's close enough to the goal position, make it in position
+            if distance <= one_time_step_length:
+                self.position = self.desierd_position
+                self.in_position = True
+            else:
+                # move to the goal point at the given speed
+                direction = difference/distance
+                self.position = self.position + direction*self.speed/self.rate
+
+    def job(self):
+        self.waypoint_ctrl()
+        pass

Simsim.py

@@ -3,22 +3,45 @@ import numpy as np
 from target import Target
 from tracker import Tracker
 import logging
+from collections import deque
 
 
 class Simsim:
+    class Topics:
+        def __init__(self) -> None:
+            self.topics = dict()
+
+        def add_topic(self, topic_name):
+            self.topics[topic_name] = deque(maxlen=20)
+
+        def del_topic(self, topic_name):
+            try:
+                del self.topics[topic_name]
+            except:
+                pass
+
+        def publish(self, topic_name, msg):
+            self.topics[topic_name].append(msg)
+
+        def subs(self, topic_name):
+            return self.topics[topic_name]
+
     def __init__(self) -> None:
         self.trackers = []
         self.edges = []
         self.targets = []
         self.map_size = [1000, 1000]
         self.rate = 30
+        self.topics = self.Topics()
 
-        plt.figure(figsize=(8, 8))
         plt.ion()
         plt.axis('equal')
+        self.fig, (self.plt_sim, self.plt_pm) = plt.subplots(
+            1, 2, figsize=(10, 5),dpi=160)
+        
 
     def add_tracker(self, name, position, sensor_rad):
-        tracker = Tracker(name, len(self.trackers),
+        tracker = Tracker(self, name, len(self.trackers),
                           position, sensor_rad, self.rate)
         self.trackers.append(tracker)
 
@@ -29,7 +52,7 @@ class Simsim:
             self.trackers[e[1]].neighbor.add(e[0])
 
     def add_target(self, name, position):
-        target = Target(name, len(self.targets), position, self.rate)
+        target = Target(self, name, len(self.targets), position, self.rate)
         self.targets.append(target)
 
     def _update_all(self):
@@ -44,54 +67,43 @@ class Simsim:
     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])
+            self.plt_sim.cla()
+            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(0, self.map_size[0])
+            self.plt_pm.set_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')
+                    self.plt_sim.scatter(
+                        target.position[0], target.position[1], marker='x', s=2, 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)
+                self.plt_sim.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))
+                self.plt_sim.scatter(t.position[0], t.position[1],
+                                     marker='s', s=20, c='b')
+                self.plt_sim.annotate(t.id, (t.position[0], t.position[1]+10))
                 # draw the camera coverage
                 circle = plt.Circle(
                     (t.position), t.sensor.coverage_radius, fill=False, color='grey', alpha=0.3)
-                plt.gcf().gca().add_artist(circle)
-                # draw the detections
-                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)
+                self.plt_sim.add_patch(circle)
 
-
-s = Simsim()
-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_target('tgt', np.array([300, 500]))
-s.add_edges([[0, 1], [0, 2], [1,2], [0,3]])
-s.run(logging.INFO, True)
+                for est in t.target_estimates:
+                    det_pos = est[0:2]
+                    det_abs_pos = det_pos+t.position
+                    self.plt_sim.scatter(det_abs_pos[0], det_abs_pos[1],
+                                         marker='^', s=2)
+                    for ind in t.prob_map.prob_map:
+                        new_ind = np.array(ind) - self.map_size + t.position
+                        self.plt_pm.scatter(
+                            new_ind[0], new_ind[1], marker='s', s=1, c='r', alpha=t.prob_map.prob_map[ind])
+            plt.pause(1/self.rate)

main.py

@@ -0,0 +1,20 @@
+from Simsim import Simsim
+import numpy as np
+import logging
+
+if __name__ == "__main__":
+    s = Simsim()
+    s.rate = 5
+    s.add_tracker('tracker', np.array([200, 100]), sensor_rad=300)
+    s.add_tracker('tracker', np.array([100, 100]), sensor_rad=200)
+    s.add_tracker('tracker', np.array([500, 380]), sensor_rad=200)
+    s.add_tracker('tracker', np.array([700, 580]), sensor_rad=200)
+    s.add_target('tgt', np.array([50, 50]))
+    s.add_target('tgt', np.array([140, 160]))
+    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]])
+    # s.add_edges([[0,1]])
+    s.run(logging.DEBUG, ground_truth=True)

target.py

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

tracker.py

@@ -1,98 +1,149 @@
-import numpy as np
-import warnings
 import logging
 
+import matplotlib.pyplot as plt
+import numpy as np
+import scipy.stats as st
+
+from ProbMap import ProbMap, ProbMapData
+from Robot import Robot
+
 
 class Sensor:
-    def __init__(self, tracker, sensor_rad=150) -> None:
+    def __init__(self, tracker, coverage_radius=150) -> None:
         self.type = 'Cam'
+        # host tracker
         self.tracker = tracker
-        self.coverage_radius = sensor_rad
+        self.coverage_radius = coverage_radius
 
-    def get_detection(self, targets):
+    def get_detection(self):
+        all_targets = self.tracker.simulator.targets
         detections = []
-        for target in targets:
+        for target in all_targets:
             if np.linalg.norm(target.position - self.tracker.position) < self.coverage_radius:
                 detection = (target.position-self.tracker.position)
-                logging.info(
-                    f"Ture {self.tracker.name}{self.tracker.id} detection:\n\t\t" + str(detection))
-                detection += np.random.normal(loc=0, scale=2, size=2)
+                # logging.debug(
+                #     f"Ture {self.tracker.name}{self.tracker.id} detection:\t" + str(detection))
+                # make up some noise and calculate the confidence of the detection
+                std_dev = 1
+                noise = np.random.normal(loc=0, scale=std_dev, size=2)
+                confidence = sum(st.norm.pdf(
+                    noise, loc=0, scale=std_dev)*2)/2*std_dev
+                detection = detection + noise
                 distance = np.linalg.norm(detection)
                 if distance > self.coverage_radius:
                     detection = detection * (self.coverage_radius/distance)
+                detection = np.append(detection, confidence)
                 detections.append(detection)
-                logging.info(
-                    f"Noisy {self.tracker.name}{self.tracker.id} detection:\n\t\t"+str(detection))
+                logging.debug(
+                    f"Noisy {self.tracker.name}{self.tracker.id} detection: {detection}")
         return detections
 
 
-class Robot:
-    def __init__(self, name: str, id: int, position: np.array, rate: int) -> None:
-        """The base Robot class
-
-        Parameters
-        ----------
-        name : str
-            Robot's name
-        id : int
-            Robot's ID
-        position : np.array
-            Robot's start position
-        rate : int
-            Simulator's update rate
-        """
-        self.name = name
-        self.id = id
-        self.position = position
-        self.desierd_position = self.position
-        self.in_position = True
-        self.speed = 0.0
-        self.rate = rate
-        self.max_speed = 10.0  # m/s
-
-    def move(self):
-        """Move the robot to a position
-
-        Parameters
-        ----------
-        f : Function, optional
-            a funcrtion that returns a position, by default None
+class Tracker(Robot):
+    def __init__(self, simulator, name: str, id: int, position: np.array, coverage_radius, rate: int) -> None:
+        super().__init__(simulator, name, id, position, rate)
+        self.sensor = Sensor(self, coverage_radius)
+        self.neighbor = set()
+
+        self.area_width = 2000  # meter
+        self.area_height = 2000  # meter
+        self.resolution = 1.0  # meter
+        self.prob_map = ProbMap(self.area_width, self.area_height, self.resolution,
+                                center_x=0.0, center_y=0.0, init_val=0.01,
+                                false_alarm_prob=0.05)
+
+        self.observations = dict()  # type: dict[tuple]
+        self.shareable_v = ProbMapData()
+        self.shareable_Q = ProbMapData()
+        self.neighbors_v = dict()
+        self.neighbors_Q = dict()
+
+    def build_shareable_info(self, shareable_info, info_type):
+        """Generate shareable information from local
+
+        Args:
+            shareable_info (dict): Stores all local infomation. Format: {(x, y) : value}
         """
-        logging.warning("Moving function not impelemented")
-        # warnings.warn("moving function not Impelement")
-        pass
-
-    def waypoint_ctrl(self, speed, desierd_pos: np.array = None):
-        # if got new waypoint, update
-        if not np.any(desierd_pos == None):
-            self.desierd_position = desierd_pos
-
-        if speed > self.max_speed:
-            logging.warning("Given speed is over maximum")
-        self.speed = speed
-
-        if np.any(self.position != self.desierd_position):
-            one_time_step_length = speed/self.rate
-            difference = self.desierd_position - self.position
-            distance = np.linalg.norm(difference)
-            # if it's close enough to the goal position, make it in position
-            if distance <= one_time_step_length:
-                self.position = self.desierd_position
-            else:
-                # move to the goal point at the given speed
-                direction = difference/distance
-                self.position = self.position + direction*self.speed/self.rate
+        local_meas_info = ProbMapData()
+        local_meas_info.tracker_id = self.id
+        local_meas_info.type = info_type
+        for k, v in shareable_info.items():
+            local_meas_info.grid_ind += k
+            local_meas_info.values.append(v)
+        self.shareable_v = local_meas_info
 
-    def job(self):
-        pass
-        # self.position = self.speed*(1/self.rate) + self.position
+    def get_info_from_neighbors(self, req_type):
+        neighbors_info = dict()
+        # Send requests and get responses from all neighbors' services
+        # Collect info from neighbors
+        if req_type == 'v':
+            for e in self.neighbor:
+                self.neighbors_v[e] = self.simulator.trackers[e].shareable_v
 
+            for _id, res in self.neighbors_v.items():
+                for i in range(len(res.values)):
+                    cell_ind = tuple([res.grid_ind[i*2], res.grid_ind[i*2+1]])
+                    # sum up all neighbors' measurement values
+                    value = res.values[i]
+                    try:
+                        neighbors_info[cell_ind] += value
+                    except KeyError:
+                        neighbors_info[cell_ind] = value
+        elif req_type == 'Q':
+            for e in self.neighbor:
+                self.neighbors_v[e] = self.simulator.trackers[e].shareable_Q
+            for _id, res in self.neighbors_Q.items():
+                for i in range(len(res.values)):
+                    cell_ind = tuple([res.grid_ind[i*2], res.grid_ind[i*2+1]])
+                    # sum up all neighbors' values and counting, need to calculate average value
+                    value = res.values[i]
+                    try:
+                        neighbors_info[cell_ind][0] += value
+                        neighbors_info[cell_ind][1] += 1.
+                    except KeyError:
+                        neighbors_info[cell_ind] = [value, 1.]
+        return neighbors_info
 
-class Tracker(Robot):
-    def __init__(self, name: str, id: int, position: np.array, sensor_rad, rate: int) -> None:
-        super().__init__(name, id, position, rate)
-        self.sensor = Sensor(self, sensor_rad)
-        self.neighbor = set()
+    def sensing(self):
+        detections = self.sensor.get_detection()
+        output_detection = dict()
+        id_counter = 0
+        for det in detections:
+            output_detection[id_counter] = det
+            id_counter += 1
+        self.observations = output_detection
 
     def job(self):
-        self.waypoint_ctrl(speed=50, desierd_pos=np.array([500, 500]))
+        self.sensing()
+        shareable_v = self.prob_map.generate_shareable_v(
+            self.observations)
+
+        # build shareable_v and publish it
+        self.build_shareable_info(shareable_v, 'v')
+        # logging.debug(f"{self.name}_{self.id}: {self.shareable_v.grid_ind}")
+
+        # get all neighbors' detections
+        neighbors_meas = self.get_info_from_neighbors('v')
+        # logging.debug("{}{} got neighbor {} info: {}".format(
+        #     self.name, self.id, self.neighbor, neighbors_meas))
+
+        # # Update the local map by all detections (local and neighbors')
+        self.prob_map.map_update(shareable_v, neighbors_meas,
+                                 len(self.simulator.trackers), len(self.neighbor))
+
+        # Convert prob map to a shareable information and publish it
+        self.build_shareable_info(
+            self.prob_map.non_empty_cell, 'Q')
+
+        # Collect neighbors' map (Q) for consensus
+        neighbors_map = self.get_info_from_neighbors('Q')
+        # # rospy.loginfo("{} got neighbors' map: {}".format(
+        # #     self.name, neighbors_map))
+
+        # Make consensus, merge neighbors' map
+        self.prob_map.consensus(neighbors_map)
+
+        self.target_estimates = self.prob_map.get_target_est(
+            0.8, normalization=False)
+        print(self.prob_map.prob_map)
+        # print(target_estimates)