Merge pull request #363 from ltindall/db_and_replay

Updated db. Updated replay file handling.

birdseye/sensor.py

@@ -46,7 +46,10 @@ def shift(seq, n):
 
 def get_directivity(radiation_pattern, theta):
     theta_degrees = theta * 180 / np.pi
-    theta_degrees = theta_degrees.astype(int)
+    if isinstance(theta_degrees, np.ndarray): 
+        theta_degrees = theta_degrees.astype(int)
+    else: 
+        theta_degrees = int(theta_degrees)
     return radiation_pattern[theta_degrees % len(radiation_pattern)]
 
 
@@ -62,7 +65,7 @@ def rssi(
     Calculate the received signal strength at a receiver in dB
     """
     power_rx = (
-        float(power_tx)
+        float(power_tx) - 30 # -30 dbm to dbW
         + directivity_rx
         + float(directivity_tx)
         + (20 * np.log10(speed_of_light / (4 * np.pi)))
@@ -302,7 +305,7 @@ def observation_vectorized(self, states, target, fading_sigma=None):
                 power_tx=self.power_tx[target],
                 directivity_tx=self.directivity_tx[target],
                 freq=self.freq[target],
-                fading_sigma=self.fading_sigma,
+                fading_sigma=fading_sigma,
             )
         )
         rssi_power = power_to_dB(power)
@@ -329,7 +332,7 @@ def observation(self, state, target=None, fading_sigma=None):
                 power_tx=self.power_tx[target],
                 directivity_tx=self.directivity_tx[target],
                 freq=self.freq[target],
-                fading_sigma=self.fading_sigma,
+                fading_sigma=fading_sigma,
             )
         )
         rssi_power = power_to_dB(power)

birdseye/utils.py

@@ -880,11 +880,12 @@ def live_plot(
         )  # https://matplotlib.org/3.5.0/api/_as_gen/matplotlib.pyplot.legend.html
 
         separable_color_array = [
-            ["lightgreen", "green"],
             ["deepskyblue", "blue"],
             ["pink", "red"],
             ["wheat", "orange"],
+            ["lightgreen", "green"],
         ]
+        sensor_color = "green"
         legend_elements = []
         for t in range(env.state.n_targets):
             # PLOT PARTICLES
@@ -947,7 +948,7 @@ def live_plot(
                 color=centroid_color,
                 markeredgecolor="black",
                 label="Mean Estimate",
-                markersize=12,
+                markersize=20,
                 zorder=7,
             )
 
@@ -1034,27 +1035,35 @@ def live_plot(
         if self.transform is not None:
             sensor_x += self.transform[0]
             sensor_y += self.transform[1]
+
         if len(self.sensor_hist) > 1:
+            arrow_x, arrow_y = pol2cart(
+                6, np.radians(env.state.sensor_state[2])
+            )
             line4 = mpatches.FancyArrow(
-                sensor_x[-2],
-                sensor_y[-2],
-                4 * (sensor_x[-1] - sensor_x[-2]),
-                4 * (sensor_y[-1] - sensor_y[-2]),
-                width=2.5,
-                head_width=9,
-                head_length=6,
-                facecolor="rebeccapurple",
+                # sensor_x[-2],
+                # sensor_y[-2],
+                # 4 * (sensor_x[-1] - sensor_x[-2]),
+                # 4 * (sensor_y[-1] - sensor_y[-2]),
+                sensor_x[-1],
+                sensor_y[-1],
+                arrow_x,
+                arrow_y,
+                width=5,
+                head_width=15,
+                head_length=15,
+                facecolor=sensor_color,
                 zorder=7,
                 # edgecolor="black",
                 label="Sensor",
                 linewidth=1,
             )
             ax.add_patch(line4)
             ax.plot(
-                sensor_x[len(sensor_x)-self.history_length:-1],
-                sensor_y[len(sensor_x)-self.history_length:-1],
+                sensor_x[len(sensor_x)-self.history_length:],
+                sensor_y[len(sensor_x)-self.history_length:],
                 linewidth=5,
-                color="rebeccapurple",
+                color=sensor_color,
                 # markeredgecolor="black",
                 # markersize=4,
                 zorder=6,
@@ -1072,7 +1081,7 @@ def live_plot(
             lines.extend([line4])
             legend_elements.append(
                 mpatches.Patch(
-                    facecolor="rebeccapurple", edgecolor="black", label="Sensor"
+                    facecolor=sensor_color, edgecolor="black", label="Sensor"
                 )
             )
 

geolocate.ini

@@ -6,7 +6,7 @@ n_targets =  2
 # antenna_type options are [omni, directional]
 antenna_type = directional
 n_antennas = 1
-power_tx =  26
+power_tx = 26
 directivity_tx = 1
 freq =  5.7e9
 fading_sigma =  8
@@ -18,6 +18,7 @@ particle_distance = 250
 #static_position = 32.922651, -117.120815
 #static_heading = 0
 #replay_file = gamutrf_fieldtest/aligned_1653599174_1653599808.json
+#replay_file = replay_files/mqtt-inference-1701073241.113538.log
 #plot_dir = gamutrf_fieldtest/aligned_1653599174_1653599808/
 
 ####

geolocate.py

@@ -150,14 +150,11 @@ def data_handler(self, message_data):
         if predictions:
             self.data["rssi"] = {}
             for class_name in predictions.keys(): 
-                #self.class_map[class_name] = self.class_map.get(class_name, len(self.class_map))
-                #if class_name not in self.class_map:
-                #    self.class_map[class_name] = self.class_iter
-                #    self.class_iter += 1
 
                 self.data["rssi"][class_name] = np.mean(
-                    [float(prediction["rssi"]) for prediction in predictions[class_name]]
+                    [float(prediction.get("rssi_max", metadata["rssi_max"])) for prediction in predictions[class_name]]
                 )
+
         elif metadata: 
             # TODO: how to handle when tracking multiple targets
             #self.data["rssi"] = float(metadata["rssi_mean"])
@@ -241,6 +238,18 @@ def index():
                 host=host_name, port=port, debug=False, use_reloader=False
             )
         ).start()
+
+    def get_replay_json(self, replay_file):
+        with open(replay_file, "r", encoding="UTF-8") as open_file:
+            replay_data = json.load(open_file)
+            for ts in replay_data:
+                yield replay_data[ts]
+
+    def get_replay_log(self, replay_file):
+        with open(replay_file, "r", encoding="UTF-8") as open_file:
+            for line in open_file:
+                replay_data = json.loads(line)
+                yield replay_data
 
     def main(self):
         """
@@ -338,9 +347,10 @@ def main(self):
                 mqtt_host, mqtt_port, self.data_handler
             )
         else:
