1. Fix alt_ellipsoid property in px4_micrortps.py. PX4 VehicleGlobalPosition message in SITL simulation seems to give an invalid value so it is calculated from altitude AMSL using egm96 geoid instead. 2. Rename _camera_pitch_too_high to _camera_roll_or_pitch_too_high and check for roll as well. This is relevant for the static camera case in the ArduPilot gazebo-iris simulation which is implemented here. This is less relevant for the roll-stabilized gimbal on the PX4 simulated Typhoon H480 model but a TODO is added for that as well.

Author: hmakelin

gisnav/autopilots/px4_micrortps.py

@@ -8,6 +8,7 @@
 from sensor_msgs.msg import CameraInfo, Image
 from px4_msgs.msg import VehicleAttitude, VehicleLocalPosition, VehicleGlobalPosition, GimbalDeviceAttitudeStatus, \
     GimbalDeviceSetAttitude
+from pygeodesy.geoids import GeoidPGM
 
 from gisnav.assertions import assert_type
 from gisnav.data import TimePair, Attitude
@@ -59,6 +60,8 @@ def __init__(self, node: rclpy.node.Node, camera_info_topic: str, image_topic: s
         self._gimbal_attitude = None
         self._time_sync = None
 
+        self._egm96 = GeoidPGM('/usr/share/GeographicLib/geoids/egm96-5.pgm', kind=-3)
+
     # region Properties
     @property
     def _vehicle_local_position(self) -> Optional[VehicleLocalPosition]:
@@ -113,6 +116,30 @@ 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 getting alt_ellipsoid from alt (AMSL) altitude
+
+        It looks like the PX4 SITL simulation VehcileGlobalPosition provides an invalid alt_ellipsoid so this is used
+        to calculate it from alt (AMSL) instead.
+
+        .. 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
@@ -288,11 +315,21 @@ def altitude_amsl(self) -> Optional[float]:
     def altitude_ellipsoid(self) -> Optional[float]:
         """Vehicle altitude in meters above WGS 84 ellipsoid or None if not available
 
+        .. note::
+            Looks like the PX4 SITL returns an invalid alt_ellipsoid in the VehicleGlobalPosition message:
+            ``alt_ellipsoid: -1844674428928.0``. Ellipsoid altitude is therefore calculated from altitude AMSL using
+            egm96 geoid instead.
+
         .. seealso::
             :py:attr:`.altitude_amsl`
         """
-        if self._vehicle_global_position is not None and hasattr(self._vehicle_global_position, 'alt_ellipsoid'):
-            return self._vehicle_global_position.alt_ellipsoid
+        # alt_ellipsoid invalid -- commented out
+        #if self._vehicle_global_position is not None and hasattr(self._vehicle_global_position, 'alt_ellipsoid'):
+        #    return self._vehicle_global_position.alt_ellipsoid
+        #else:
+        #    return None
+        if self.altitude_amsl is not None and self.global_position is not None:
+            return self.altitude_amsl + self._egm96.height(self.global_position.lat, self.global_position.lon)
         else:
             return None
 

gisnav/data.py

@@ -558,8 +558,10 @@ def _estimate_position(self, ground_elevation: Optional[float], ground_elevation
             # Not a valid position estimate
             return None
 
+
         lon, lat = t_wgs84.squeeze()[1::-1]
         alt = t_wgs84[2]
+
         position = Position(
             xy=GeoPoint(lon, lat, crs),  # lon-lat order
             z_ground=alt,

gisnav/nodes/base_node.py

@@ -923,7 +923,7 @@ def _should_request_new_map(self, bbox: GeoSquare) -> bool:
         use_gimbal_projection = self.get_parameter('map_update.gimbal_projection').get_parameter_value().bool_value
         if use_gimbal_projection:
             max_pitch = self.get_parameter('map_update.max_pitch').get_parameter_value().integer_value
-            if self._camera_pitch_too_high(max_pitch):
+            if self._camera_roll_or_pitch_too_high(max_pitch):
                 self.get_logger().warn(f'Camera pitch not available or above maximum {max_pitch}. Will not update map.')
                 return False
 
@@ -1091,7 +1091,8 @@ def _should_estimate(self, img: np.ndarray) -> bool:
         """Determines whether :meth:`._estimate` should be called
 
         Match should be attempted if (1) a reference map has been retrieved, (2) there are no pending match results,
-        (3) camera pitch is not too high (e.g. facing horizon instead of nadir), and (4) drone is not flying too low.
+        (3) camera roll or pitch is not too high (e.g. facing horizon instead of nadir), and (4) drone is not flying
+        too low.
 
         :param img: Image from which to estimate pose
         :return: True if pose estimation be attempted
@@ -1106,10 +1107,11 @@ def _should_estimate(self, img: np.ndarray) -> bool:
             self.get_logger().debug(f'Previous pose estimation results pending. Skipping pose estimation.')
             return False
 
-        # Check condition (3) - whether camera pitch is too large
+        # Check condition (3) - whether camera roll/pitch is too large
         max_pitch = self.get_parameter('misc.max_pitch').get_parameter_value().integer_value
-        if self._camera_pitch_too_high(max_pitch):
-            self.get_logger().warn(f'Camera pitch not available or above limit {max_pitch}. Skipping pose estimation.')
+        if self._camera_roll_or_pitch_too_high(max_pitch):
+            self.get_logger().warn(f'Camera roll or pitch not available or above limit {max_pitch}. Skipping pose '
+                                   f'estimation.')
             return False
 
         # Check condition (4) - whether vehicle altitude is too low
@@ -1205,11 +1207,11 @@ def _is_valid_estimate(self, fixed_camera: FixedCamera, input_data: InputData) -
     # endregion
 
     # region Shared Logic
-    def _camera_pitch_too_high(self, max_pitch: Union[int, float]) -> bool:
-        """Returns True if (set) camera pitch exceeds given limit OR camera pitch is unknown
+    def _camera_roll_or_pitch_too_high(self, max_pitch: Union[int, float]) -> bool:
+        """Returns True if (set) camera roll or pitch exceeds given limit OR camera pitch is unknown
 
-        Used to determine whether camera pitch setting is too high up from nadir to make matching against a map
-        not worthwhile.
+        Used to determine whether camera roll or pitch is too high up from nadir to make matching against a map
+        not worthwhile. Checks roll for static camera, but assumes zero roll for 2-axis gimbal (static_camera: False).
 
         .. note::
             Uses actual vehicle attitude (instead of gimbal set attitude) if static_camera ROS param is True
@@ -1221,6 +1223,7 @@ def _camera_pitch_too_high(self, max_pitch: Union[int, float]) -> bool:
         static_camera = self.get_parameter('misc.static_camera').get_parameter_value().bool_value
         pitch = None
         if self._bridge.gimbal_set_attitude is not None and not static_camera:
+            # TODO: do not assume zero roll here - camera attitude handling needs refactoring
             # +90 degrees to re-center from FRD frame to nadir-facing camera as origin for max pitch comparison
             pitch = np.degrees(self._bridge.gimbal_set_attitude.pitch) + 90
         else:
@@ -1232,7 +1235,7 @@ def _camera_pitch_too_high(self, max_pitch: Union[int, float]) -> bool:
                     self.get_logger().warn('Vehicle attitude was not available, assuming static camera pitch too high.')
                     return True
                 else:
-                    pitch = self._bridge.attitude.pitch
+                    pitch = max(self._bridge.attitude.pitch, self._bridge.attitude.roll)
 
         assert pitch is not None
         if pitch > max_pitch: