Merge pull request #22 from hmakelin/hmakelin-dem-altitude-adjustment

Use elevation layer for altitude AGL estimate

Author: hmakelin

README.md

@@ -32,7 +32,7 @@ docker-compose build sitl
 
 > **Note** The build for the `sitl` image takes a long time.
 
-Once the `sitl` image has been built, run the `mapserver` and `px4-sitl` services:
+Once the `sitl` image has been built, run the `mapserver` and `sitl` services:
 
 ```bash
 docker-compose up -d mapserver sitl

docs/pages/setup.rst

@@ -72,7 +72,8 @@ better either through the PX4 shell, through QGroundControl, or in the
     should not be made unreasonably large.
 
     The other parameters are mainly to increase tolerance for variation in the GPS position estimate. GISNav in its
-    default configuration seems to be more accurate in estimating vertical position than horizontal position, so the
+    default configuration `seems to be more accurate in estimating vertical position than horizontal position
+    <https://github.com/hmakelin/gisnav/blob/master/test/sitl/ulog_analysis/variance_estimation.ipynb>`_, so the
     example has lower tolerance for vertical position error.
 
     `*camera yaw rotation speed may be less of an issue if a rotation agnostic neural network is used (not the case by
@@ -298,8 +299,8 @@ You may need to change the file permissions and/or extract it before running it:
 WMS Endpoint
 ===================================================
 The :class:`.BaseNode` class gets map rasters for the estimated location of the vehicle from a WMS endpoint. The WMS
-client :class:`.WMSClient` uses runs in a dedicated process, although it can be quite easily changed to run in a
-separate thread to reduce serialization overhead (no ROS oparameter option currently exists for this, however).
+client :class:`.WMSClient` runs in a dedicated process, although it can be quite easily changed to run in a
+separate thread to reduce serialization overhead (no ROS parameter option currently exists for this, however).
 
 Configure the WMS client via the ROS parameter server, or provide a YAML file when spinning up your node:
 
@@ -320,7 +321,7 @@ WMS Proxy
 ___________________________________________________
 If you already have a third party high-resolution aerial or satellite imagery endpoint available, you only need to
 proxy it through a WMS service. Follow the `gisnav-docker README.md <https://github.com/hmakelin/gisnav-docker>`_ to set
-up a WNS MapProxy using the provided Docker image.
+up a WMS MapProxy using the provided Docker image.
 
 .. note::
     Commercial web-based map services are often
