Skip to content

Commit 323e07f

Browse files
committed
1. Fix terrain altitude handling when using DEM and add new altitude definitions. 2. Fix UDP socket close bug, remove commtented out code and fix indentation issues in mock GPS node.C
1 parent b749dc0 commit 323e07f

File tree

6 files changed

+526
-318
lines changed

6 files changed

+526
-318
lines changed

gisnav/autopilots/ardupilot_mavros.py

Lines changed: 86 additions & 61 deletions
Original file line numberDiff line numberDiff line change
@@ -4,15 +4,14 @@
44
import numpy as np
55
from typing import Optional, Callable, get_args
66

7-
from pygeodesy.geoids import GeoidPGM
87
from scipy.spatial.transform import Rotation
98
from sensor_msgs.msg import CameraInfo, Image
109
from geometry_msgs.msg import PoseStamped
1110
from sensor_msgs.msg import NavSatFix
12-
from mavros_msgs.msg import MountControl
11+
from mavros_msgs.msg import MountControl, HomePosition
1312

1413
from gisnav.assertions import assert_type
15-
from gisnav.data import TimePair, Attitude
14+
from gisnav.data import TimePair, Attitude, Position, Altitude
1615
from gisnav.geo import GeoPoint
1716

1817
from gisnav.autopilots.autopilot import Autopilot
@@ -34,6 +33,10 @@ class ArduPilotMAVROS(Autopilot):
3433
Autopilot._TOPICS_MSG_KEY: NavSatFix,
3534
Autopilot._TOPICS_QOS_KEY: rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value
3635
},
36+
'/mavros/home_position/home': {
37+
Autopilot._TOPICS_MSG_KEY: HomePosition,
38+
Autopilot._TOPICS_QOS_KEY: rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value
39+
},
3740
'/mavros/mount_control/command': {
3841
Autopilot._TOPICS_MSG_KEY: MountControl,
3942
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
4952
"""
5053
super().__init__(node, image_callback)
5154

52-
# TODO: copy to shared folder and use relative path
53-
self._egm96 = GeoidPGM('/usr/share/GeographicLib/geoids/egm96-5.pgm', kind=-3)
54-
5555
self._local_position_pose = None
5656
self._global_position_global = None
5757
self._mount_control_command = None
58+
self._home_position_home = None
5859

5960
self._time_sync = None
6061

@@ -81,6 +82,16 @@ def _global_position_global(self, value: Optional[NavSatFix]) -> None:
8182
assert_type(value, get_args(Optional[NavSatFix]))
8283
self.__global_position_global = value
8384

85+
@property
86+
def _home_position_home(self) -> Optional[HomePosition]:
87+
"""Home position received via MAVROS."""
88+
return self.__home_position_home
89+
90+
@_home_position_home.setter
91+
def _home_position_home(self, value: Optional[HomePosition]) -> None:
92+
assert_type(value, get_args(Optional[HomePosition]))
93+
self.__home_position_home = value
94+
8495
@property
8596
def _mount_control_command(self) -> Optional[MountControl]:
8697
"""Mount control command received via MAVROS."""
@@ -104,27 +115,6 @@ def _time_sync(self) -> Optional[TimePair]:
104115
def _time_sync(self, value: Optional[TimePair]) -> None:
105116
assert_type(value, get_args(Optional[TimePair]))
106117
self.__time_sync = value
107-
108-
@property
109-
def _egm96(self) -> GeoidPGM:
110-
"""Geoid for converting MAVROS global position WGS 84 altitude to AMSL altitude
111-
112-
.. code-block: bash
113-
:caption: geographiclib needed
114-
115-
sudo apt install geographiclib-tools
116-
sudo geographiclib-get-geoids egm96-5
117-
118-
.. seealso::
119-
`Avoiding Pitfalls Related to Ellipsoid Height and Height Above Mean Sea Level
120-
<https://wiki.ros.org/mavros#mavros.2FPlugins.Avoiding_Pitfalls_Related_to_Ellipsoid_Height_and_Height_Above_Mean_Sea_Level>`_
121-
"""
122-
return self.__egm96
123-
124-
@_egm96.setter
125-
def _egm96(self, value: GeoidPGM) -> None:
126-
assert_type(value, GeoidPGM)
127-
self.__egm96 = value
128118
# endregion
129119

130120
# region Private
@@ -184,6 +174,14 @@ def _mount_control_command_callback(self, msg: MountControl) -> None:
184174
"""
185175
self._mount_control_command = msg
186176

177+
def _home_position_home_callback(self, msg: HomePosition) -> None:
178+
"""Handles latest :class:`mavros_msgs.msg.HomePosition` message
179+
180+
:param msg: :class:`mavros_msgs.msg.HomePosition` via MAVROS
181+
:return:
182+
"""
183+
self._home_position_home = msg
184+
187185
def _is_expired(self, message) -> bool:
188186
"""Returns True if the given message is older than :py:attr:`._TELEMETRY_EXPIRATION_LIMIT`
189187
@@ -262,20 +260,6 @@ def gimbal_set_attitude(self) -> Optional[np.ndarray]:
262260
"""
263261
return None
264262

265-
@property
266-
def altitude_agl(self) -> Optional[float]:
267-
"""Vehicle altitude in meters above ground level (AGL) or None if not available
268-
269-
.. note::
270-
Assumes local position reference altitude is ground level
271-
"""
272-
if self._local_position_pose is not None and hasattr(self._local_position_pose, 'pose') and \
273-
hasattr(self._local_position_pose.pose, 'position') and \
274-
hasattr(self._local_position_pose.pose.position, 'z'):
275-
return self._local_position_pose.pose.position.z
276-
277-
return None
278-
279263
@property
280264
def altitude_amsl(self) -> Optional[float]:
281265
"""Vehicle altitude in meters above mean sea level (AMSL) or None if not available"""
@@ -288,44 +272,85 @@ def altitude_amsl(self) -> Optional[float]:
288272

289273
@property
290274
def altitude_ellipsoid(self) -> Optional[float]:
291-
"""Vehicle altitude in meters above WGS 84 ellipsoid or None if not available"""
275+
"""Vehicle altitude in meters above WGS 84 ellipsoid or None if not available
276+
277+
.. note::
278+
Overrides parent implementation, which should also work
279+
"""
292280
if self._global_position_global is not None and hasattr(self._global_position_global, 'altitude') and \
293281
self.global_position is not None:
294282
return self._global_position_global.altitude
295283
else:
296284
return None
297285

298286
@property
299-
def ground_elevation_amsl(self) -> Optional[float]:
300-
"""Ground elevation in meters above mean sea level (AMSL) or None if information is not available
287+
def global_position(self) -> Optional[GeoPoint]:
288+
"""Returns vehicle global position as a :class:`.GeoPoint`"""
289+
if self._global_position_global is not None and hasattr(self._global_position_global, 'latitude') \
290+
and hasattr(self._global_position_global, 'longitude'):
291+
return GeoPoint(self._global_position_global.longitude, self._global_position_global.latitude,
292+
crs='epsg:4326')
293+
else:
294+
return None
301295

302-
.. note::
303-
Ground elevation assumed to be same as local position reference altitude.
304-
TODO: use DEM raster
296+
@property
297+
def altitude_home(self) -> Optional[float]:
298+
"""Vehicle altitude in meters above home (starting) position or None if not available
305299
306300
.. seealso::
307-
:py:attr:`.altitude_agl`
301+
`Altitude definitions <https://ardupilot.org/copter/docs/common-understanding-altitude.html>`_
308302
"""
309-
if self.altitude_amsl is not None and self.altitude_agl is not None:
310-
return self.altitude_amsl - self.altitude_agl
303+
if self._local_position_pose is not None and hasattr(self._local_position_pose, 'position') and \
304+
hasattr(self._local_position_pose.position, 'z'):
305+
return self._local_position_pose.position.z
311306
else:
312307
return None
313308

314-
# TODO: move to base class?
315309
@property
316-
def ground_elevation_ellipsoid(self) -> Optional[float]:
317-
"""Ground elevation in meters above WGS 84 ellipsoid or None if information is not available"""
318-
if self.altitude_ellipsoid is not None and self.altitude_agl is not None:
319-
return self.altitude_ellipsoid - self.altitude_agl
310+
def home_altitude_amsl(self) -> Optional[float]:
311+
"""Home (starting position) altitude in meters above mean sea level (AMSL) or None if not available
312+
313+
.. note::
314+
Uses :class:`.VehicleGlobaLPosition` ``ref_alt`` attribute to determine Home altitude
315+
316+
.. seealso::
317+
`Altitude definitions <https://ardupilot.org/copter/docs/common-understanding-altitude.html>`_
318+
"""
319+
if self._home_position_home is not None and \
320+
hasattr(self._home_position_home, 'geo') and \
321+
hasattr(self._home_position_home.geo, 'altitude'):
322+
return self._home_position_home.geo.altitude - self._egm96.height(self.global_position.lat,
323+
self.global_position.lon)
320324
else:
321325
return None
322326

323327
@property
324-
def global_position(self) -> Optional[GeoPoint]:
325-
"""Returns vehicle global position as a :class:`.GeoPoint`"""
326-
if self._global_position_global is not None and hasattr(self._global_position_global, 'latitude') \
327-
and hasattr(self._global_position_global, 'longitude'):
328-
return GeoPoint(self._global_position_global.longitude, self._global_position_global.latitude,
329-
crs='epsg:4326')
328+
def local_frame_origin(self) -> Optional[Position]:
329+
"""Vehicle local frame origin as a :class:`.Position`
330+
331+
.. warning::
332+
Assumes local frame origin is on ground (starting location)
333+
"""
334+
if self._home_position_home is not None and \
335+
hasattr(self._home_position_home, 'geo') and \
336+
hasattr(self._home_position_home.geo, 'latitude') and \
337+
hasattr(self._home_position_home.geo, 'longitude') and \
338+
hasattr(self._home_position_home.geo, 'altitude'):
339+
home_altitude_amsl = self.home_altitude_amsl
340+
341+
altitude = Altitude(
342+
agl=0, # local frame origin assumed on ground level
343+
amsl=home_altitude_amsl,
344+
ellipsoid=self._home_position_home.geo.altitude,
345+
home=0
346+
)
347+
position = Position(
348+
xy=GeoPoint(self._home_position_home.geo.longitude, self._home_position_home.geo.latitude,
349+
crs='epsg:4326'),
350+
altitude=altitude,
351+
attitude=None,
352+
timestamp=None
353+
)
354+
return position
330355
else:
331356
return None

0 commit comments

Comments
 (0)