diff --git a/birdseye/sensor.py b/birdseye/sensor.py index 992b76fd..ab95eb99 100644 --- a/birdseye/sensor.py +++ b/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) diff --git a/birdseye/utils.py b/birdseye/utils.py index 87aa5700..6a8d0ff2 100644 --- a/birdseye/utils.py +++ b/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,16 +1035,24 @@ 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", @@ -1051,10 +1060,10 @@ def live_plot( ) 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" ) ) diff --git a/geolocate.ini b/geolocate.ini index fc30216e..e784d0c4 100644 --- a/geolocate.ini +++ b/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/ #### diff --git a/geolocate.py b/geolocate.py index 9e201c32..0f9facf6 100644 --- a/geolocate.py +++ b/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()