@@ -382,7 +383,7 @@ download the GeoTIFF imagery from EarthExplorer, or from the Esri-maintained `AW
     :caption: Example: Downloading a NAIP imagery product from the AWS S3 bucket
 
     cd ~/gisnav-docker
-    mkdir -p tmp/
+    mkdir -p mapfiles/
     aws s3 cp \
       --request-payer requester \
       s3://naip-source/ca/2020/60cm/rgbir_cog/37122/m_3712230_se_10_060_20200524.tif \

gisnav/autopilots/ardupilot_mavros.py

@@ -4,15 +4,14 @@
 import numpy as np
 from typing import Optional, Callable, get_args
 
-from pygeodesy.geoids import GeoidPGM
 from scipy.spatial.transform import Rotation
 from sensor_msgs.msg import CameraInfo, Image
 from geometry_msgs.msg import PoseStamped
 from sensor_msgs.msg import NavSatFix
-from mavros_msgs.msg import MountControl
+from mavros_msgs.msg import MountControl, HomePosition
 
 from gisnav.assertions import assert_type
-from gisnav.data import TimePair, Attitude
+from gisnav.data import TimePair, Attitude, Position, Altitude
 from gisnav.geo import GeoPoint
 
 from gisnav.autopilots.autopilot import Autopilot
@@ -34,6 +33,10 @@ class ArduPilotMAVROS(Autopilot):
             Autopilot._TOPICS_MSG_KEY: NavSatFix,
             Autopilot._TOPICS_QOS_KEY: rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value
         },
+        '/mavros/home_position/home': {
+            Autopilot._TOPICS_MSG_KEY: HomePosition,
+            Autopilot._TOPICS_QOS_KEY: rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value
+        },
         '/mavros/mount_control/command': {
             Autopilot._TOPICS_MSG_KEY: MountControl,
             Autopilot._TOPICS_QOS_KEY: rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value
@@ -49,12 +52,10 @@ def __init__(self, node: rclpy.node.Node, image_callback: Callable[[Image], None
         """
         super().__init__(node, image_callback)
 
-        # TODO: copy to shared folder and use relative path
-        self._egm96 = GeoidPGM('/usr/share/GeographicLib/geoids/egm96-5.pgm', kind=-3)
-
         self._local_position_pose = None
         self._global_position_global = None
         self._mount_control_command = None
+        self._home_position_home = None
 
         self._time_sync = None
 
@@ -81,6 +82,16 @@ def _global_position_global(self, value: Optional[NavSatFix]) -> None:
         assert_type(value, get_args(Optional[NavSatFix]))
         self.__global_position_global = value
 
+    @property
+    def _home_position_home(self) -> Optional[HomePosition]:
+        """Home position received via MAVROS."""
+        return self.__home_position_home
+
+    @_home_position_home.setter
+    def _home_position_home(self, value: Optional[HomePosition]) -> None:
+        assert_type(value, get_args(Optional[HomePosition]))
+        self.__home_position_home = value
+
     @property
     def _mount_control_command(self) -> Optional[MountControl]:
         """Mount control command received via MAVROS."""
@@ -104,27 +115,6 @@ def _time_sync(self) -> Optional[TimePair]:
     def _time_sync(self, value: Optional[TimePair]) -> None:
         assert_type(value, get_args(Optional[TimePair]))
         self.__time_sync = value
-
-    @property
-    def _egm96(self) -> GeoidPGM:
-        """Geoid for converting MAVROS global position WGS 84 altitude to AMSL altitude
-
-        .. code-block: bash
-            :caption: geographiclib needed
-
-            sudo apt install geographiclib-tools
-            sudo geographiclib-get-geoids egm96-5
-
-        .. seealso::
-            `Avoiding Pitfalls Related to Ellipsoid Height and Height Above Mean Sea Level
-            <https://wiki.ros.org/mavros#mavros.2FPlugins.Avoiding_Pitfalls_Related_to_Ellipsoid_Height_and_Height_Above_Mean_Sea_Level>`_
-        """
-        return self.__egm96
-
-    @_egm96.setter
-    def _egm96(self, value: GeoidPGM) -> None:
-        assert_type(value, GeoidPGM)
-        self.__egm96 = value
     # endregion
 
     # region Private
@@ -184,6 +174,14 @@ def _mount_control_command_callback(self, msg: MountControl) -> None:
         """
         self._mount_control_command = msg
 
+    def _home_position_home_callback(self, msg: HomePosition) -> None:
+        """Handles latest :class:`mavros_msgs.msg.HomePosition` message
+
+        :param msg: :class:`mavros_msgs.msg.HomePosition` via MAVROS
+        :return:
+        """
+        self._home_position_home = msg
+
     def _is_expired(self, message) -> bool:
         """Returns True if the given message is older than :py:attr:`._TELEMETRY_EXPIRATION_LIMIT`
 
@@ -262,20 +260,6 @@ def gimbal_set_attitude(self) -> Optional[np.ndarray]:
         """
         return None
 
-    @property
-    def altitude_agl(self) -> Optional[float]:
-        """Vehicle altitude in meters above ground level (AGL) or None if not available
-
-        .. note::
-            Assumes local position reference altitude is ground level
-        """
-        if self._local_position_pose is not None and hasattr(self._local_position_pose, 'pose') and \
-                hasattr(self._local_position_pose.pose, 'position') and \
-                hasattr(self._local_position_pose.pose.position, 'z'):
-            return self._local_position_pose.pose.position.z
-
-        return None
-
     @property
     def altitude_amsl(self) -> Optional[float]:
         """Vehicle altitude in meters above mean sea level (AMSL) or None if not available"""
@@ -288,44 +272,85 @@ def altitude_amsl(self) -> Optional[float]:
 
     @property
     def altitude_ellipsoid(self) -> Optional[float]:
