Merge pull request #84 from Stanford-NavLab/derek/viz-axes

Derek/viz axes

Author: kanhereashwin

gnss_lib_py/utils/coordinates.py

@@ -33,6 +33,7 @@
 import numpy as np
 
 import gnss_lib_py.utils.constants as consts
+from gnss_lib_py.parsers.navdata import NavData
 
 EPSILON = 1e-7
 
@@ -467,3 +468,81 @@ def ecef_to_el_az(rx_pos, sv_pos):
         el_az[:,1][el_az[:,1] < 0] += 360
 
     return el_az
+
+def add_el_az(navdata, receiver_state, inplace=False):
+    """Adds elevation and azimuth to NavData object.
+
+    Parameters
+    ----------
+    navdata : gnss_lib_py.parsers.navdata.NavData
+        Instance of the NavData class. Must include ``gps_millis`` as
+        well as satellite ECEF positions as ``x_sv_m``, ``y_sv_m``,
+        ``z_sv_m``, ``gnss_id`` and ``sv_id``.
+    receiver_state : gnss_lib_py.parsers.navdata.NavData
+        Either estimated or ground truth receiver position in ECEF frame
+        in meters as an instance of the NavData class with the
+        following rows: ``x_*_m``, ``y_*_m``, ``z_*_m``, ``gps_millis``.
+    inplace : bool
+        If false (default) will add elevation and azimuth to a new
+        NavData instance. If true, will add elevation and azimuth to the
+        existing NavData instance.
+
+    Returns
+    -------
+    data_el_az : gnss_lib_py.parsers.navdata.NavData
+        If inplace is True, adds ``el_sv_deg`` and ``az_sv_deg`` to
+        the input navdata and returns the same object.
+        If inplace is False, returns ``el_sv_deg`` and ``az_sv_deg``
+        in a new NavData instance along with ``gps_millis`` and the
+        corresponding satellite and receiver rows.
+
+    """
+
+    # check for missing rows
+    navdata.in_rows(["gps_millis","x_sv_m","y_sv_m","z_sv_m",
+                     "gnss_id","sv_id"])
+    receiver_state.in_rows(["gps_millis"])
+
+    # check for receiver_state indexes
+    rx_idxs = receiver_state.find_wildcard_indexes(["x_*_m","y_*_m",
+                                                    "z_*_m"],max_allow=1)
+
+    sv_el_az = None
+    for timestamp, _, navdata_subset in navdata.loop_time("gps_millis"):
+
+        pos_sv_m = navdata_subset[["x_sv_m","y_sv_m","z_sv_m"]].T
+
+        # find time index for receiver_state NavData instance
+        rx_t_idx = np.argmin(np.abs(receiver_state["gps_millis"] - timestamp))
+
+        pos_rx_m = receiver_state[[rx_idxs["x_*_m"][0],
+                                   rx_idxs["y_*_m"][0],
+                                   rx_idxs["z_*_m"][0]],
+                                   rx_t_idx].reshape(1,-1)
+
+        timestep_el_az = ecef_to_el_az(pos_rx_m, pos_sv_m)
+
+        if sv_el_az is None:
+            sv_el_az = timestep_el_az.T
+        else:
+            sv_el_az = np.hstack((sv_el_az,timestep_el_az.T))
+
+    if inplace:
+        navdata["el_sv_deg"] = sv_el_az[0,:]
+        navdata["az_sv_deg"] = sv_el_az[1,:]
+        return navdata
+
+    data_el_az = NavData()
+    data_el_az["gps_millis"] = navdata["gps_millis"]
+    data_el_az["gnss_id"] = navdata["gnss_id"]
+    data_el_az["sv_id"] = navdata["sv_id"]
+    data_el_az["x_sv_m"] = navdata["x_sv_m"]
+    data_el_az["y_sv_m"] = navdata["y_sv_m"]
+    data_el_az["z_sv_m"] = navdata["z_sv_m"]
+    data_el_az[rx_idxs["x_*_m"][0]] = receiver_state[rx_idxs["x_*_m"][0]]
+    data_el_az[rx_idxs["y_*_m"][0]] = receiver_state[rx_idxs["y_*_m"][0]]
+    data_el_az[rx_idxs["z_*_m"][0]] = receiver_state[rx_idxs["z_*_m"][0]]
+    data_el_az["el_sv_deg"] = sv_el_az[0,:]
+    data_el_az["az_sv_deg"] = sv_el_az[1,:]
+
+    return data_el_az