-            with open(replay_file, "r", encoding="UTF-8") as open_file:
-                replay_data = json.load(open_file)
-                replay_ts = sorted(replay_data.keys())
+            if replay_file.endswith(".log"):
+                get_replay_data = self.get_replay_log(replay_file)
+            elif replay_file.endswith(".json"):
+                get_replay_data = self.get_replay_json(replay_file)
         ###########
 
         # BirdsEye
@@ -374,6 +384,10 @@ def main(self):
             data=self.data,
         )  # fading sigm = 8dB, threshold = -120dB
 
+        # Test expected RSSI
+        # for distance in [10, 20, 30, 40, 50, 60, 70]:
+        #     print(f"{distance=}, rssi={sensor.observation([distance,0],0,0)}")
+
         # Action space
         # actions = WalkingActions()
         actions = birdseye.actions.BaselineActions(sensor_speed=sensor_speed)
@@ -451,7 +465,7 @@ def main(self):
         control_actions = []
         step_time = 0
 
-        while self.data["gps"] != "fix":
+        while self.data["gps"] != "fix" and not replay_file:
             time.sleep(1)
             logging.info("Waiting for GPS...")
 
@@ -461,9 +475,12 @@ def main(self):
 
             if replay_file:
                 # load data from saved file
-                if time_step == len(replay_ts):
+                try: 
+                    replay_data = next(get_replay_data)
+                except StopIteration:
                     break
-                self.data_handler(replay_data[replay_ts[time_step]])
+
+                self.data_handler(replay_data)
 
             action_start = timer()