-        """Vehicle altitude in meters above WGS 84 ellipsoid or None if not available"""
+        """Vehicle altitude in meters above WGS 84 ellipsoid or None if not available
+
+        .. note::
+            Overrides parent implementation, which should also work
+        """
         if self._global_position_global is not None and hasattr(self._global_position_global, 'altitude') and \
                 self.global_position is not None:
             return self._global_position_global.altitude
         else:
             return None
 
     @property
-    def ground_elevation_amsl(self) -> Optional[float]:
-        """Ground elevation in meters above mean sea level (AMSL) or None if information is not available
+    def global_position(self) -> Optional[GeoPoint]:
+        """Returns vehicle global position as a :class:`.GeoPoint`"""
+        if self._global_position_global is not None and hasattr(self._global_position_global, 'latitude') \
+                and hasattr(self._global_position_global, 'longitude'):
+            return GeoPoint(self._global_position_global.longitude, self._global_position_global.latitude,
+                            crs='epsg:4326')
+        else:
+            return None
 
-        .. note::
-            Ground elevation assumed to be same as local position reference altitude.
-            TODO: use DEM raster
+    @property
+    def altitude_home(self) -> Optional[float]:
+        """Vehicle altitude in meters above home (starting) position or None if not available
 
         .. seealso::
-            :py:attr:`.altitude_agl`
+            `Altitude definitions <https://ardupilot.org/copter/docs/common-understanding-altitude.html>`_
         """
-        if self.altitude_amsl is not None and self.altitude_agl is not None:
-            return self.altitude_amsl - self.altitude_agl
+        if self._local_position_pose is not None and hasattr(self._local_position_pose, 'position') and \
+                hasattr(self._local_position_pose.position, 'z'):
+            return self._local_position_pose.position.z
         else:
             return None
 
-    # TODO: move to base class?
     @property
-    def ground_elevation_ellipsoid(self) -> Optional[float]:
-        """Ground elevation in meters above WGS 84 ellipsoid or None if information is not available"""
-        if self.altitude_ellipsoid is not None and self.altitude_agl is not None:
-            return self.altitude_ellipsoid - self.altitude_agl
+    def home_altitude_amsl(self) -> Optional[float]:
+        """Home (starting position) altitude in meters above mean sea level (AMSL) or None if not available
+
+        .. note::
+            Uses :class:`.VehicleGlobaLPosition` ``ref_alt`` attribute to determine Home altitude
+
+        .. seealso::
+            `Altitude definitions <https://ardupilot.org/copter/docs/common-understanding-altitude.html>`_
+        """
+        if self._home_position_home is not None and \
+                hasattr(self._home_position_home, 'geo') and \
+                hasattr(self._home_position_home.geo, 'altitude'):
+            return self._home_position_home.geo.altitude - self._egm96.height(self.global_position.lat,
+                                                                              self.global_position.lon)
         else:
             return None
 
     @property
-    def global_position(self) -> Optional[GeoPoint]:
-        """Returns vehicle global position as a :class:`.GeoPoint`"""
-        if self._global_position_global is not None and hasattr(self._global_position_global, 'latitude') \
-                and hasattr(self._global_position_global, 'longitude'):
-            return GeoPoint(self._global_position_global.longitude, self._global_position_global.latitude,
-                            crs='epsg:4326')
+    def local_frame_origin(self) -> Optional[Position]:
+        """Vehicle local frame origin as a :class:`.Position`
+
+        .. warning::
+            Assumes local frame origin is on ground (starting location)
+        """
+        if self._home_position_home is not None and \
+                hasattr(self._home_position_home, 'geo') and \
+                hasattr(self._home_position_home.geo, 'latitude') and \
+                hasattr(self._home_position_home.geo, 'longitude') and \
+                hasattr(self._home_position_home.geo, 'altitude'):
+            home_altitude_amsl = self.home_altitude_amsl
+
+            altitude = Altitude(
+                agl=0,  # local frame origin assumed on ground level
+                amsl=home_altitude_amsl,
+                ellipsoid=self._home_position_home.geo.altitude,
+                home=0
+            )
+            position = Position(
+                xy=GeoPoint(self._home_position_home.geo.longitude, self._home_position_home.geo.latitude,
+                            crs='epsg:4326'),
+                altitude=altitude,
+                attitude=None,
+                timestamp=None
+            )
+            return position
         else:
             return None