Plot FOV on Sensor Management tutorials

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,56 @@
             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.
+
+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(sensorA)
 plotterA.plot_ground_truths(truths, [0, 2])
 plotterA.plot_tracks(tracksA, [0, 2], uncertainty=True, plot_history=False)
+
+
+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
 
 # %%
@@ -405,6 +455,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 +471,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 +491,11 @@
 # %%
 # 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_fov(plotterB.fig, sensor_history_B)
 plotterB.fig
 
 # %%

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,10 @@
 # :class:`~.ChangeDwellAction`, selected at random.
 
 from ordered_set import OrderedSet
+from collections import defaultdict
+import copy
 
+sensor_history_A = defaultdict(dict)
 for timestep in timesteps[1:]:
 
     # Generate chosen configuration
@@ -322,6 +325,7 @@
 
     for sensor in sensor_setA:
         sensor.act(timestep)
+        sensor_history_A[timestep][sensor] = copy.copy(sensor)
 
         # Observe this ground truth
         measurementsA |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True)
@@ -339,12 +343,61 @@
 
 # %%
 # 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.
 
-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(sensor_setA)
 plotterA.plot_ground_truths(truths, [0, 2])
 plotterA.plot_tracks(tracksA, [0, 2], uncertainty=True, plot_history=False)
+
+
+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
 
 # %%
@@ -378,6 +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 = defaultdict(dict)
 for timestep in timesteps[1:]:
 
     # Generate chosen configuration
@@ -392,6 +446,7 @@
 
     for sensor in sensor_setB:
         sensor.act(timestep)
+        sensor_history_B[timestep][sensor] = copy.copy(sensor)
 
         # Observe this ground truth
         measurementsB |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True)
@@ -410,10 +465,11 @@
 # %%
 # 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_fov(plotterB.fig, sensor_setB, sensor_history_B)
 plotterB.fig
 
 # %%

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
 
@@ -308,11 +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 = defaultdict(dict)
 for timestep in timesteps[1:]:
 
     # Generate chosen configuration
@@ -327,6 +330,7 @@
 
     for sensor in sensor_setA:
         sensor.act(timestep)
+        sensor_history_A[timestep][sensor] = copy.copy(sensor)
 
         # Observe this ground truth
         measurementsA |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True)
@@ -346,12 +350,61 @@
 
 # %%
 # 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.
 
-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(sensor_setA)
 plotterA.plot_ground_truths(truths, [0, 2])
 plotterA.plot_tracks(set(tracksA), [0, 2], uncertainty=True, plot_history=False)
+
+
+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
 
 # %%
@@ -364,6 +417,7 @@
 # Start timer for cell execution time
 cell_start_time2 = time.time()
 
+sensor_history_B = defaultdict(dict)
 for timestep in timesteps[1:]:
 
     # Generate chosen configuration
@@ -378,6 +432,7 @@
 
     for sensor in sensor_setB:
         sensor.act(timestep)
+        sensor_history_B[timestep][sensor] = copy.copy(sensor)
 
         # Observe this ground truth
         measurementsB |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True)
@@ -398,10 +453,11 @@
 # %%
 # 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_fov(plotterB.fig, sensor_setB, sensor_history_B)
 plotterB.fig
 
 # %%
@@ -411,6 +467,7 @@
 # Start timer for cell execution time
 cell_start_time3 = time.time()
 
+sensor_history_C = defaultdict(dict)
 for timestep in timesteps[1:]:
 
     # Generate chosen configuration
@@ -425,6 +482,7 @@
 
     for sensor in sensor_setC:
         sensor.act(timestep)
+        sensor_history_C[timestep][sensor] = copy.copy(sensor)
 
         # Observe this ground truth
         measurementsC |= sensor.measure(OrderedSet(truth[timestep] for truth in truths), noise=True)
@@ -445,10 +503,11 @@
 # %%
 # 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_fov(plotterC.fig, sensor_setC, sensor_history_C)
 plotterC.fig
 
 # %%