gnss_lib_py/utils/visualizations.py

@@ -21,7 +21,7 @@
 
 import gnss_lib_py.utils.file_operations as fo
 from gnss_lib_py.parsers.navdata import NavData
-from gnss_lib_py.utils.coordinates import ecef_to_el_az
+from gnss_lib_py.utils.coordinates import add_el_az
 
 STANFORD_COLORS = [
                    "#8C1515",   # cardinal red
@@ -274,39 +274,9 @@ def plot_skyplot(navdata, receiver_state, save=False, prefix="",
 
     if not isinstance(prefix, str):
         raise TypeError("Prefix must be a string.")
-    # check for missing rows
-    navdata.in_rows(["gps_millis","x_sv_m","y_sv_m","z_sv_m",
-                     "gnss_id","sv_id"])
-    receiver_state.in_rows(["gps_millis"])
-
-    # check for receiver_state indexes
-    rx_idxs = receiver_state.find_wildcard_indexes(["x_*_m","y_*_m",
-                                                    "z_*_m"],max_allow=1)
 
     if "el_sv_deg" not in navdata.rows or "az_sv_deg" not in navdata.rows:
-        sv_el_az = None
-
-        for timestamp, _, navdata_subset in navdata.loop_time("gps_millis"):
-
-            pos_sv_m = navdata_subset[["x_sv_m","y_sv_m","z_sv_m"]].T
-
-            # find time index for receiver_state NavData instance
-            rx_t_idx = np.argmin(np.abs(receiver_state["gps_millis"] - timestamp))
-
-            pos_rx_m = receiver_state[[rx_idxs["x_*_m"][0],
-                                       rx_idxs["y_*_m"][0],
-                                       rx_idxs["z_*_m"][0]],
-                                       rx_t_idx].reshape(1,-1)
-
-            timestep_el_az = ecef_to_el_az(pos_rx_m, pos_sv_m)
-
-            if sv_el_az is None:
-                sv_el_az = timestep_el_az.T
-            else:
-                sv_el_az = np.hstack((sv_el_az,timestep_el_az.T))
-
-        navdata["el_sv_deg"] = sv_el_az[0,:]
-        navdata["az_sv_deg"] = sv_el_az[1,:]
+        add_el_az(navdata, receiver_state, inplace=True)
 
     # create new figure
     fig = plt.figure(figsize=(6,4.5))
@@ -579,6 +549,23 @@ def _get_label(inputs):
     if not isinstance(inputs,dict):
         raise TypeError("_get_label input must be dictionary.")
 
+    # handle units specially.
+    units = {"m","km",
+             "deg","rad",
+             "sec","s","hr","min",
+             "mps","kmph","mph",
+             "dgps","radps",
+             "mps2",
+             }
+    unit_replacements = {
+                         "mps" : "m/s",
+                         "kmph" : "km/hr",
+                         "mph" : "miles/hr",
+                         "degps" : "deg/s",
+                         "radps" : "rad/s",
+                         "mps2" : "m/s^2",
+                        }
+
     label = ""
     for key, value in inputs.items():
 
@@ -593,8 +580,14 @@ def _get_label(inputs):
         except ValueError:
             pass
 
-        value = value.upper()
-        value = value.replace("_"," ")
+        value = value.split("_")
+        if value[-1] in units:
+            # make units lowercase and bracketed.
+            if value[-1] in unit_replacements:
+                value[-1] = unit_replacements[value[-1]]
+            value = " ".join(value[:-1]).upper() + " [" + value[-1] + "]"
+        else:
+            value = " ".join(value).upper()
 
         if key == "gnss_id": # use GNSS specific capitalization
             constellation_map = {"GALILEO" : "Galileo",

tests/utils/test_coordinates.py

@@ -13,7 +13,7 @@
 
 import gnss_lib_py.utils.constants as consts
 from gnss_lib_py.parsers.android import AndroidDerived2022
-from gnss_lib_py.utils.coordinates import ecef_to_el_az
+from gnss_lib_py.utils.coordinates import ecef_to_el_az, add_el_az
 from gnss_lib_py.utils.coordinates import geodetic_to_ecef
 from gnss_lib_py.utils.coordinates import ecef_to_geodetic, LocalCoord
 
@@ -409,3 +409,33 @@ def test_ecef_to_el_az_fails(set_sv_pos, set_rx_pos):
     with pytest.raises(RuntimeError) as excinfo:
         ecef_to_el_az(set_rx_pos,set_sv_pos.T)
     assert "Nx3" in str(excinfo.value)
+
+
+@pytest.mark.parametrize('navdata',[
+                                    lazy_fixture('derived_2022'),
+                                    ])
+def test_add_el_az(navdata):
+    """Test for plotting skyplot.
+
+    Parameters
+    ----------
+    navdata : AndroidDerived
+        Instance of AndroidDerived for testing.
+
+    """
+
+    receiver_state = navdata.copy(rows=["gps_millis",
+                                        "WlsPositionXEcefMeters",
+                                        "WlsPositionYEcefMeters",
+                                        "WlsPositionZEcefMeters"])
+    row_map = {
+               "WlsPositionXEcefMeters" : "x_rx_m",
+               "WlsPositionYEcefMeters" : "y_rx_m",
+               "WlsPositionZEcefMeters" : "z_rx_m",
+                }
+    receiver_state.rename(row_map,inplace=True)
+    data_el_az = add_el_az(navdata, receiver_state)
+    np.testing.assert_array_almost_equal(data_el_az["el_sv_deg"],
+                                         navdata["el_sv_deg"])
+    np.testing.assert_array_almost_equal(data_el_az["az_sv_deg"],
+                                         navdata["az_sv_deg"])

tests/utils/test_visualizations.py

@@ -369,11 +369,11 @@ def test_plot_skyplot(navdata, state_estimate):
             navdata["x_sv_m",col_idx] = np.nan
 
     # don't save figures
-    fig = viz.plot_skyplot(navdata, state_estimate, save=True)
+    fig = viz.plot_skyplot(navdata.copy(), state_estimate, save=False)
     viz.close_figures(fig)
 
     with pytest.raises(TypeError) as excinfo:
-        viz.plot_skyplot(navdata, state_estimate, save=True, prefix=1)
+        viz.plot_skyplot(navdata.copy(), state_estimate, save=True, prefix=1)
     assert "Prefix" in str(excinfo.value)
 
     for row in ["x_sv_m","y_sv_m","z_sv_m","gps_millis"]:
@@ -415,6 +415,10 @@ def test_get_label():
     assert viz._get_label({"gnss_id" : "galileo",
                            "signal_type" : "b1i"}) == "Galileo B1i"
 
+    assert viz._get_label({"row" : "x_rx_m"}) == "X RX [m]"
+    assert viz._get_label({"row" : "lat_rx_deg"}) == "LAT RX [deg]"
+    assert viz._get_label({"row" : "vx_sv_mps"}) == "VX SV [m/s]"
+
     with pytest.raises(TypeError) as excinfo:
         viz._get_label(["should","fail"])
     assert "dictionary" in str(excinfo.value)