From 179065ab4c2b164ee40e7d3a97927dd096603d72 Mon Sep 17 00:00:00 2001 From: Nicola Perree Date: Fri, 9 Jun 2023 10:09:09 +0100 Subject: [PATCH 1/2] Add sensor field of view to plot animations --- .../01_SingleSensorManagement.py | 94 ++++++++++- .../02_MultiSensorManagement.py | 107 +++++++++++- .../03_OptimisedSensorManagement.py | 157 +++++++++++++++++- 3 files changed, 345 insertions(+), 13 deletions(-) diff --git a/docs/tutorials/sensormanagement/01_SingleSensorManagement.py b/docs/tutorials/sensormanagement/01_SingleSensorManagement.py index c9380dea7..8bad7debe 100644 --- a/docs/tutorials/sensormanagement/01_SingleSensorManagement.py +++ b/docs/tutorials/sensormanagement/01_SingleSensorManagement.py @@ -154,7 +154,7 @@ ydirection *= -1 # %% -# Plot the ground truths. This is done using the :class:`~.Plotterly` class from Stone Soup. +# Plot the ground truths. This is done using the :class:`~.AnimatedPlotterly` class from Stone Soup. from stonesoup.plotter import AnimatedPlotterly @@ -340,6 +340,9 @@ # Here the chosen target for observation is selected randomly using the method :meth:`choose_actions()` from the class # :class:`~.RandomSensorManager`. +import copy + +sensor_history_A = dict() for timestep in timesteps[1:]: # Generate chosen configuration @@ -355,6 +358,9 @@ sensorA.act(timestep) + # Store sensor history for plotting + sensor_history_A[timestep] = copy.copy(sensorA) + # Observe this ground truth # i.e. {z}k measurementsA |= sensorA.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) @@ -371,12 +377,52 @@ track.append(hypothesis.prediction) # %% -# Plot ground truths, tracks and uncertainty ellipses for each target. +# Plot ground truths, tracks and uncertainty ellipses for each target. This uses the Stone Soup +# :class:`~.AnimatedPlotterly`, with added code to plot the field of view of the sensor. -plotterA = AnimatedPlotterly(timesteps, tail_length=1) +import plotly.graph_objects as go +from stonesoup.functions import pol2cart + +plotterA = AnimatedPlotterly(timesteps, tail_length=1, sim_duration=10) plotterA.plot_sensors(sensorA) plotterA.plot_ground_truths(truths, [0, 2]) plotterA.plot_tracks(tracksA, [0, 2], uncertainty=True, plot_history=False) + +# Plot sensor field of view +trace_base = len(plotterA.fig.data) +plotterA.fig.add_trace(go.Scatter(mode='lines', + line=go.scatter.Line(color='black', + dash='dash'))) + +for frame in plotterA.fig.frames: + traces_ = list(frame.traces) + data_ = list(frame.data) + x = [0, 0] + y = [0, 0] + timestring = frame.name + timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") + + if timestamp in sensor_history_A.keys(): + sensor = sensor_history_A[timestamp] + for i, fov_side in enumerate((-1, 1)): + range_ = min(getattr(sensor, 'max_range', np.inf), 100) + x[i], y[i] = pol2cart(range_, + sensor.dwell_centre[0, 0] + + sensor.fov_angle / 2 * fov_side) \ + + sensor.position[[0, 1], 0] + else: + continue + + data_.append(go.Scatter(x=[x[0], sensorA.position[0], x[1]], + y=[y[0], sensorA.position[1], y[1]], + mode="lines", + line=go.scatter.Line(color='black', + dash='dash'), + showlegend=False)) + traces_.append(trace_base) + frame.traces = traces_ + frame.data = data_ + plotterA.fig # %% @@ -405,6 +451,7 @@ # The chosen action is given to the sensor, measurements are made and the tracks updated based on these measurements. # Predictions are made for tracks which have not been observed by the sensor. +sensor_history_B = dict() for timestep in timesteps[1:]: # Generate chosen configuration @@ -420,6 +467,9 @@ sensorB.act(timestep) + # Store sensor history for plotting + sensor_history_B[timestep] = copy.copy(sensorB) + # Observe this ground truth # i.e. {z}k measurementsB |= sensorB.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) @@ -437,10 +487,46 @@ # %% # Plot ground truths, tracks and uncertainty ellipses for each target. -plotterB = AnimatedPlotterly(timesteps, tail_length=1) +plotterB = AnimatedPlotterly(timesteps, tail_length=1, sim_duration=10) plotterB.plot_sensors(sensorB) plotterB.plot_ground_truths(truths, [0, 2]) plotterB.plot_tracks(tracksB, [0, 2], uncertainty=True, plot_history=False) + +# Plot sensor field of view +trace_base = len(plotterB.fig.data) +plotterB.fig.add_trace(go.Scatter(mode='lines', + line=go.scatter.Line(color='black', + dash='dash'))) + +for frame in plotterB.fig.frames: + traces_ = list(frame.traces) + data_ = list(frame.data) + x = [0, 0] + y = [0, 0] + timestring = frame.name + timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") + + if timestamp in sensor_history_B.keys(): + sensor = sensor_history_B[timestamp] + for i, fov_side in enumerate((-1, 1)): + range_ = min(getattr(sensor, 'max_range', np.inf), 100) + x[i], y[i] = pol2cart(range_, + sensor.dwell_centre[0, 0] + + sensor.fov_angle / 2 * fov_side) \ + + sensor.position[[0, 1], 0] + else: + continue + + data_.append(go.Scatter(x=[x[0], sensorB.position[0], x[1]], + y=[y[0], sensorB.position[1], y[1]], + mode="lines", + line=go.scatter.Line(color='black', + dash='dash'), + showlegend=False)) + traces_.append(trace_base) + frame.traces = traces_ + frame.data = data_ + plotterB.fig # %% diff --git a/docs/tutorials/sensormanagement/02_MultiSensorManagement.py b/docs/tutorials/sensormanagement/02_MultiSensorManagement.py index f6bdcf4a8..b1856fd10 100644 --- a/docs/tutorials/sensormanagement/02_MultiSensorManagement.py +++ b/docs/tutorials/sensormanagement/02_MultiSensorManagement.py @@ -104,7 +104,7 @@ ydirection *= -1 # %% -# Plot the ground truths. This is done using the :class:`~.Plotterly` class from Stone Soup. +# Plot the ground truths. This is done using the :class:`~.AnimatedPlotterly` class from Stone Soup. from stonesoup.plotter import AnimatedPlotterly @@ -307,7 +307,9 @@ # :class:`~.ChangeDwellAction`, selected at random. from ordered_set import OrderedSet +import copy +sensor_history_A = dict() for timestep in timesteps[1:]: # Generate chosen configuration @@ -320,12 +322,17 @@ for sensor, actions in chosen_action.items(): sensor.add_actions(actions) + sensor_history_at_t = [] for sensor in sensor_setA: sensor.act(timestep) + sensor_history_at_t.append(copy.copy(sensor)) # Observe this ground truth measurementsA |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) + # Store sensor history for plotting + sensor_history_A[timestep] = sensor_history_at_t + hypotheses = data_associator.associate(tracksA, measurementsA, timestep) @@ -339,12 +346,57 @@ # %% # Plot ground truths, tracks and uncertainty ellipses for each target. The positions of the sensors are indicated -# by black x markers. +# by black x markers. This uses the Stone Soup +# :class:`~.AnimatedPlotterly`, with added code to plot the field of view of the sensor. + +import plotly.graph_objects as go +from stonesoup.functions import pol2cart -plotterA = AnimatedPlotterly(timesteps, tail_length=1) +plotterA = AnimatedPlotterly(timesteps, tail_length=1, sim_duration=10) plotterA.plot_sensors(sensor_setA) plotterA.plot_ground_truths(truths, [0, 2]) plotterA.plot_tracks(tracksA, [0, 2], uncertainty=True, plot_history=False) + +# Plot sensor field of view +trace_base = len(plotterA.fig.data) +for _ in sensor_setA: + plotterA.fig.add_trace(go.Scatter(mode='lines', + line=go.scatter.Line(color='black', + dash='dash'))) + +for frame in plotterA.fig.frames: + traces_ = list(frame.traces) + data_ = list(frame.data) + + timestring = frame.name + timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") + + for n, sensor_ in enumerate(sensor_setA): + x = [0, 0] + y = [0, 0] + + if timestamp in sensor_history_A.keys(): + sensor = sensor_history_A[timestamp][n] + for i, fov_side in enumerate((-1, 1)): + range_ = min(getattr(sensor, 'max_range', np.inf), 100) + x[i], y[i] = pol2cart(range_, + sensor.dwell_centre[0, 0] + + sensor.fov_angle / 2 * fov_side) \ + + sensor.position[[0, 1], 0] + else: + continue + + data_.append(go.Scatter(x=[x[0], sensor.position[0], x[1]], + y=[y[0], sensor.position[1], y[1]], + mode="lines", + line=go.scatter.Line(color='black', + dash='dash'), + showlegend=False)) + traces_.append(trace_base + n) + + frame.traces = traces_ + frame.data = data_ + plotterA.fig # %% @@ -378,6 +430,7 @@ # the tracks updated based on these measurements. Predictions are made for tracks # which have not been observed by the sensors. +sensor_history_B = dict() for timestep in timesteps[1:]: # Generate chosen configuration @@ -390,12 +443,17 @@ for sensor, actions in chosen_action.items(): sensor.add_actions(actions) + sensor_history_at_t = [] for sensor in sensor_setB: sensor.act(timestep) + sensor_history_at_t.append(copy.copy(sensor)) # Observe this ground truth measurementsB |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) + # Store sensor history for plotting + sensor_history_B[timestep] = sensor_history_at_t + hypotheses = data_associator.associate(tracksB, measurementsB, timestep) @@ -410,10 +468,51 @@ # %% # Plot ground truths, tracks and uncertainty ellipses for each target. -plotterB = AnimatedPlotterly(timesteps, tail_length=1) +plotterB = AnimatedPlotterly(timesteps, tail_length=1, sim_duration=10) plotterB.plot_sensors(sensor_setB) plotterB.plot_ground_truths(truths, [0, 2]) plotterB.plot_tracks(tracksB, [0, 2], uncertainty=True, plot_history=False) + +# Plot sensor field of view +trace_base = len(plotterB.fig.data) +for _ in sensor_setB: + plotterB.fig.add_trace(go.Scatter(mode='lines', + line=go.scatter.Line(color='black', + dash='dash'))) + +for frame in plotterB.fig.frames: + traces_ = list(frame.traces) + data_ = list(frame.data) + + timestring = frame.name + timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") + + for n, sensor_ in enumerate(sensor_setB): + x = [0, 0] + y = [0, 0] + + if timestamp in sensor_history_B.keys(): + sensor = sensor_history_B[timestamp][n] + for i, fov_side in enumerate((-1, 1)): + range_ = min(getattr(sensor, 'max_range', np.inf), 100) + x[i], y[i] = pol2cart(range_, + sensor.dwell_centre[0, 0] + + sensor.fov_angle / 2 * fov_side) \ + + sensor.position[[0, 1], 0] + else: + continue + + data_.append(go.Scatter(x=[x[0], sensor.position[0], x[1]], + y=[y[0], sensor.position[1], y[1]], + mode="lines", + line=go.scatter.Line(color='black', + dash='dash'), + showlegend=False)) + traces_.append(trace_base + n) + + frame.traces = traces_ + frame.data = data_ + plotterB.fig # %% diff --git a/docs/tutorials/sensormanagement/03_OptimisedSensorManagement.py b/docs/tutorials/sensormanagement/03_OptimisedSensorManagement.py index 92eafaf82..77d3f6890 100644 --- a/docs/tutorials/sensormanagement/03_OptimisedSensorManagement.py +++ b/docs/tutorials/sensormanagement/03_OptimisedSensorManagement.py @@ -97,7 +97,7 @@ ydirection *= -1 # %% -# Plot the ground truths. This is done using the :class:`~.Plotterly` class from Stone Soup. +# Plot the ground truths. This is done using the :class:`~.AnimatedPlotterly` class from Stone Soup. from stonesoup.plotter import AnimatedPlotterly @@ -309,10 +309,12 @@ from ordered_set import OrderedSet import time +import copy # Start timer for cell execution time cell_start_time1 = time.time() +sensor_history_A = dict() for timestep in timesteps[1:]: # Generate chosen configuration @@ -325,12 +327,17 @@ for sensor, actions in chosen_action.items(): sensor.add_actions(actions) + sensor_history_at_t = [] for sensor in sensor_setA: sensor.act(timestep) + sensor_history_at_t.append(copy.copy(sensor)) # Observe this ground truth measurementsA |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) + # Store sensor history for plotting + sensor_history_A[timestep] = sensor_history_at_t + hypotheses = data_associator.associate(tracksA, measurementsA, timestep) @@ -346,12 +353,57 @@ # %% # Plot ground truths, tracks and uncertainty ellipses for each target. The positions of the sensors are indicated -# by black x markers. +# by black x markers.This uses the Stone Soup +# :class:`~.AnimatedPlotterly`, with added code to plot the field of view of the sensor. + +import plotly.graph_objects as go +from stonesoup.functions import pol2cart -plotterA = AnimatedPlotterly(timesteps, tail_length=1) +plotterA = AnimatedPlotterly(timesteps, tail_length=1, sim_duration=10) plotterA.plot_sensors(sensor_setA) plotterA.plot_ground_truths(truths, [0, 2]) plotterA.plot_tracks(set(tracksA), [0, 2], uncertainty=True, plot_history=False) + +# Plot sensor field of view +trace_base = len(plotterA.fig.data) +for _ in sensor_setA: + plotterA.fig.add_trace(go.Scatter(mode='lines', + line=go.scatter.Line(color='black', + dash='dash'))) + +for frame in plotterA.fig.frames: + traces_ = list(frame.traces) + data_ = list(frame.data) + + timestring = frame.name + timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") + + for n, sensor_ in enumerate(sensor_setA): + x = [0, 0] + y = [0, 0] + + if timestamp in sensor_history_A.keys(): + sensor = sensor_history_A[timestamp][n] + for i, fov_side in enumerate((-1, 1)): + range_ = min(getattr(sensor, 'max_range', np.inf), 100) + x[i], y[i] = pol2cart(range_, + sensor.dwell_centre[0, 0] + + sensor.fov_angle / 2 * fov_side) \ + + sensor.position[[0, 1], 0] + else: + continue + + data_.append(go.Scatter(x=[x[0], sensor.position[0], x[1]], + y=[y[0], sensor.position[1], y[1]], + mode="lines", + line=go.scatter.Line(color='black', + dash='dash'), + showlegend=False)) + traces_.append(trace_base + n) + + frame.traces = traces_ + frame.data = data_ + plotterA.fig # %% @@ -364,6 +416,7 @@ # Start timer for cell execution time cell_start_time2 = time.time() +sensor_history_B = dict() for timestep in timesteps[1:]: # Generate chosen configuration @@ -376,12 +429,17 @@ for sensor, actions in chosen_action.items(): sensor.add_actions(actions) + sensor_history_at_t = [] for sensor in sensor_setB: sensor.act(timestep) + sensor_history_at_t.append(copy.copy(sensor)) # Observe this ground truth measurementsB |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) + # Store sensor history for plotting + sensor_history_B[timestep] = sensor_history_at_t + hypotheses = data_associator.associate(tracksB, measurementsB, timestep) @@ -398,10 +456,52 @@ # %% # Plot ground truths, tracks and uncertainty ellipses for each target. -plotterB = AnimatedPlotterly(timesteps, tail_length=1) +plotterB = AnimatedPlotterly(timesteps, tail_length=1, sim_duration=10) plotterB.plot_sensors(sensor_setB) plotterB.plot_ground_truths(truths, [0, 2]) plotterB.plot_tracks(tracksB, [0, 2], uncertainty=True, plot_history=False) + + +# Plot sensor field of view +trace_base = len(plotterB.fig.data) +for _ in sensor_setB: + plotterB.fig.add_trace(go.Scatter(mode='lines', + line=go.scatter.Line(color='black', + dash='dash'))) + +for frame in plotterB.fig.frames: + traces_ = list(frame.traces) + data_ = list(frame.data) + + timestring = frame.name + timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") + + for n, sensor_ in enumerate(sensor_setB): + x = [0, 0] + y = [0, 0] + + if timestamp in sensor_history_B.keys(): + sensor = sensor_history_B[timestamp][n] + for i, fov_side in enumerate((-1, 1)): + range_ = min(getattr(sensor, 'max_range', np.inf), 100) + x[i], y[i] = pol2cart(range_, + sensor.dwell_centre[0, 0] + + sensor.fov_angle / 2 * fov_side) \ + + sensor.position[[0, 1], 0] + else: + continue + + data_.append(go.Scatter(x=[x[0], sensor.position[0], x[1]], + y=[y[0], sensor.position[1], y[1]], + mode="lines", + line=go.scatter.Line(color='black', + dash='dash'), + showlegend=False)) + traces_.append(trace_base + n) + + frame.traces = traces_ + frame.data = data_ + plotterB.fig # %% @@ -411,6 +511,7 @@ # Start timer for cell execution time cell_start_time3 = time.time() +sensor_history_C = dict() for timestep in timesteps[1:]: # Generate chosen configuration @@ -423,12 +524,17 @@ for sensor, actions in chosen_action.items(): sensor.add_actions(actions) + sensor_history_at_t = [] for sensor in sensor_setC: sensor.act(timestep) + sensor_history_at_t.append(copy.copy(sensor)) # Observe this ground truth measurementsC |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) + # Store sensor history for plotting + sensor_history_C[timestep] = sensor_history_at_t + hypotheses = data_associator.associate(tracksC, measurementsC, timestep) @@ -445,10 +551,51 @@ # %% # Plot ground truths, tracks and uncertainty ellipses for each target. -plotterC = AnimatedPlotterly(timesteps, tail_length=1) +plotterC = AnimatedPlotterly(timesteps, tail_length=1, sim_duration=10) plotterC.plot_sensors(sensor_setC) plotterC.plot_ground_truths(truths, [0, 2]) plotterC.plot_tracks(tracksC, [0, 2], uncertainty=True, plot_history=False) + +# Plot sensor field of view +trace_base = len(plotterC.fig.data) +for _ in sensor_setC: + plotterC.fig.add_trace(go.Scatter(mode='lines', + line=go.scatter.Line(color='black', + dash='dash'))) + +for frame in plotterC.fig.frames: + traces_ = list(frame.traces) + data_ = list(frame.data) + + timestring = frame.name + timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") + + for n, sensor_ in enumerate(sensor_setC): + x = [0, 0] + y = [0, 0] + + if timestamp in sensor_history_C.keys(): + sensor = sensor_history_C[timestamp][n] + for i, fov_side in enumerate((-1, 1)): + range_ = min(getattr(sensor, 'max_range', np.inf), 100) + x[i], y[i] = pol2cart(range_, + sensor.dwell_centre[0, 0] + + sensor.fov_angle / 2 * fov_side) \ + + sensor.position[[0, 1], 0] + else: + continue + + data_.append(go.Scatter(x=[x[0], sensor.position[0], x[1]], + y=[y[0], sensor.position[1], y[1]], + mode="lines", + line=go.scatter.Line(color='black', + dash='dash'), + showlegend=False)) + traces_.append(trace_base + n) + + frame.traces = traces_ + frame.data = data_ + plotterC.fig # %% From d53044aaf0374482ef5e305ddb088aaf4a424e6a Mon Sep 17 00:00:00 2001 From: Steven Hiscocks Date: Mon, 19 Jun 2023 14:41:25 +0100 Subject: [PATCH 2/2] Move sensor FOV plotting into function in tutorials --- .../01_SingleSensorManagement.py | 109 ++++------ .../02_MultiSensorManagement.py | 141 +++++-------- .../03_OptimisedSensorManagement.py | 192 +++++------------- 3 files changed, 140 insertions(+), 302 deletions(-) diff --git a/docs/tutorials/sensormanagement/01_SingleSensorManagement.py b/docs/tutorials/sensormanagement/01_SingleSensorManagement.py index 8bad7debe..18701c8c2 100644 --- a/docs/tutorials/sensormanagement/01_SingleSensorManagement.py +++ b/docs/tutorials/sensormanagement/01_SingleSensorManagement.py @@ -388,41 +388,45 @@ plotterA.plot_ground_truths(truths, [0, 2]) plotterA.plot_tracks(tracksA, [0, 2], uncertainty=True, plot_history=False) -# Plot sensor field of view -trace_base = len(plotterA.fig.data) -plotterA.fig.add_trace(go.Scatter(mode='lines', - line=go.scatter.Line(color='black', - dash='dash'))) - -for frame in plotterA.fig.frames: - traces_ = list(frame.traces) - data_ = list(frame.data) - x = [0, 0] - y = [0, 0] - timestring = frame.name - timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") - - if timestamp in sensor_history_A.keys(): - sensor = sensor_history_A[timestamp] - for i, fov_side in enumerate((-1, 1)): - range_ = min(getattr(sensor, 'max_range', np.inf), 100) - x[i], y[i] = pol2cart(range_, - sensor.dwell_centre[0, 0] - + sensor.fov_angle / 2 * fov_side) \ - + sensor.position[[0, 1], 0] - else: - continue - - data_.append(go.Scatter(x=[x[0], sensorA.position[0], x[1]], - y=[y[0], sensorA.position[1], y[1]], - mode="lines", - line=go.scatter.Line(color='black', - dash='dash'), - showlegend=False)) - traces_.append(trace_base) - frame.traces = traces_ - frame.data = data_ +def plot_sensor_fov(fig, sensor_history): + # Plot sensor field of view + trace_base = len(fig.data) + fig.add_trace(go.Scatter(mode='lines', + line=go.scatter.Line(color='black', + dash='dash'))) + + for frame in fig.frames: + traces_ = list(frame.traces) + data_ = list(frame.data) + x = [0, 0] + y = [0, 0] + timestring = frame.name + timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") + + if timestamp in sensor_history: + sensor = sensor_history[timestamp] + for i, fov_side in enumerate((-1, 1)): + range_ = min(getattr(sensor, 'max_range', np.inf), 100) + x[i], y[i] = pol2cart(range_, + sensor.dwell_centre[0, 0] + + sensor.fov_angle / 2 * fov_side) \ + + sensor.position[[0, 1], 0] + else: + continue + + data_.append(go.Scatter(x=[x[0], sensor.position[0], x[1]], + y=[y[0], sensor.position[1], y[1]], + mode="lines", + line=go.scatter.Line(color='black', + dash='dash'), + showlegend=False)) + traces_.append(trace_base) + frame.traces = traces_ + frame.data = data_ + + +plot_sensor_fov(plotterA.fig, sensor_history_A) plotterA.fig # %% @@ -491,42 +495,7 @@ plotterB.plot_sensors(sensorB) plotterB.plot_ground_truths(truths, [0, 2]) plotterB.plot_tracks(tracksB, [0, 2], uncertainty=True, plot_history=False) - -# Plot sensor field of view -trace_base = len(plotterB.fig.data) -plotterB.fig.add_trace(go.Scatter(mode='lines', - line=go.scatter.Line(color='black', - dash='dash'))) - -for frame in plotterB.fig.frames: - traces_ = list(frame.traces) - data_ = list(frame.data) - x = [0, 0] - y = [0, 0] - timestring = frame.name - timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") - - if timestamp in sensor_history_B.keys(): - sensor = sensor_history_B[timestamp] - for i, fov_side in enumerate((-1, 1)): - range_ = min(getattr(sensor, 'max_range', np.inf), 100) - x[i], y[i] = pol2cart(range_, - sensor.dwell_centre[0, 0] - + sensor.fov_angle / 2 * fov_side) \ - + sensor.position[[0, 1], 0] - else: - continue - - data_.append(go.Scatter(x=[x[0], sensorB.position[0], x[1]], - y=[y[0], sensorB.position[1], y[1]], - mode="lines", - line=go.scatter.Line(color='black', - dash='dash'), - showlegend=False)) - traces_.append(trace_base) - frame.traces = traces_ - frame.data = data_ - +plot_sensor_fov(plotterB.fig, sensor_history_B) plotterB.fig # %% diff --git a/docs/tutorials/sensormanagement/02_MultiSensorManagement.py b/docs/tutorials/sensormanagement/02_MultiSensorManagement.py index b1856fd10..a8312c918 100644 --- a/docs/tutorials/sensormanagement/02_MultiSensorManagement.py +++ b/docs/tutorials/sensormanagement/02_MultiSensorManagement.py @@ -307,9 +307,10 @@ # :class:`~.ChangeDwellAction`, selected at random. from ordered_set import OrderedSet +from collections import defaultdict import copy -sensor_history_A = dict() +sensor_history_A = defaultdict(dict) for timestep in timesteps[1:]: # Generate chosen configuration @@ -322,17 +323,13 @@ for sensor, actions in chosen_action.items(): sensor.add_actions(actions) - sensor_history_at_t = [] for sensor in sensor_setA: sensor.act(timestep) - sensor_history_at_t.append(copy.copy(sensor)) + sensor_history_A[timestep][sensor] = copy.copy(sensor) # Observe this ground truth measurementsA |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) - # Store sensor history for plotting - sensor_history_A[timestep] = sensor_history_at_t - hypotheses = data_associator.associate(tracksA, measurementsA, timestep) @@ -357,46 +354,50 @@ plotterA.plot_ground_truths(truths, [0, 2]) plotterA.plot_tracks(tracksA, [0, 2], uncertainty=True, plot_history=False) -# Plot sensor field of view -trace_base = len(plotterA.fig.data) -for _ in sensor_setA: - plotterA.fig.add_trace(go.Scatter(mode='lines', - line=go.scatter.Line(color='black', - dash='dash'))) - -for frame in plotterA.fig.frames: - traces_ = list(frame.traces) - data_ = list(frame.data) - - timestring = frame.name - timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") - - for n, sensor_ in enumerate(sensor_setA): - x = [0, 0] - y = [0, 0] - - if timestamp in sensor_history_A.keys(): - sensor = sensor_history_A[timestamp][n] - for i, fov_side in enumerate((-1, 1)): - range_ = min(getattr(sensor, 'max_range', np.inf), 100) - x[i], y[i] = pol2cart(range_, - sensor.dwell_centre[0, 0] - + sensor.fov_angle / 2 * fov_side) \ - + sensor.position[[0, 1], 0] - else: - continue - - data_.append(go.Scatter(x=[x[0], sensor.position[0], x[1]], - y=[y[0], sensor.position[1], y[1]], - mode="lines", - line=go.scatter.Line(color='black', - dash='dash'), - showlegend=False)) - traces_.append(trace_base + n) - - frame.traces = traces_ - frame.data = data_ +def plot_sensor_fov(fig, sensor_set, sensor_history): + # Plot sensor field of view + trace_base = len(fig.data) + for _ in sensor_set: + fig.add_trace(go.Scatter(mode='lines', + line=go.scatter.Line(color='black', + dash='dash'))) + + for frame in fig.frames: + traces_ = list(frame.traces) + data_ = list(frame.data) + + timestring = frame.name + timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") + + for n, sensor_ in enumerate(sensor_set): + x = [0, 0] + y = [0, 0] + + if timestamp in sensor_history: + sensor = sensor_history[timestamp][sensor_] + for i, fov_side in enumerate((-1, 1)): + range_ = min(getattr(sensor, 'max_range', np.inf), 100) + x[i], y[i] = pol2cart(range_, + sensor.dwell_centre[0, 0] + + sensor.fov_angle / 2 * fov_side) \ + + sensor.position[[0, 1], 0] + else: + continue + + data_.append(go.Scatter(x=[x[0], sensor.position[0], x[1]], + y=[y[0], sensor.position[1], y[1]], + mode="lines", + line=go.scatter.Line(color='black', + dash='dash'), + showlegend=False)) + traces_.append(trace_base + n) + + frame.traces = traces_ + frame.data = data_ + + +plot_sensor_fov(plotterA.fig, sensor_setA, sensor_history_A) plotterA.fig # %% @@ -430,7 +431,7 @@ # the tracks updated based on these measurements. Predictions are made for tracks # which have not been observed by the sensors. -sensor_history_B = dict() +sensor_history_B = defaultdict(dict) for timestep in timesteps[1:]: # Generate chosen configuration @@ -443,17 +444,13 @@ for sensor, actions in chosen_action.items(): sensor.add_actions(actions) - sensor_history_at_t = [] for sensor in sensor_setB: sensor.act(timestep) - sensor_history_at_t.append(copy.copy(sensor)) + sensor_history_B[timestep][sensor] = copy.copy(sensor) # Observe this ground truth measurementsB |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) - # Store sensor history for plotting - sensor_history_B[timestep] = sensor_history_at_t - hypotheses = data_associator.associate(tracksB, measurementsB, timestep) @@ -472,47 +469,7 @@ plotterB.plot_sensors(sensor_setB) plotterB.plot_ground_truths(truths, [0, 2]) plotterB.plot_tracks(tracksB, [0, 2], uncertainty=True, plot_history=False) - -# Plot sensor field of view -trace_base = len(plotterB.fig.data) -for _ in sensor_setB: - plotterB.fig.add_trace(go.Scatter(mode='lines', - line=go.scatter.Line(color='black', - dash='dash'))) - -for frame in plotterB.fig.frames: - traces_ = list(frame.traces) - data_ = list(frame.data) - - timestring = frame.name - timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") - - for n, sensor_ in enumerate(sensor_setB): - x = [0, 0] - y = [0, 0] - - if timestamp in sensor_history_B.keys(): - sensor = sensor_history_B[timestamp][n] - for i, fov_side in enumerate((-1, 1)): - range_ = min(getattr(sensor, 'max_range', np.inf), 100) - x[i], y[i] = pol2cart(range_, - sensor.dwell_centre[0, 0] - + sensor.fov_angle / 2 * fov_side) \ - + sensor.position[[0, 1], 0] - else: - continue - - data_.append(go.Scatter(x=[x[0], sensor.position[0], x[1]], - y=[y[0], sensor.position[1], y[1]], - mode="lines", - line=go.scatter.Line(color='black', - dash='dash'), - showlegend=False)) - traces_.append(trace_base + n) - - frame.traces = traces_ - frame.data = data_ - +plot_sensor_fov(plotterB.fig, sensor_setB, sensor_history_B) plotterB.fig # %% diff --git a/docs/tutorials/sensormanagement/03_OptimisedSensorManagement.py b/docs/tutorials/sensormanagement/03_OptimisedSensorManagement.py index 77d3f6890..8ae081676 100644 --- a/docs/tutorials/sensormanagement/03_OptimisedSensorManagement.py +++ b/docs/tutorials/sensormanagement/03_OptimisedSensorManagement.py @@ -308,13 +308,14 @@ # Each sensor manager is run in the same way as in the previous tutorials. from ordered_set import OrderedSet +from collections import defaultdict import time import copy # Start timer for cell execution time cell_start_time1 = time.time() -sensor_history_A = dict() +sensor_history_A = defaultdict(dict) for timestep in timesteps[1:]: # Generate chosen configuration @@ -327,17 +328,13 @@ for sensor, actions in chosen_action.items(): sensor.add_actions(actions) - sensor_history_at_t = [] for sensor in sensor_setA: sensor.act(timestep) - sensor_history_at_t.append(copy.copy(sensor)) + sensor_history_A[timestep][sensor] = copy.copy(sensor) # Observe this ground truth measurementsA |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) - # Store sensor history for plotting - sensor_history_A[timestep] = sensor_history_at_t - hypotheses = data_associator.associate(tracksA, measurementsA, timestep) @@ -364,46 +361,50 @@ plotterA.plot_ground_truths(truths, [0, 2]) plotterA.plot_tracks(set(tracksA), [0, 2], uncertainty=True, plot_history=False) -# Plot sensor field of view -trace_base = len(plotterA.fig.data) -for _ in sensor_setA: - plotterA.fig.add_trace(go.Scatter(mode='lines', - line=go.scatter.Line(color='black', - dash='dash'))) - -for frame in plotterA.fig.frames: - traces_ = list(frame.traces) - data_ = list(frame.data) - - timestring = frame.name - timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") - - for n, sensor_ in enumerate(sensor_setA): - x = [0, 0] - y = [0, 0] - - if timestamp in sensor_history_A.keys(): - sensor = sensor_history_A[timestamp][n] - for i, fov_side in enumerate((-1, 1)): - range_ = min(getattr(sensor, 'max_range', np.inf), 100) - x[i], y[i] = pol2cart(range_, - sensor.dwell_centre[0, 0] - + sensor.fov_angle / 2 * fov_side) \ - + sensor.position[[0, 1], 0] - else: - continue - - data_.append(go.Scatter(x=[x[0], sensor.position[0], x[1]], - y=[y[0], sensor.position[1], y[1]], - mode="lines", - line=go.scatter.Line(color='black', - dash='dash'), - showlegend=False)) - traces_.append(trace_base + n) - - frame.traces = traces_ - frame.data = data_ +def plot_sensor_fov(fig, sensor_set, sensor_history): + # Plot sensor field of view + trace_base = len(fig.data) + for _ in sensor_set: + fig.add_trace(go.Scatter(mode='lines', + line=go.scatter.Line(color='black', + dash='dash'))) + + for frame in fig.frames: + traces_ = list(frame.traces) + data_ = list(frame.data) + + timestring = frame.name + timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") + + for n, sensor_ in enumerate(sensor_set): + x = [0, 0] + y = [0, 0] + + if timestamp in sensor_history: + sensor = sensor_history[timestamp][sensor_] + for i, fov_side in enumerate((-1, 1)): + range_ = min(getattr(sensor, 'max_range', np.inf), 100) + x[i], y[i] = pol2cart(range_, + sensor.dwell_centre[0, 0] + + sensor.fov_angle / 2 * fov_side) \ + + sensor.position[[0, 1], 0] + else: + continue + + data_.append(go.Scatter(x=[x[0], sensor.position[0], x[1]], + y=[y[0], sensor.position[1], y[1]], + mode="lines", + line=go.scatter.Line(color='black', + dash='dash'), + showlegend=False)) + traces_.append(trace_base + n) + + frame.traces = traces_ + frame.data = data_ + + +plot_sensor_fov(plotterA.fig, sensor_setA, sensor_history_A) plotterA.fig # %% @@ -416,7 +417,7 @@ # Start timer for cell execution time cell_start_time2 = time.time() -sensor_history_B = dict() +sensor_history_B = defaultdict(dict) for timestep in timesteps[1:]: # Generate chosen configuration @@ -429,17 +430,13 @@ for sensor, actions in chosen_action.items(): sensor.add_actions(actions) - sensor_history_at_t = [] for sensor in sensor_setB: sensor.act(timestep) - sensor_history_at_t.append(copy.copy(sensor)) + sensor_history_B[timestep][sensor] = copy.copy(sensor) # Observe this ground truth measurementsB |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) - # Store sensor history for plotting - sensor_history_B[timestep] = sensor_history_at_t - hypotheses = data_associator.associate(tracksB, measurementsB, timestep) @@ -460,48 +457,7 @@ plotterB.plot_sensors(sensor_setB) plotterB.plot_ground_truths(truths, [0, 2]) plotterB.plot_tracks(tracksB, [0, 2], uncertainty=True, plot_history=False) - - -# Plot sensor field of view -trace_base = len(plotterB.fig.data) -for _ in sensor_setB: - plotterB.fig.add_trace(go.Scatter(mode='lines', - line=go.scatter.Line(color='black', - dash='dash'))) - -for frame in plotterB.fig.frames: - traces_ = list(frame.traces) - data_ = list(frame.data) - - timestring = frame.name - timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") - - for n, sensor_ in enumerate(sensor_setB): - x = [0, 0] - y = [0, 0] - - if timestamp in sensor_history_B.keys(): - sensor = sensor_history_B[timestamp][n] - for i, fov_side in enumerate((-1, 1)): - range_ = min(getattr(sensor, 'max_range', np.inf), 100) - x[i], y[i] = pol2cart(range_, - sensor.dwell_centre[0, 0] - + sensor.fov_angle / 2 * fov_side) \ - + sensor.position[[0, 1], 0] - else: - continue - - data_.append(go.Scatter(x=[x[0], sensor.position[0], x[1]], - y=[y[0], sensor.position[1], y[1]], - mode="lines", - line=go.scatter.Line(color='black', - dash='dash'), - showlegend=False)) - traces_.append(trace_base + n) - - frame.traces = traces_ - frame.data = data_ - +plot_sensor_fov(plotterB.fig, sensor_setB, sensor_history_B) plotterB.fig # %% @@ -511,7 +467,7 @@ # Start timer for cell execution time cell_start_time3 = time.time() -sensor_history_C = dict() +sensor_history_C = defaultdict(dict) for timestep in timesteps[1:]: # Generate chosen configuration @@ -524,17 +480,13 @@ for sensor, actions in chosen_action.items(): sensor.add_actions(actions) - sensor_history_at_t = [] for sensor in sensor_setC: sensor.act(timestep) - sensor_history_at_t.append(copy.copy(sensor)) + sensor_history_C[timestep][sensor] = copy.copy(sensor) # Observe this ground truth measurementsC |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True) - # Store sensor history for plotting - sensor_history_C[timestep] = sensor_history_at_t - hypotheses = data_associator.associate(tracksC, measurementsC, timestep) @@ -555,47 +507,7 @@ plotterC.plot_sensors(sensor_setC) plotterC.plot_ground_truths(truths, [0, 2]) plotterC.plot_tracks(tracksC, [0, 2], uncertainty=True, plot_history=False) - -# Plot sensor field of view -trace_base = len(plotterC.fig.data) -for _ in sensor_setC: - plotterC.fig.add_trace(go.Scatter(mode='lines', - line=go.scatter.Line(color='black', - dash='dash'))) - -for frame in plotterC.fig.frames: - traces_ = list(frame.traces) - data_ = list(frame.data) - - timestring = frame.name - timestamp = datetime.strptime(timestring, "%Y-%m-%d %H:%M:%S") - - for n, sensor_ in enumerate(sensor_setC): - x = [0, 0] - y = [0, 0] - - if timestamp in sensor_history_C.keys(): - sensor = sensor_history_C[timestamp][n] - for i, fov_side in enumerate((-1, 1)): - range_ = min(getattr(sensor, 'max_range', np.inf), 100) - x[i], y[i] = pol2cart(range_, - sensor.dwell_centre[0, 0] - + sensor.fov_angle / 2 * fov_side) \ - + sensor.position[[0, 1], 0] - else: - continue - - data_.append(go.Scatter(x=[x[0], sensor.position[0], x[1]], - y=[y[0], sensor.position[1], y[1]], - mode="lines", - line=go.scatter.Line(color='black', - dash='dash'), - showlegend=False)) - traces_.append(trace_base + n) - - frame.traces = traces_ - frame.data = data_ - +plot_sensor_fov(plotterC.fig, sensor_setC, sensor_history_C) plotterC.fig # %%