diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 976ee026..a93454c7 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -37,6 +37,21 @@ jobs: repository: PX4/px4_msgs path: colcon_ws/src/px4_msgs + - name: Install geographic_msgs and GeographicLib datasets + run: | + sudo apt-get install wget + sudo apt-get install -y ros-foxy-mavros ros-foxy-mavros-extras + wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh + chmod +x install_geographiclib_datasets.sh + ./install_geographiclib_datasets.sh + sudo apt-get install -y ros-foxy-geographic-msgs + + - name: Checkout GISNav messages + uses: actions/checkout@v3 + with: + repository: hmakelin/gisnav_msgs + path: colcon_ws/src/gisnav_msgs + - name: Build colcon workspace run: | cd colcon_ws/src/px4_ros_com/scripts @@ -58,6 +73,7 @@ jobs: - name: Install GISNav run: | cd colcon_ws + source /opt/ros/foxy/setup.bash rosdep update rosdep install --from-paths src --ignore-src -y --rosdistro=foxy cd src/gisnav @@ -65,15 +81,19 @@ jobs: pip install -r requirements.txt pip install -r requirements-dev.txt cd ../.. - colcon build --packages-select gisnav + colcon build --packages-select gisnav gisnav_msgs - - name: Run ROS 2 integration tests + - name: Run ROS 2 launch tests run: | cd colcon_ws + sudo apt-get -y update + sudo apt-get -y dist-upgrade source /opt/ros/foxy/setup.bash source install/setup.bash - python3 -m coverage run --branch --include */site-packages/gisnav/* src/gisnav/test/test_mock_gps_node.py - python3 -m coverage report + ros2 daemon stop + ros2 daemon start + launch_test src/gisnav/test/launch/test_px4_launch.py + launch_test src/gisnav/test/launch/test_ardupilot_launch.py - name: Build Sphinx docs run: | diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index a7ad9b23..010e2ccc 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,7 +1,7 @@ # Contribute All kinds of contributions are welcome, from raising issues and improvement suggestions with the software to software -commits and pull requests (PRs). Please see below for guidance and suggestions on how and where to contribute. +commits and pull requests (PRs). Please see below for guidance and suggestions on how to contribute. ## Issues and improvement suggestions @@ -12,51 +12,33 @@ you could add your own comments. If not, use the provided templates to submit a ## Pull requests and software commits -> **Note** +> **Warning** > By contributing to this repository, you agree that **your contributions will be licensed under the repository's MIT > license**. All changes to the software are made through [pull requests][2]. Please pay attention to the following before you submit your pull request: +* See [Style Guide for Python Code][4] for general code style guidelines. + +* Consider following [Conventional Commits][3] when making your commit message. + * If your pull request is related to an existing [open issue][1], please mention it in your pull request description. * If your pull request addresses a new issue or improvement, consider posting it on the [open issues][1] page before you start working on it so that others will also be aware of your pending work. -* If your pull request makes use of 3rd party software, please ensure that it is MIT license compatible. - -* Consider creating a [draft pull request][3] to get early feedback on your work before you commit to it further. +* Consider creating a [draft pull request][5] to get early feedback on your work before you commit to it further. * In your pull request, please describe not only *what* you have done, but also *why* you have done it. This helps the reviewer understand your thought process faster. -## Improvements and new features - -If you are planning to add a new feature or improve an existing feature of the software, please ensure that your -contribution is aligned with GISNav's **development focus**. Currently the **development focus** is to make the software: - -* **Easier to use**, for example by... - * ...improving documentation - * ...streamlining the public API - * ...smoothing out configuration quirks -* **Better tested**, for example by... - * ...improving unit test coverage - * ...adding a (better) testing framework -* **Support relevant technology stacks**, for example by... - * ...adding support for Ardupilot - * ...adding an adapter for TensorFlow-based neural networks -* **More accurate**, for example by... - * ...adding a new state-of-the art pose estimation algorithm - * ...making use of digital-elevation maps (DEM) to retrieve elevation of ground plane - * ...making use of the [OSM buildings][4] database to remove assumption of planar ground surface - -> **Note** -> Please consider the examples in the second level bullet points as *suggestions* on where to find low-hanging fruit rather than -> as *recommendations* on what to do. +* If your pull request makes use of 3rd party software, please ensure that it is MIT license compatible. [2]: https://docs.github.com/en/pull-requests -[3]: https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/proposing-changes-to-your-work-with-pull-requests/about-pull-requests#draft-pull-requests +[3]: https://www.conventionalcommits.org/ + +[4]: https://peps.python.org/pep-0008/ -[4]: https://osmbuildings.org/data/ \ No newline at end of file +[5]: https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/proposing-changes-to-your-work-with-pull-requests/about-pull-requests#draft-pull-requests diff --git a/README.md b/README.md index 243d7909..defe9539 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ https://user-images.githubusercontent.com/22712178/187902004-480397cc-460f-4d57- # Introduction -> **Warning** Do not use this software for real drone flights. This software is untested and has only been demonstrated +> **Warning** Do not use this software for real drone flights. GISNav is untested and has only been demonstrated > in a software-in-the-loop (SITL) simulation environment. GISNav is a ROS 2 package that enables map-based visual navigation for airborne drones **in a simulation environment**. diff --git a/config/test_typhoon_h480__ksql_airport.yaml b/config/test_typhoon_h480__ksql_airport.yaml deleted file mode 100644 index f3ccd038..00000000 --- a/config/test_typhoon_h480__ksql_airport.yaml +++ /dev/null @@ -1,30 +0,0 @@ -# Test version used by test_mock_gps_node.launch.py -# Intentionally comments out and changes some values -mock_gps_node: - ros__parameters: - wms: - url: 'http://localhost:80/wms' - #version: '1.1.1' - layers: ['imagery'] - styles: [''] - srs: 'EPSG:4326' - image_format: 'image/jpeg' - request_timeout: 10 - misc: - autopilot: 'gisnav.autopilots.px4_micrortps.PX4microRTPS' - mock_gps_selection: 1 - attitude_deviation_threshold: 10 - #max_pitch: 30 - #min_match_altitude: 80 - min_match_altitude: 70 # Non-default value - map_update: - update_delay: 1 - gimbal_projection: True - max_pitch: 30 - max_map_radius: 1000 - update_map_area_threshold: 0.85 - #pose_estimator: - # params_file: 'config/loftr_params.yaml' - #debug: - # export_position: 'position.json' - # export_projection: 'projection.json' \ No newline at end of file diff --git a/config/typhoon_h480__ksql_airport.yaml b/config/typhoon_h480__ksql_airport.yaml deleted file mode 100644 index 19c2511f..00000000 --- a/config/typhoon_h480__ksql_airport.yaml +++ /dev/null @@ -1,28 +0,0 @@ -mock_gps_node: - ros__parameters: - wms: - url: 'http://localhost:80/wms' - version: '1.3.0' - layers: ['imagery'] - elevation_layers: ['osm-buildings'] - styles: [''] - srs: 'EPSG:4326' - image_format: 'image/jpeg' - request_timeout: 10 - misc: - autopilot: 'gisnav.autopilots.px4_micrortps.PX4microRTPS' - attitude_deviation_threshold: 10 # degrees - max_pitch: 30 # 30 # Used by _should_match - min_match_altitude: 50 - map_update: - update_delay: 1 # Keep low (e.g. 1 second) if vehicle flying fast or gimbal_projection: True - gimbal_projection: True - max_pitch: 30 # Applies if gimbal_projection: True, used by _should_update_map - max_map_radius: 1000 - update_map_area_threshold: 0.85 - pose_estimator: - #params_file: 'config/superglue_params.yaml' # For parsing args for matcher's static initializer method - params_file: 'config/loftr_params.yaml' - debug: - export_position: '' - export_projection: '' \ No newline at end of file diff --git a/config/typhoon_h480__ksql_airport_ardupilot.yaml b/config/typhoon_h480__ksql_airport_ardupilot.yaml deleted file mode 100644 index 9d8bfd61..00000000 --- a/config/typhoon_h480__ksql_airport_ardupilot.yaml +++ /dev/null @@ -1,29 +0,0 @@ -mock_gps_node: - ros__parameters: - wms: - url: 'http://localhost:80/wms' - version: '1.3.0' - layers: ['imagery'] - elevation_layers: ['osm-buildings'] - styles: [''] - srs: 'EPSG:4326' - image_format: 'image/jpeg' - request_timeout: 10 - misc: - autopilot: 'gisnav.autopilots.ardupilot_mavros.ArduPilotMAVROS' - attitude_deviation_threshold: 10 # degrees - max_pitch: 50 # 30 # Make bigger with static_camera: True # Used by _should_match - min_match_altitude: 50 - static_camera: True # Static down-facing camera (relative to vehicle body) - map_update: - update_delay: 1 # Keep low (e.g. 1 second) if vehicle flying fast or gimbal_projection: True - gimbal_projection: False # set to False for ArduPilot (until gimbal is implemented) - max_pitch: 30 # Applies if gimbal_projection: True, used by _should_update_map - max_map_radius: 1000 - update_map_area_threshold: 0.85 - pose_estimator: - #params_file: 'config/superglue_params.yaml' # For parsing args for matcher's static initializer method - params_file: 'config/loftr_params.yaml' - debug: - export_position: '' - export_projection: '' \ No newline at end of file diff --git a/docs/_static/img/gisnav_sitl_featureless_buildings.png b/docs/_static/img/gisnav_sitl_featureless_buildings.png new file mode 100644 index 00000000..1b82549b Binary files /dev/null and b/docs/_static/img/gisnav_sitl_featureless_buildings.png differ diff --git a/docs/conf.py b/docs/conf.py index 8eb12d6e..9ee8b0c3 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -37,7 +37,13 @@ # Add any Sphinx extension module names here, as strings. They can be # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. -extensions = ['sphinx.ext.autodoc', 'sphinx.ext.coverage', 'autodocsumm', 'myst_parser'] +extensions = ['sphinx.ext.autodoc', + 'sphinx.ext.coverage', + 'sphinx.ext.autosectionlabel', + 'sphinx_design', + 'autodocsumm', + 'myst_parser' + ] # Add any paths that contain templates here, relative to this directory. templates_path = ['_templates'] @@ -79,6 +85,7 @@ } ], "icon_links_label": "Quick Links", + "show_toc_level": 3, } # Make version number accessible in .rst files diff --git a/docs/index.rst b/docs/index.rst index ca96bf7e..acd154b2 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -3,27 +3,38 @@ GISNav Developer Documentation ##################################### Welcome to GISNav's developer documentation! -This site contains the following pages: +.. card:: Get Started + :link: pages/get_started.html -* `Get Started `_ + A quick demonstration of GNSS-free visual navigation with GISNav - A quick demonstration of GNSS-free visual navigation with GISNav's :class:`.MockGPSNode` +.. card:: Developer Guide + :link: pages/developer_guide/index.html -* `Setup `_ + Instructions on how to integrate GISNav with your own project and how to extend its functionality - Instructions on how to to setup your own PX4 development and simulation environment +.. card:: API Documentation + :link: pages/api_documentation/index.html -* `Developer Guide `_ + GISNav public API reference for developers - Instructions on how to integrate GISNav with your own project and how to extend its functionality +.. card:: Contribute + :link: pages/contribute.html -* `API Documentation `_ + Contributions to GISNav are welcome! Please see this page for guidance - GISNav public API reference for developers +Generate documentation +_________________________________________ +To build this Sphinx documentation yourself, first install the :ref:`GISNav ROS package` including the +:ref:`Development dependencies` and then run: + +.. code-block:: bash + :caption: Build Sphinx documentation -* `Contribute `_ + cd ~/colcon_ws/src/gisnav/docs + make html - Contributions to GISNav are welcome! Please see this page for guidance. +The HTML documentation will then appear in the ``~/colcon_ws/src/gisnav/docs/_build/`` folder. .. seealso:: @@ -39,7 +50,6 @@ This site contains the following pages: :hidden: pages/get_started - pages/setup - pages/developer_guide - pages/api_documentation + pages/developer_guide/index + pages/api_documentation/index pages/contribute diff --git a/docs/pages/api_documentation.rst b/docs/pages/api_documentation.rst deleted file mode 100644 index 184830d9..00000000 --- a/docs/pages/api_documentation.rst +++ /dev/null @@ -1,116 +0,0 @@ -************************************************** -API Documentation -************************************************** -This is an automatically generated public API documentation for the `GISNav ROS 2 package`_. - -Unless you want to contribute to GISNav itself or really get into the weeds of how GISNav works, you probably -will only be interested in the :class:`.BaseNode` and :class:`.PoseEstimator` class APIs. - -.. _GISNav ROS 2 package: https://gitlab.com/px4-ros2-map-nav/python_px4_ros2_map_nav - -.. note:: - GISNav is under active development and stability of the API is not guaranteed. Use a specific commit or version tag - to mitigate the impact of breaking changes. - -Indices and Tables -================================================= -Use the search in the sidebar or these indices to find what you are looking for. - -* :ref:`genindex` -* :ref:`modindex` - - -nodes package -================================================= -base_node module --------------------------------------------- -.. automodule:: gisnav.nodes.base_node - :autosummary: - :members: - :undoc-members: - :special-members: __init__ - :show-inheritance: - -mock_gps_node module --------------------------------------------- -.. automodule:: gisnav.nodes.mock_gps_node - :autosummary: - :members: - :undoc-members: - :special-members: __init__ - :show-inheritance: - -pose_estimators package -================================================= -pose_estimator module --------------------------------------------- -.. automodule:: gisnav.pose_estimators.pose_estimator - :autosummary: - :members: - :undoc-members: - :special-members: __init__ - :show-inheritance: - -keypoint_pose_estimator module --------------------------------------------- -.. automodule:: gisnav.pose_estimators.keypoint_pose_estimator - :autosummary: - :members: - :undoc-members: - :special-members: __init__ - :show-inheritance: - -loftr_pose_estimator module --------------------------------------------- -.. automodule:: gisnav.pose_estimators.loftr_pose_estimator - :autosummary: - :members: - :undoc-members: - :special-members: __init__ - :show-inheritance: - -superglue_pose_estimator module --------------------------------------------- -.. automodule:: gisnav.pose_estimators.superglue_pose_estimator - :autosummary: - :members: - :undoc-members: - :special-members: __init__ - :show-inheritance: - -data module -================================================= -.. automodule:: gisnav.data - :autosummary: - :members: - :undoc-members: - :special-members: __init__, __post_init__ - :show-inheritance: - -geo module -================================================= -.. automodule:: gisnav.geo - :autosummary: - :members: - :undoc-members: - :special-members: __init__ - :show-inheritance: - -assertions module -================================================= -.. automodule:: gisnav.assertions - :autosummary: - :members: - :undoc-members: - :special-members: __init__ - :show-inheritance: - -wms module -================================================= -.. automodule:: gisnav.wms - :autosummary: - :members: - :undoc-members: - :special-members: __init__ - :show-inheritance: - diff --git a/docs/pages/api_documentation/common_automodule.rst b/docs/pages/api_documentation/common_automodule.rst new file mode 100644 index 00000000..42403f84 --- /dev/null +++ b/docs/pages/api_documentation/common_automodule.rst @@ -0,0 +1,35 @@ +gisnav +-------------------------------------------- +.. automodule:: gisnav + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +assertions +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.assertions + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +data +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.data + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +geo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.geo + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: diff --git a/docs/pages/api_documentation/index.rst b/docs/pages/api_documentation/index.rst new file mode 100644 index 00000000..a1189858 --- /dev/null +++ b/docs/pages/api_documentation/index.rst @@ -0,0 +1,18 @@ +************************************************** +API Documentation +************************************************** +Here you can find the automatically generated **public API** documentation for the `GISNav ROS 2 package`_. Use the +search bar or the below module index to find what you are looking for. + +.. _GISNav ROS 2 package: https://github.com/hmakelin/gisnav + +.. warning:: + GISNav is under active development and stability of the API is not guaranteed. Use a specific commit or version tag + to mitigate the impact of breaking changes. + +.. toctree:: + :caption: Python module index + + common_automodule + nodes/automodule + pose_estimators/automodule diff --git a/docs/pages/api_documentation/nodes/automodule.rst b/docs/pages/api_documentation/nodes/automodule.rst new file mode 100644 index 00000000..e22b921d --- /dev/null +++ b/docs/pages/api_documentation/nodes/automodule.rst @@ -0,0 +1,100 @@ +gisnav.nodes +-------------------------------------------- +.. automodule:: gisnav.nodes + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +ardupilot_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.nodes.ardupilot_node + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +bbox_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.nodes.bbox_node + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +map_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.nodes.map_node + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +messaging +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.nodes.messaging + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +mock_gps_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.nodes.mock_gps_node + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +pose_estimation_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.nodes.pose_estimation_node + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +px4_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.nodes.px4_node + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +gisnav.nodes.base +-------------------------------------------- +autopilot_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.nodes.base.autopilot_node + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +base_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.nodes.base.base_node + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +camera_subscriber_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.nodes.base.camera_subscriber_node + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: diff --git a/docs/pages/api_documentation/pose_estimators/automodule.rst b/docs/pages/api_documentation/pose_estimators/automodule.rst new file mode 100644 index 00000000..fc297a01 --- /dev/null +++ b/docs/pages/api_documentation/pose_estimators/automodule.rst @@ -0,0 +1,44 @@ +gisnav.pose_estimators +-------------------------------------------- +.. automodule:: gisnav.pose_estimators + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +keypoint_pose_estimator +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.pose_estimators.keypoint_pose_estimator + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +loftr_pose_estimator +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.pose_estimators.loftr_pose_estimator + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +pose_estimator +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.pose_estimators.pose_estimator + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: + +superglue_pose_estimator +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. automodule:: gisnav.pose_estimators.superglue_pose_estimator + :autosummary: + :members: + :undoc-members: + :special-members: __init__ + :show-inheritance: diff --git a/docs/pages/developer_guide.rst b/docs/pages/developer_guide.rst deleted file mode 100644 index 9d6c4fee..00000000 --- a/docs/pages/developer_guide.rst +++ /dev/null @@ -1,394 +0,0 @@ -************************************************** -Developer Guide -************************************************** - -.. warning:: - GISNav is untested and has only been demonstrated in a software-in-the-loop (SITL) simulation environment. - Do not use this software for real drone flights. - -.. note:: - GISNav is under active development and stability of the API is not guaranteed. Use a specific commit or version tag - to mitigate the impact of breaking changes. - -This section provides instruction on how you can integrate GISNav with your project as well as configure and extend -it to match your use case. - -You should start from `Extend BaseNode`_ and only move on to the other sections if your project needs more specific -configuration. - - -Integrate GISNav -==================================================== -The `ROS 2 `_ nodes can be found in the :py:mod:`.gisnav.nodes` package. The package includes -the :class:`.BaseNode` abstract base class which must be extended by all implementing nodes. The :class:`.MockGPSNode` -sample class is provided for demonstration and to help you get started with your own node. - - -.. _Extend BaseNode: - -Extend BaseNode -____________________________________________________ -The :class:`.BaseNode` abstract base class extends :class:`rclpy.node.Node` by providing a new -:meth:`.BaseNode.publish` method. The method provides you a :class:`.Position` instance to make integration to other -systems (e.g. via a ROS publisher) convenient. - -To integrate GISNav with your project, you must implement the :class:`.BaseNode` class by writing your own -:meth:`.BaseNode.publish` method: - -.. code-block:: python - :caption: Example of custom node that logs the position estimates - - from gisnav.nodes import BaseNode - - class MyNode(BaseNode): - - def __init__(self, name, share_dir): - self.super().__init__(name, share_dir) - - def publish(self, position): - self.get_logger().info(f'Estimated WGS 84 lat: {position.xy.lat}, lon: {position.xy.lon}.') - -.. _Position Class: - -Position Class -____________________________________________________ -The attributes in the :class:`.Position` input to the :meth:`.BaseNode.publish` method are in the following formats: - - * Latitude and longitude are provided in `WGS 84 `_, although :class:`.GeoPoint` can provide the values in other CRS as well - * Altitude above mean sea level (AMSL) and above ground is provided in meters. - * Standard deviations are provided in meters in `ENU `_ frame `(x, y := longitude, latitude; z := altitude)`. - * Attitude quaternion is in (x, y, z, w) format (SciPy format, :class:`px4_msgs.VehicleAttitude` has different format). - * Timestamp is synchronized with the `PX4 EKF2 reference time `_. - -.. note:: - Currently the attitude of the (gimbal stabilized) camera is returned, not the attitude of the vehicle itself. - -For more information on the dimensions and units, please see the source code for :meth:`.Position`. The x and y -coordinates (in ENU frame) are provided as a :class:`.GeoPoint`, which is a wrapper for :class:`geopandas.GeoSeries`. - -Modify ROS Parameters -____________________________________________________ -ROS parameter server is used to manage the configuration of the :class:`.BaseNode` instance at runtime. An example -configuration is provided in ``config/typhoon_h480__ksql_airport.yml``. :class:`.BaseNode` has pre-configured default -values for all required parameters, so it is not necessary pass this parameter file to your ROS node. However, it is -likely that you will at least need to edit the `WMS Client`_ URL to get GISNav working. To initialize :class:`.BaseNode` -with your own parameter values, you will need to provide it with the YAML parameter file in your - -.. code-block:: bash - :caption: Launch with ``ros2 run`` - - ros2 run gisnav mock_gps_node --ros-args --log-level info --params-file src/gisnav/config/typhoon_h480__ksql_airport.yml - - -.. code-block:: bash - :caption: Launch with launch file - - ros2 launch gisnav mock_gps_node.launch.py - - -Spin up your own node -____________________________________________________ -Once you have `extended BaseNode `_, you can spin it up in the main script of your ``colcon`` package -(:class:`.BaseNode` extends ``rclpy.nodes.Node``): - -.. code-block:: python - - import rclpy - from my_package import MyNode - - def main(args=None): - rclpy.init(args=args) - my_node = MyNode() - rclpy.spin(my_node) - my_node.destroy_timers() # BaseNode method, see API reference - my_node.terminate_pools() # BaseNode method, see API reference - my_node.destroy_node() - rclpy.shutdown() - - if __name__ == '__main__': - main() - -.. seealso:: - `ROS Publisher-Subscriber (Python) tutorial `_ for a step-by-step guide on how to implement a ROS node. - -.. _The MockGPSNode class: - -Example Integration (MockGPSNode) -____________________________________________________ -.. warning:: - The configurations presented in this section are intended for simulation use only. Do not attempt these on a real - flight. - -The :class:`.MockGPSNode` extends the :class:`.BaseNode` abstract base class to publish a mock -:class:`px4_msgs.SensorGps` message to the PX4-ROS 2 bridge ``/fmu/sensor_gps/in`` topic. - -You can configure your PX4 to use the new GPS only to simulate loss of primary GPS. This can be either configured -before flight in the file ``~/PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480``, or during -flight by setting the `SENS_GPS_PRIME `_ parameter with -the `param `_ command: - -.. code-block:: - :caption: Use GISNav as primary GPS `(assumes GISNav mock GPS node publishes with ``selection=1``)` - - param set SENS_GPS_PRIME 1 - -.. note:: - If you disable primary GPS in the file before flight, you will not be able to takeoff in Mission mode since GISNav - cannot provide a mock GPS fix until the drone is already above the minimum configured flight altitude - (``misc.min_match_altitude`` ROS parameter). - -.. seealso:: - See `SENS_GPS_MASK `_ parameter - for configuring GPS blending in PX4 - - -You may also want to try configuring the PX4 GPS consistency gates to initially be more tolerant for your build -target, e.g. in the ``6011_typhoon_h480`` file mentioned earlier in this section: - - * `EKF2_GPS_P_GATE `_ - * `EKF2_GPS_P_NOISE `_ - * `EKF2_GPS_V_GATE `_ - * `EKF2_GPS_V_NOISE `_ - -.. note:: - You must ensure that PX4 is receiving the :class:`px4_msgs.VehicleGpsMessage` messages over the `PX4-ROS 2 Bridge`_. - - You can check that the messages are being published with: - - .. code-block:: - - ros2 topic echo VehicleGpsPosition_PubSubTopic - - -.. _WMS Client: - -WMS Client -=================================================== -The :class:`.BaseNode` continuously requests new map rasters from a WMS endpoint when the drone or the drone camera's -field of view moves away from the area defined by previous maps. The requests are handled by :class:`.WMSClient`. - -The :class:`.WMSClient` class is by default instantiated in a separate process, but can also be run in a separate thread -to reduce serialization overhead, since under the hood it uses the :class:`multiprocessing.pool.Pool` API which is -compatible with the :class:`multiprocessing.pool.ThreadPool` multithreading API. - -.. note:: - Multithreading must be enabled in :class:`.BaseNode` source code, currently no configuration parameter for it exists - -A :py:attr:`._wms_timer` periodically requests the :class:`.WMSClient` to fetch a new map based -on criteria defined in :meth:`._should_update_map` to keep unnecessary WMS requests to a minimum. Generally a new map -is requested if the field of view (FOV) of the vehicle's camera no longer significantly overlaps with the previously -requested map. The update behavior can be adjusted via the ROS parameter server through the parameters under the -``wms.map_update`` namespace. - -.. _Pose Estimators: - -Pose Estimators -=================================================== - -.. _SuperGlue & LoFTR: - -SuperGlue & LoFTR -____________________________________________________ -Two pose estimators, SuperGlue and SuperGlue-inspired LoFTR, are provided with LoFTR as the default pose estimator. -These were seen as state-of-the-art image matching networks at the time GISNav was written. However, newer networks may -provide better results. - -.. note:: - SuperGlue has restrictive licensing requirements (see license file in the repository), while LoFTR has a permissive - license. - -.. warning:: - LoFTR uses SuperGlue for *optimal transport* so make sure you use the *dual-softmax* version instead or otherwise - SuperGlue licensing terms apply. - - -.. _Extend Pose Estimator: - -Extend PoseEstimator -____________________________________________________ -You must extend the :class:`.PoseEstimator` abstract base and write your own :meth:`.PoseEstimator.estimate_pose` -method to implement your own pose estimator. If your pose estimator is keypoint-based, you may want to extend -:class:`.KeypointPoseEstimator` and implement the :meth:`.find_matching_keypoints` method instead. The base classes -implement the required static initializer and worker methods that make them work with Python's -:class:`.multiprocessing.pool.Pool` and :class:`.multiprocessing.pool.ThreadPool` APIs. - -You can use the below snippets to get started with your own :class:`.PoseEstimator`: - -.. code-block:: python - - from typing import Optional - from python_px4_ros2_map_nav.pose_estimators.pose_estimator import PoseEstimator - - class MyPoseEstimator(PoseEstimator): - - def __init__(self): - # TODO: implement initializer - raise NotImplementedError - - def estimate_pose(query, reference, k, guess = None, elevation_ref = None): - """Returns pose between query and reference images""" - # Do your pose estimation magic here - #r = ... # np.ndarray of shape (3, 3) - #t = ... # np.ndarray of shape (3, 1) - #return r, t - raise NotImplementedError - -.. note:: - If you can't estimate a pose with the given query and reference frames, you can return ``None`` from your - :meth:`.PoseEstimator.estimate_pose` - -.. _Keypoint-Based Pose Estimator: - -Keypoint-Based Pose Estimator -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -If you want to create a :class:`.KeypointPoseEstimator`, you can also start with the below snippet: - -.. code-block:: python - - from gisnav.pose_estimators.keypoint_pose_estimator import KeypointPoseEstimator - - class MyPoseEstimator(KeypointPoseEstimator): - - def __init__(self): - # TODO: implement initializer - raise NotImplementedError - - def find_matching_keypoints(query, reference): - """Returns matched keypoints between query and reference images""" - # Find matching keypoints here - #mkp_qry = ... - #mkp_ref = ... - #return mkp_qry, mkp_ref - raise NotImplementedError - -.. _Configuration: - -Configuration -____________________________________________________ -After you have implemented your pose estimator, you need to tell :class:`.BaseNode` where to find its initialization -arguments in your ROS YAML parameter file: - -.. code-block:: - - my_node: - ros__parameters: - pose_estimator: - params_file: 'config/my_node_params.yaml' - -See the provided ``loftr_params.yaml`` and ``superglue_params.yaml`` for examples on how to format the file. - -.. _Autopilots: - -Autopilots -____________________________________________________ -The :py:mod:`.gisnav.autopilots` module has a :class:`.Autopilot` base class that you can extend to implement support -for a custom autopilot setup. Example implementations are provided for PX4 over microRTPS bridge -(:class:`.PX4microRTPS`) and ArduPilot over MAVROS (:class:`.ArduPilotMAVROS`). The autopilot adapter configured in -the ``misc.autopilot`` ROS parameter and loaded when the :class:`.BaseNode` is initialized. - -.. note:: - Currently when using :class:`.ArduPilotMAVROS` you need to set the ``misc.static_camera`` ROS - parameter to ``True`` and the ``map_update.gimbal_projection`` ROS parameter to ``False``, because - :meth:`.ArduPilotMAVROS.gimbal_attitude` and :meth:`.ArduPilotMAVROS.gimbal_set_attitude` are not implemented. - See the ``config/typhoon_h480__ksql_airport_ardupilot.yaml`` file for an example. - -.. _ROS 2 Topic Configuration: - -ROS 2 Topic Configuration -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -As an example, the :class:`.PX4microRTPS` adapter automatically subscribes to the following -required telemetry and video input ROS topics: - - #. :class:`px4_msgs.VehicleGlobalPosition` messages via ``VehicleGlobalPosition_PubSubTopic`` - #. :class:`px4_msgs.VehicleLocalPosition` messages via ``VehicleLocalPosition_PubSubTopic`` - #. :class:`px4_msgs.VehicleAttitude` messages via ``VehicleAttitude_PubSubTopic`` - #. :class:`px4_msgs.GimbalDeviceSetAttitude` messages via ``GimbalDeviceSetAttitude_PubSubTopic`` - #. :class:`px4_msgs.Image` messages via ``image_raw`` - #. :class:`px4_msgs.CameraInfo` messages via ``camera_info`` - -.. note:: - In the Mock GPS Example, ``gscam`` is used to stream the UDP stream to the ``image_raw`` and ``camera_info`` ROS - topics. They are not broadcast via the PX4-ROS 2 bridge. - -You may need to add more subscribe and publish topics if you decide to implement your own node. You may need to edit -the ``uorb_rtps_message_ids.yaml`` file as described in the -`supported UORB messages `_ section of -the PX4 User Guide. - -.. seealso:: - `PX4-ROS 2 bridge `_ for more information on the PX4-ROS 2 bridge - -Remap ROS 2 Topics -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -By default, the :class:`.CameraInfo` and :class:`px4_msgs.Image` messages are expected at ``camera/camera_info`` and -``camera/image_raw`` topics, respectively. The topics can be remapped: - -.. code-block:: bash - - ros2 run gisnav mock_gps_node --mavros --ros-args --log-level info \ - --params-file src/gisnav/config/typhoon_h480__ksql_airport_ardupilot.yaml \ - -r camera/camera_info:=camera_info -r camera/image_raw:=image_raw - -Testing -==================================================== -Unit & ROS 2 integration tests -____________________________________________________ -First you must install the dev dependencies for your workspace: - -.. code-block:: bash - - python3 -m pip install -r requirements-dev.txt - -You can then run existing tests in the ``test`` folder with: - -.. code-block:: bash - - cd ~/px4_ros_com_ros2 - launch_test src/gisnav/test/test_mock_gps_node.py - -For code coverage you can use ``coverage.py``. See the -`official instructions `_ on how to configure what source files -to measure: - -.. code-block:: bash - - cd ~/px4_ros_com_ros2 - python3 -m coverage run --branch --include */site-packages/gisnav/* src/gisnav/test/test_mock_gps_node.py - python3 -m coverage report - -SITL tests -____________________________________________________ -SITL tests are under the ``test/sitl`` folder. They are simple Python scripts: - -.. code-block:: bash - - cd ~/px4_ros_com_ros2/src/gisnav/test/sitl - python sitl_test_mock_gps_node.py - -Flight Log Analysis -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -The flight log generated by the SITL test can be analyzed with the Jupyter notebooks in ``test/ulog_analysis`` folder. -You must first start ``jupyter-notebook``: - -.. code-block:: bash - - cd ~/px4_ros_com_ros2/src/gisnav/test/sitl/ulog_analysis - jupyter-notebook - -The notebook documents the analysis and displays the results. Download the example ULog file from Google Drive `here -`_. - -Documentation -==================================================== - -If you have the development environment setup, you can generate this GISNav documentation yourself with Sphinx: - -.. code-block:: bash - - cd ~/colcon_ws/src/gisnav - python3 -m pip install -r requirements-dev.txt - cd docs - make html - - -The HTML documentation will then appear in the ``_build/`` folder. diff --git a/docs/pages/developer_guide/index.rst b/docs/pages/developer_guide/index.rst new file mode 100644 index 00000000..353f4e96 --- /dev/null +++ b/docs/pages/developer_guide/index.rst @@ -0,0 +1,41 @@ +************************************************** +Developer Guide +************************************************** +This section provides instruction on how you can integrate GISNav with your project as well as configure and extend +it to match your use case. + +.. warning:: + Do not use this software for real drone flights. GISNav is untested and has only been demonstrated in a + software-in-the-loop (SITL) simulation environment. + +.. toctree:: + :caption: Setup SITL environment + + sitl_environment/docker + sitl_environment/prerequisites + sitl_environment/autopilot + sitl_environment/qgroundcontrol + sitl_environment/gis_server + sitl_environment/install_gisnav + +.. toctree:: + :caption: Launch GISNav + + launch/launch_files + launch/run_individual_nodes + launch/modify_params + +.. toctree:: + :caption: Run tests + + test/test_prerequisites + test/launch_tests + test/code_coverage + test/sitl_tests + +.. toctree:: + :caption: Integrate with your own project + + integrate/ros_messaging + integrate/mock_gps + integrate/pose_estimators diff --git a/docs/pages/developer_guide/integrate/mock_gps.rst b/docs/pages/developer_guide/integrate/mock_gps.rst new file mode 100644 index 00000000..5149fdfb --- /dev/null +++ b/docs/pages/developer_guide/integrate/mock_gps.rst @@ -0,0 +1,77 @@ +Mock GPS messages +=================================================== +:class:`.MockGPSNode` uses the read-only ROS parameter ``px4_micrortps`` to determine whether to publish a +:class:`px4_msgs.msg.SensorGps` (PX4) or a :class:`mavros_msgs.msg.GPSINPUT` (ArduPilot)* message from the received +:class:`geographic_msgs.msg.GeoPoseStamped` and :class:`mavros_msgs.msg.Altitude` messages. + +.. note:: + \* Currently :class:`.MockGPSNode` does not use the MAVROS GPS_INPUT plugin to publish a + :class:`mavros_msgs.msg.GPSINPUT` message and publishes the `MAVLink GPS_INPUT`_ message directly over UDP instead + + .. _MAVLink GPS_INPUT: https://mavlink.io/en/messages/common.html#GPS_INPUT + +Autopilot specific considerations +____________________________________________________ +.. warning:: + The configurations presented in this section are intended for simulation use only. Do not use these on real drone + flights + +PX4 +**************************************************** +The :ref:`PX4 parameter configuration` section introduced some GPS related PX4 parameters and how and where to modify +them. This section introduces more parameters that may be relevant to you depending on how you want PX4 to use +GISNav's mock GPS messages. + +Configure primary GPS and blending: + +* `SENS_GPS_PRIME`_ for configuring primary GPS +* `SENS_GPS_MASK`_ for configuring GPS blending criteria + +.. _SENS_GPS_PRIME: https://docs.px4.io/master/en/advanced_config/parameter_reference.html#SENS_GPS_PRIME +.. _SENS_GPS_MASK: https://docs.px4.io/v1.12/en/advanced_config/parameter_reference.html#SENS_GPS_MASK + +.. note:: + In an `earlier version of the GISNav mock GPS demo for PX4`_, primary GPS loss was simulated by manipulating the + `SENS_GPS_PRIME`_ parameter with the `param`_ command mid-flight with GPS blending disabled. With the introduction + of :class:`px4_msgs.msg.SensorGps` and the retirement of :class:`px4_msgs.VehicleGpsMessage` messages in later + versions of PX4, the demo has also transitioned to simulating GPS failure the `canonical way`_. + + .. _earlier version of the GISNav mock GPS demo for PX4: https://www.youtube.com/watch?v=JAK2DPZC33w + .. _param: https://dev.px4.io/master/en/middleware/modules_command.html#param + .. _canonical way: https://docs.px4.io/main/en/simulation/failsafes.html#sensor-system-failure + +You may also want to modify the PX4 GPS consistency gates to initially be more tolerant for your build +target: + + * `EKF2_GPS_P_GATE `_ + * `EKF2_GPS_P_NOISE `_ + * `EKF2_GPS_V_GATE `_ + * `EKF2_GPS_V_NOISE `_ + +Ardupilot +**************************************************** +See the `GPS Input page in ArduPilot official documentation`_ for instructions on configuring the GPSInput module for +ArduPilot. + +.. _GPS Input page in ArduPilot official documentation: https://ardupilot.org/mavproxy/docs/modules/GPSInput.html + +Below is an example for loading and configuring the module to listen on port ``25101`` on SITL simulation startup: + +.. code-block:: bash + :caption: ArduPilot GPSInput module configuration + + cd ~/ardupilot + python3 Tools/autotest/sim_vehicle.py -v ArduCopter -f gazebo-iris -L KSQL_Airport \ + -m '--cmd="module load GPSInput; GPSInput.port=25101"' + +.. note:: + The ``KSQL_Airport`` location is not included by default, you have to `configure the starting location`_ + + .. _configure the starting location: https://ardupilot.org/dev/docs/using-sitl-for-ardupilot-testing.html#setting-vehicle-start-location + +.. seealso:: + `List of ArduPilot GPS parameters`_ (does not include parameters prefixed ``SIM_GPS*``) and ArduPilot's + `instructions on how to test GPS failure`_ + + .. _List of ArduPilot GPS parameters: https://ardupilot.org/copter/docs/parameters.html#gps-parameters + .. _instructions on how to test GPS failure: https://ardupilot.org/dev/docs/using-sitl-for-ardupilot-testing.html#testing-gps-failure \ No newline at end of file diff --git a/docs/pages/developer_guide/integrate/pose_estimators.rst b/docs/pages/developer_guide/integrate/pose_estimators.rst new file mode 100644 index 00000000..ab5e0fee --- /dev/null +++ b/docs/pages/developer_guide/integrate/pose_estimators.rst @@ -0,0 +1,134 @@ +Pose Estimators +==================================================== +SuperGlue & LoFTR +____________________________________________________ +GISNav comes with adapters for two pose estimators - `SuperGlue`_ and the SuperGlue-inspired `LoFTR`_ - with LoFTR as +the default adapter. They were seen as state-of-the-art image matching neural networks at the time GISNav was +written. However, newer networks may provide better results and you may want to try integrating them with GISNav. + +.. _SuperGlue: https://github.com/magicleap/SuperGluePretrainedNetwork +.. _LoFTR: https://github.com/zju3dv/LoFTR + +To integrate a new neural network you can either :ref:`Extend PoseEstimator` or +:ref:`Replace PoseEstimationNode` completely. + +.. warning:: + * SuperGlue has `restrictive licensing requirements`_, while LoFTR has a `permissive + license`_. + * LoFTR uses SuperGlue for *optimal transport* so make sure you use the *dual-softmax* version instead or + otherwise SuperGlue licensing terms apply. + + .. _restrictive licensing requirements: https://github.com/magicleap/SuperGluePretrainedNetwork/blob/master/LICENSE + .. _permissive license: https://github.com/zju3dv/LoFTR/blob/master/LICENSE + +SITL simulation quirks +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +The `KSQL Airport Gazebo model`_ buildings in the SITL simulation demo are featureless grey blocks, so any +:class:`.PoseEstimator` will most likely not use them for matching. This means any building elevation data (see +:ref:`Rasterizing vector data`) will not technically be used to improve pose estimates in the SITL simulation. The +below figure illustrates how LoFTR finds keypoints at an even density throughout the simulated drone's field of view +except on the featureless buildings. + +.. _KSQL Airport Gazebo model: https://docs.px4.io/main/en/simulation/gazebo_worlds.html#ksql-airport + +.. figure:: ../../../_static/img/gisnav_sitl_featureless_buildings.png + + LoFTR does not find keypoints on featureless buildings or terrain (SITL simulation) + +Extend PoseEstimator +____________________________________________________ +You must extend the :class:`.PoseEstimator` abstract base class and write your own :meth:`.PoseEstimator.estimate` +method. If your pose estimator is keypoint-based, you may want to extend :class:`.KeypointPoseEstimator` abstract base +class and implement the :meth:`._find_matching_keypoints` method, which will save you the time of estimating the pose +from detected keypoints. + +You can use the below snippets to get started with your own :class:`.PoseEstimator` or :class:`.KeypointPoseEstimator`: + +.. tab-set:: + + .. tab-item:: Extend PoseEstimator + :selected: + + .. code-block:: python + + import numpy as np + from typing import Optional, Tuple + from gisnav.pose_estimators import PoseEstimator + + class MyPoseEstimator(PoseEstimator): + + def __init__(self): + # TODO + raise NotImplementedError + + def estimate(self, query: np.ndarray, reference: np.ndarray, k: np.ndarray, + guess: Optional[Tuple[np.ndarray, np.ndarray]] = None, + elevation_reference: Optional[np.ndarray] = None) -> Optional[Tuple[np.ndarray, np.ndarray]]: + """Returns pose between provided images, or None if pose cannot be estimated + + :param query: The first (query) image for pose estimation + :param reference: The second (reference) image for pose estimation + :param k: Camera intrinsics matrix of shape (3, 3) + :param guess: Optional initial guess for camera pose + :param elevation_reference: Optional elevation raster (same size resolution as reference image, grayscale) + :return: Pose tuple of rotation (3, 3) and translation (3, 1) numpy arrays, or None if could not estimate + """ + # TODO + raise NotImplementedError + + .. note:: + If you can't estimate a pose with the given query and reference frames, you can return ``None`` from your + :meth:`.PoseEstimator.estimate` method + + .. tab-item:: Extend KeypointPoseEstimator + + .. code-block:: python + + import numpy as np + from typing import Optional, Tuple + from gisnav.pose_estimators import KeypointPoseEstimator + + class MyKeypointPoseEstimator(KeypointPoseEstimator): + + def __init__(self): + # TODO + raise NotImplementedError + + def _find_matching_keypoints(self, query: np.ndarray, reference: np.ndarray) \ + -> Optional[Tuple[np.ndarray, np.ndarray]]: + """Returns matching keypoints between provided query and reference image + + Note that this method is called by :meth:`.estimate_pose` and should not be used outside the implementing + class. + + :param query: The first (query) image for pose estimation + :param reference: The second (reference) image for pose estimation + :return: Tuple of matched keypoint arrays for the images, or None if none could be found + """ + raise NotImplementedError + +Passing init args to :class:`.PoseEstimator` +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +You need to override the ``pose_estimator_params`` ROS parameter of :class:`.PoseEstimator` with the path to your own +YAML configuration file. See ``launch/params/pose_estimators/`` folder for current examples. + +.. note:: + The way this is currently done needs refactoring. See below the relevant section of code in + :class:`.PoseEstimationNode` initializer: + + .. literalinclude:: ../../../../gisnav/nodes/pose_estimation_node.py + :caption: :meth:`.PoseEstimationNode.__init__` pose estimator initialization + :start-after: # region setup pose estimator + :end-before: # endregion setup pose estimator + :language: python + :dedent: + +Replace PoseEstimationNode +____________________________________________________ +The :ref:`Aircraft GeoPose estimate topics` section describes the topic names and messages the GISNav's +:class:`.MockGPSNode` expects from :class:`.PoseEstimationNode`. You can create your own node that publishes these +messages and :class:`.MockGPSNode` will then be able to use them. + +You would have to create your own launch file that strips :class:`.PoseEstimationNode` from the launch description to +prevent the two nodes from publishing their own estimates to the same topic. See :ref:`Launch from ROS launch file` for +more information on launch files. diff --git a/docs/pages/developer_guide/integrate/ros_messaging.rst b/docs/pages/developer_guide/integrate/ros_messaging.rst new file mode 100644 index 00000000..23be3339 --- /dev/null +++ b/docs/pages/developer_guide/integrate/ros_messaging.rst @@ -0,0 +1,141 @@ +ROS messaging +____________________________________________________ +The natural way to integrate GISNav with other systems is via `ROS 2 `_. GISNav depends on ROS 2 +for both external and internal communication. If you have a ROS 2 node, you can talk to GISNav. + +For simple integrations you might only be interested in the :ref:`Aircraft GeoPose estimate topics`. For an overview of +all available topics, see :ref:`Remapping ROS 2 topics`. + +Remapping ROS 2 topics +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +To integrate GISNav with your own ROS system, you will likely have to do some topic name remapping. See the examples +below on how to :ref:`Launch from ROS launch file` and :ref:`Run individual node` with remapped topic names: + +.. tab-set:: + + .. tab-item:: Launch file + :selected: + + The below diff is an example remapping for the camera topics for :class:`.PoseEstimationNode`: + + .. literalinclude:: ../../../../launch/examples/base_camera_topic_remap.launch.py + :diff: ../../../../launch/base.launch.py + :caption: Camera topic name remap in a launch file + :language: python + + To launch the example base configuration (without :class:`.MockGPSNode` nor any :class:`.AutopilotNode`) + + .. code-block:: bash + :caption: Launch topic name remap configuration + + ros2 launch gisnav examples/base_camera_topic_remap.launch.py + + .. tab-item:: Run individual node + + The below command launches camera topics for :class:`.PoseEstimationNode`: + + .. code-block:: bash + :caption: Camera topic name remapping example using ``ros2 run`` + + cd ~/colcon_ws + ros2 run gisnav pose_estimation_node --ros-args --log-level info \ + --params-file src/gisnav/launch/params/pose_estimation_node.yaml \ + -r camera/camera_info:=camera_info \ + -r camera/image_raw:=image + +All\* default ROS topic names used by GISNav are listed in the :py:mod:`.messaging` module: + +.. note:: + \* :class:`.CameraSubscriberNode` currently uses its own hard-coded topic names and does not use the + :py:mod:`.messaging` module. + +.. literalinclude:: ../../../../gisnav/nodes/messaging.py + :caption: :py:mod:`.messaging` module ROS topic name defaults + :start-after: # region ROS topic names + :end-before: # endregion ROS topic names + :language: python + +Autopilot topics +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Depending on the launch configuration, :class:`.PX4Node` or :class:`.ArduPilotNode` is launched along with a number of +other nodes. These two nodes subscribe to your autopilot's specific ROS message types and translate them to a set of +common GISNav internal message types. + +You can see the subscribed topic names and message types in the ``__init__`` methods of the two nodes: + +.. note:: + See :ref:`Remapping ROS 2 topics` to adapt these nodes to your system. For example, you must remap the PX4 topic + names from ``fmu/*/out`` to ``fmu/out/*`` if you are using PX4 v1.14.0-beta1 instead of v1.13.X. + + .. _name remapping: https://design.ros2.org/articles/static_remapping.html + +.. tab-set:: + + .. tab-item:: PX4 + :selected: + + .. literalinclude:: ../../../../gisnav/nodes/px4_node.py + :caption: :py:meth:`.PX4Node.__init__` method + :pyobject: PX4Node.__init__ + + .. tab-item:: ArduPilot + + .. literalinclude:: ../../../../gisnav/nodes/ardupilot_node.py + :caption: :py:meth:`.ArduPilotNode.__init__` method + :pyobject: ArduPilotNode.__init__ + +The parent :class:`.AutopilotNode` defines the common GISNav internal message types that the extending +:class:`.PX4Node` and :class:`.ArduPilotNode` must implement. The parent class also defines publish methods that the +extending nodes call when they want to publish the GISNav internal messages. Typically this happens whenever they +receive a relevant message from the autopilot: + +.. literalinclude:: ../../../../gisnav/nodes/base/autopilot_node.py + :caption: :class:`.AutopilotNode` public properties and publish hooks + :start-after: # region publish hooks + :end-before: # endregion publish hooks + :language: python + +Camera topics +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +GISNav's nodes use the :class:`.CameraSubscriberNode` abstract base class to subscribe to +:class:`sensor_msgs.msg.CameraInfo` and :class:`sensor_msgs.msg.Image` topics. The topic default names are stored in +the :py:attr:`.CameraSubscriberNode.ROS_CAMERA_INFO_TOPIC` and :py:attr:`.CameraSubscriberNode.ROS_IMAGE_TOPIC` +class constants. + +The :class:`.CameraSubscriberNode` parent class handles the :class:`sensor_msgs.msg.CameraInfo` message and provides +the info through the :py:attr:`.CameraSubscriberNode.camera_data` property, but leaves the handling of the +:class:`sensor_msgs.msg.Image` to the extending classes: + +.. literalinclude:: ../../../../gisnav/nodes/base/camera_subscriber_node.py + :caption: :py:meth:`.CameraSubscriberNode.__camera_info_callback` method + :pyobject: CameraSubscriberNode.__camera_info_callback + +.. literalinclude:: ../../../../gisnav/nodes/base/camera_subscriber_node.py + :caption: :py:meth:`.CameraSubscriberNode.camera_data` property + :pyobject: CameraSubscriberNode.camera_data + +.. literalinclude:: ../../../../gisnav/nodes/base/camera_subscriber_node.py + :caption: :py:meth:`.CameraSubscriberNode.image_callback` abstract method + :pyobject: CameraSubscriberNode.image_callback + +.. note:: + In the KSQL airport SITL demo, ``gscam`` is used to publish the :class:`sensor_msgs.msg.CameraInfo` and + :class:`sensor_msgs.msg.Image` messages. They are not published over the PX4-ROS 2 bridge. + +Aircraft GeoPose estimate topics +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +:class:`.PoseEstimationNode` is responsible for outputting the aircraft's geographical pose estimate to the +:py:attr:`.messaging.ROS_TOPIC_VEHICLE_GEOPOSE_ESTIMATE` and :py:attr:`.messaging.ROS_TOPIC_VEHICLE_ALTITUDE_ESTIMATE` +topics. The altitude topic provides `additional altitude types such as above-mean-sea-level (AMSL)`_ altitude. You can +see the message types defined in the class's ``__init__`` method: + +.. _additional altitude types such as above-mean-sea-level (AMSL): https://ardupilot.org/copter/docs/common-understanding-altitude.html + +.. literalinclude:: ../../../../gisnav/nodes/pose_estimation_node.py + :caption: :meth:`.PoseEstimationNode.__init__` publishers assignment + :start-after: # region publishers + :end-before: # endregion publishers + :language: python + +These two messages are used by :class:`.MockGPSNode` to generate and publish the mock GPS message that the autopilot +will use for navigation. diff --git a/docs/pages/developer_guide/launch/launch_files.rst b/docs/pages/developer_guide/launch/launch_files.rst new file mode 100644 index 00000000..7b0602cb --- /dev/null +++ b/docs/pages/developer_guide/launch/launch_files.rst @@ -0,0 +1,67 @@ +Launch from ROS launch file +____________________________________________________ +The easiest way to launch GISNav is to use the premade launch files in the ``launch/`` folder. + +Assuming you have already installed the ``gisnav`` colcon package, you can launch with a single command: + +.. tab-set:: + + .. tab-item:: PX4 + :selected: + + .. code-block:: bash + :caption: Launch GISNav for PX4 + + ros2 launch gisnav px4.launch.py + + Your shell should then start showing log messages generated by GISNav's nodes: + + .. code-block:: text + :caption: Example output when launching GISNav for PX4 + + hmakelin@hmakelin-Nitro-AN515-54:~/workspace/px4_ros_com_ros2$ ros2 launch gisnav px4.launch.py + [INFO] [launch]: All log files can be found below /home/hmakelin/.ros/log/2022-12-14-13-55-31-751707-hmakelin-Nitro-AN515-54-1269819 + [INFO] [launch]: Default logging verbosity is set to INFO + [INFO] [px4_node-1]: process started with pid [1269837] + [INFO] [mock_gps_node-2]: process started with pid [1269839] + [INFO] [map_node-3]: process started with pid [1269841] + [INFO] [bbox_node-4]: process started with pid [1269843] + [INFO] [pose_estimation_node-5]: process started with pid [1269845] + [pose_estimation_node-5] [INFO] [1671026134.317154046] [pose_estimation_node]: ROS parameter "max_pitch" already declared with value "30". + [pose_estimation_node-5] [INFO] [1671026134.317957668] [pose_estimation_node]: ROS parameter "min_match_altitude" already declared with value "50". + [pose_estimation_node-5] [INFO] [1671026134.318823560] [pose_estimation_node]: ROS parameter "attitude_deviation_threshold" already declared with value "10". + [pose_estimation_node-5] [INFO] [1671026134.319698017] [pose_estimation_node]: ROS parameter "export_position" already declared with value "". + [pose_estimation_node-5] [INFO] [1671026134.326348362] [pose_estimation_node]: Loaded params: + ... + + .. tab-item:: ArduPilot + + .. code-block:: bash + :caption: Launch GISNav for ArduPilot + + ros2 launch gisnav ardupilot.launch.py + + Your shell should then start showing log messages generated by GISNav's nodes: + + .. code-block:: text + :caption: Example output when launching GISNav for ArduPilot + + hmakelin@hmakelin-Nitro-AN515-54:~/workspace/px4_ros_com_ros2$ ros2 launch gisnav ardupilot.launch.py + [INFO] [launch]: All log files can be found below /home/hmakelin/.ros/log/2022-12-14-20-15-13-434747-hmakelin-Nitro-AN515-54-1299156 + [INFO] [launch]: Default logging verbosity is set to INFO + [INFO] [ardupilot_node-1]: process started with pid [1299173] + [INFO] [mock_gps_node-2]: process started with pid [1299175] + [INFO] [map_node-3]: process started with pid [1299177] + [INFO] [bbox_node-4]: process started with pid [1299179] + [INFO] [pose_estimation_node-5]: process started with pid [1299181] + [pose_estimation_node-5] [INFO] [1671048916.388314406] [pose_estimation_node]: ROS parameter "max_pitch" already declared with value "30". + [pose_estimation_node-5] [INFO] [1671048916.389742861] [pose_estimation_node]: ROS parameter "min_match_altitude" already declared with value "50". + [pose_estimation_node-5] [INFO] [1671048916.391036526] [pose_estimation_node]: ROS parameter "attitude_deviation_threshold" already declared with value "10". + [pose_estimation_node-5] [INFO] [1671048916.392134461] [pose_estimation_node]: ROS parameter "export_position" already declared with value "". + [pose_estimation_node-5] [INFO] [1671048916.398298326] [pose_estimation_node]: Loaded params: + ... + +.. seealso:: + See the `Creating Launch Files`_ tutorial for more information on the ROS 2 launch system + + .. _Creating Launch Files: https://docs.ros.org/en/foxy/Tutorials/Intermediate/Launch/Creating-Launch-Files.html \ No newline at end of file diff --git a/docs/pages/developer_guide/launch/modify_params.rst b/docs/pages/developer_guide/launch/modify_params.rst new file mode 100644 index 00000000..fe78f74c --- /dev/null +++ b/docs/pages/developer_guide/launch/modify_params.rst @@ -0,0 +1,19 @@ +Modify ROS parameters +____________________________________________________ +You can provide overrides for the ROS parameters at launch by modifying the node-specific YAML files in the +``launch/params/`` folder. For example, to provide parameter overrides to :class:`.MapNode` at launch, you would modify +the ``launch/params/map_node.yaml`` file: + +.. literalinclude:: ../../../../launch/params/map_node.yaml + :caption: launch/params/map_node.yaml + :language: yaml + +You can take a look at the :py:attr:`.MapNode.ROS_PARAM_DEFAULTS` property for the hard-coded default values as well as +get an exhaustive list of all ROS parameters declared by the node (not everything is necessarily included in the YAML +file). Each node will have this property and use it if no overrides are provided in the YAML files. + +.. note:: + You should at least configure your :ref:`GIS server` WMS endpoint in the ``launch/params/map_node.yaml`` file unless + you are using the `gisnav-docker`_ ``sitl`` service, in which case the defaults should work. + + .. _gisnav-docker: https://github.com/hmakelin/gisnav-docker diff --git a/docs/pages/developer_guide/launch/run_individual_nodes.rst b/docs/pages/developer_guide/launch/run_individual_nodes.rst new file mode 100644 index 00000000..ec9da9eb --- /dev/null +++ b/docs/pages/developer_guide/launch/run_individual_nodes.rst @@ -0,0 +1,28 @@ +Run individual node +____________________________________________________ +It is recommended that you :ref:`Launch from ROS launch file`. However, you can also launch individual nodes +and optionally provide launch values for ROS parameters from the YAML files in the ``launch/params/`` folder: + +.. code-block:: bash + :caption: Run :class:`.MapNode` with ``INFO`` log level and optional YAML parameter file + + cd ~/colcon_ws + ros2 run gisnav map_node --ros-args --log-level info --params-file src/gisnav/launch/params/map_node.yaml + +Your shell should then start showing log messages generated by :class:`.MapNode` at your chosen log level: + +.. code-block:: text + :caption: Example output when launching GISNav for PX4 + + hmakelin@hmakelin-Nitro-AN515-54:~/workspace/px4_ros_com_ros2$ ros2 run gisnav map_node --ros-args --log-level info --params-file src/gisnav/launch/params/map_node.yaml + [INFO] [1671026892.443398239] [map_node]: ROS parameter "layers" already declared with value "['imagery']". + [INFO] [1671026892.444331459] [map_node]: Using default value "['']" for ROS parameter "styles". + [INFO] [1671026892.445077047] [map_node]: ROS parameter "dem_layers" already declared with value "['osm-buildings']". + [INFO] [1671026892.445934245] [map_node]: Using default value "['']" for ROS parameter "dem_styles". + [INFO] [1671026892.446790070] [map_node]: Using default value "EPSG:4326" for ROS parameter "srs". + [INFO] [1671026892.447671806] [map_node]: Using default value "False" for ROS parameter "transparency". + [INFO] [1671026892.448461329] [map_node]: Using default value "image/jpeg" for ROS parameter "format". + [INFO] [1671026892.449245454] [map_node]: Using default value "0.85" for ROS parameter "map_overlap_update_threshold". + [ERROR] [1671026893.054366929] [map_node]: Connecting to WMS endpoint... + [ERROR] [1671026893.058634673] [map_node]: Could not connect to WMS endpoint, trying again in 10s... + ... \ No newline at end of file diff --git a/docs/pages/developer_guide/sitl_environment/autopilot.rst b/docs/pages/developer_guide/sitl_environment/autopilot.rst new file mode 100644 index 00000000..25fc0612 --- /dev/null +++ b/docs/pages/developer_guide/sitl_environment/autopilot.rst @@ -0,0 +1,216 @@ +Autopilots +================================================== +GISNav supports PX4 via the `RTPS/DDS`_ interface and ArduPilot via `MAVROS`_. + +.. warning:: + ArduPilot is licensed under `GPLv3`_ which is more restrictive than PX4's `BSD`_ license + +.. _GPLv3: https://ardupilot.org/dev/docs/license-gplv3.html +.. _BSD: https://docs.px4.io/main/en/contribute/licenses.html +.. _RTPS/DDS: https://docs.px4.io/main/en/middleware/micrortps.html +.. _MAVROS: https://ardupilot.org/dev/docs/ros-connecting.html + +Setup PX4 +___________________________________________________ +**These instructions are written for PX4 version >=1.13.0, <=1.14.0-beta1** + +Follow the PX4 instructions to setup your `Ubuntu Development Environment`_ with `Fast DDS`_ support. + +.. _Ubuntu Development Environment: https://docs.px4.io/master/en/simulation/ros_interface.html +.. _Fast DDS: https://docs.px4.io/main/en/dev_setup/fast-dds-installation.html + +Build Gazebo simulation +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Once you have installed PX4 Autopilot, you can try out the `PX4 Gazebo simulation`_ to make sure everything is working +correctly. The following commands should pop out a Gazebo window with a Typhoon H480 drone sitting somewhere in the +vicinity of San Carlos (KSQL) airport: + +.. _PX4 Gazebo simulation: https://docs.px4.io/main/en/simulation/gazebo.html + +.. tab-set:: + + .. tab-item:: v1.14.0-beta1 + :selected: + + .. code-block:: bash + :caption: Run PX4 Gazebo SITL simulation at KSQL airport + + cd ~/PX4-Autopilot + make px4_sitl gazebo_typhoon_h480__ksql_airport + + + .. tab-item:: v1.13.X + + Run the microRTPS agent in a separate shell: + + .. code-block:: bash + :name: Launch microRTPS agent + :caption: Run microRTPS agent + + cd ~/colcon_ws + micrortps_agent -t UDP + + Then build your simulation: + + .. code-block:: bash + :caption: Run PX4 Gazebo SITL simulation at KSQL airport + + cd ~/PX4-Autopilot + make px4_sitl_rtps gazebo_typhoon_h480__ksql_airport + +.. note:: + The initial build may take several minutes + +PX4-ROS 2 bridge topic configuration +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +For GISNav the PX4-ROS 2 bridge must be configured to send the :class:`px4_msgs.msg.GimbalDeviceSetAttitude` message +and receive the :class:`px4_msgs.msg.SensorGps` message: + +.. tab-set:: + + .. tab-item:: v1.14.0-beta1 + :selected: + + Edit the ``~/PX4-Autopilot/src/modules/microdds_client/microdds_topics.yaml`` file by adding the following + entries: + + .. code-block:: yaml + :caption: PX4-Autopilot/src/modules/microdds_client/microdds_topics.yaml + + publications: + + - topic: /fmu/out/gimbal_device_set_attitude + type: px4_msgs::msg::GimbalDeviceSetAttitude + + subscriptions: + + - topic: /fmu/in/sensor_gps + type: px4_msgs::msg::SensorGps + + .. tab-item:: v1.13.X + + See the `ROS 2 Offboard Control Example`_ for example on how to edit the ``urtps_bridge_topics.yaml`` file in + the ``~/PX4-Autopilot/msg/tools`` and ``~/colcon_ws/src/px4_ros_com/templates`` folders. Add the following + entries to the files: + + .. _ROS 2 Offboard Control Example: https://docs.px4.io/main/en/ros/ros2_offboard_control.html#ros-2-offboard-control-example + + .. list-table:: ``urtps_bridge_topics.yaml`` + :header-rows: 1 + + * - PX4-Autopilot/msg/tools + - px4_ros_com_ros2/src/px4_ros_com/templates + * - .. code-block:: yaml + + - msg: gimbal_device_set_attitude + send: true + + - msg: sensor_gps + receive: true + + - .. code-block:: yaml + + - msg: GimbalDeviceSetAttitude + send: true + + - msg: SensorGps + receive: true + +After you have configured the topics, you can :ref:`Build Gazebo simulation` again. + +PX4 parameter configuration +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +.. warning:: + Do not use this configuration for real drone flights. This configuration is intended for simulation use only. + +To make GISNav potentially work better, you can adjust the following PX4 parameters either at runtime through the PX4 +shell or the `QGroundControl Parameters screen`_, or before building the simulation in the +``~/PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480`` file : + +.. _QGroundControl Parameters screen: https://docs.qgroundcontrol.com/master/en/SetupView/Parameters.html + +.. code-block:: + :caption: PX4 parameter defaults for GISNav + + param set-default NAV_ACC_RAD 20.0 + param set-default MPC_YAWRAUTO_MAX 10.0 + param set-default COM_POS_FS_DELAY 5 + + param set-default EKF2_GPS_P_NOISE 10 + param set-default EKF2_GPS_V_NOISE 3 + + param set-default SENS_GPS_MASK 2 + +.. note:: + This is a sample configuration that seems to work, but you may want to experiment with the parameters. + + The first three parameters make the waypoint turns softer and reduces the yaw rate. This makes the field of view + move and rotate more slowly especially if the camera has some pitch (is not completely nadir-facing). A slower + moving camera field of view makes it easier for GISNav to keep track of position at tight turns and prevent the + position delay failsafe from triggering. + + Increasing the position failsafe delay may help if your GPU is slower or GISNav for some reason cannot produce a + position estimate for a number of subsequent frames. However as a failsafe parameter it should not be made + unreasonably large. + + The two EKF2 parameters 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 this + configuration example also has lower tolerance for vertical position error. + + The final parameter should make PX4 blend GPS based on horizontal position accuracy. + + .. _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 + +Video streaming with gscam +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +The ``typhoon_h480`` build target for Gazebo SITL simulation supports UDP `video streaming`_. Here we will use +``gscam`` to publish the UDP video stream to ROS 2 to make it accessible for GISNav: + +.. _video streaming: https://docs.px4.io/master/en/simulation/gazebo.html#video-streaming + +Install ``gscam`` and dependencies: + +.. code-block:: bash + :caption: Install gscam and dependencies + + sudo apt-get install -y gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl ros-foxy-gscam + +Use the sample camera and gstreamer configuration files in the GISNav repository to run ``gscam`` in a dedicated shell: + +.. code-block:: bash + :caption: Run gscam_node with example configuration files + + cd ~/colcon_ws + ros2 run gscam gscam_node --ros-args --params-file src/gisnav/test/assets/gscam_params.yaml \ + -p camera_info_url:=file://$PWD/src/gisnav/test/assets/camera_calibration.yaml + +.. seealso:: + See `How to Calibrate a Monocular Camera`_ on how to create a custom camera calibration file if you do not want to + use the provided example + + .. _How to Calibrate a Monocular Camera: https://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration + +Setup ArduPilot +___________________________________________________ +The following tutorials should help you setup an ArduPilot SITL simulation environment: + +* `Setting up SITL on Linux`_ +* `Using Gazebo simulator with SITL`_ +* `Connecting with ROS`_ + +.. _Setting up SITL on Linux: https://ardupilot.org/dev/docs/setting-up-sitl-on-linux.html +.. _Using Gazebo simulator with SITL: https://ardupilot.org/dev/docs/using-gazebo-simulator-with-sitl.html +.. _Connecting with ROS: https://ardupilot.org/dev/docs/ros-connecting.html + +The ``gazebo-iris`` model in the ArduPilot SITL simulation included in the `gisnav-docker`_ ``sitl`` service currently +has a static camera that faces directly down from the aircraft body (the ``typhoon_h480`` model in the PX4 simulation +has a proper simulated 2-axis gimbal). Because the camera is not stabilized, it possibly won't be reliable enough to +act as a full replacement for GPS in ArduPilot's mission mode, while loitering without GPS may work. + +.. _gisnav-docker: https://github.com/hmakelin/gisnav-docker + +.. note:: + *Unverified*: You may have to `enable virtual joystick`_ from QGroundControl settings and have it centered to + maintain altitude in ArduPilot's Loiter mode in the SITL simulation. + + .. _enable virtual joystick: https://docs.qgroundcontrol.com/master/en/SettingsView/VirtualJoystick.html diff --git a/docs/pages/developer_guide/sitl_environment/docker.rst b/docs/pages/developer_guide/sitl_environment/docker.rst new file mode 100644 index 00000000..e5678573 --- /dev/null +++ b/docs/pages/developer_guide/sitl_environment/docker.rst @@ -0,0 +1,9 @@ +Docker +____________________________________________________ +`Dockerized environments `_ are available. Take a look at the +Dockerfiles to see how everything is set up. These instructions should closely mirror what is included in the +Dockerfiles with only minor differences. + +You can also take a look at the prebuilt `sitl `_ and `mapserver +`_ images available in Docker Hub in case you have problems with +the build process. diff --git a/docs/pages/developer_guide/sitl_environment/gis_server.rst b/docs/pages/developer_guide/sitl_environment/gis_server.rst new file mode 100644 index 00000000..3a7388f2 --- /dev/null +++ b/docs/pages/developer_guide/sitl_environment/gis_server.rst @@ -0,0 +1,189 @@ +GIS server +______________________________________________________ +Overview +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +GISNav requires access to a GIS server that serves high resolution `orthoimagery`_ and optional +`digital elevation models`_ (DEM) for the approximate location of the aircraft. The orthoimage and DEM `rasters`_ are +requested from an `OGC Web Map Service`_ (WMS) which allows querying map images by an arbitrary `bounding box`_ via +its `GetMap`_ operation. + +.. _orthoimagery: https://en.wikipedia.org/wiki/Orthophoto +.. _digital elevation models: https://en.wikipedia.org/wiki/Digital_elevation_model +.. _rasters: https://carto.com/blog/raster-vs-vector-whats-the-difference-which-is-best +.. _OGC Web Map Service: https://www.ogc.org/standards/wms +.. _bounding box: https://wiki.openstreetmap.org/wiki/Bounding_Box +.. _GetMap: https://opengeospatial.github.io/e-learning/wms/text/operations.html#getmap + +.. seealso:: + The DEM is optionally used to input terrain z-coordinates (depth, or altitude) to the `PnP`_ problem solved by + :meth:`.KeypointPoseEstimator.estimate`. If a DEM is not available GISNav simply assumes a planar terrain which + may be less accurate. See :ref:`SuperGlue & LoFTR` for more information on how GISNav estimates aircraft pose from + the map rasters. + + .. _PnP: https://docs.opencv.org/4.x/d5/d1f/calib3d_solvePnP.html + +Example setups +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +You are encouraged to self-host a GIS server with public domain :ref:`Orthoimagery and DEMs`, although it may be more +convenient to proxy an existing commercial tile-based endpoint. + +.. tab-set:: + + .. tab-item:: Self-hosted GIS + :selected: + + The primary benefit of a self-hosted WMS service is that you can embed it onboard the drone and not rely on an + internet connection. Here we provide an example on how to host your own maps using `MapServer`_. If you are + fine with using maps for the SITL simulation demo area only, then you can simply use the Docker images from the + `gisnav-docker`_ repository. Otherwise see the instructions below. + + .. _MapServer: https://mapserver.org/ + .. _gisnav-docker: https://github.com/hmakelin/gisnav-docker + + .. seealso:: + See :ref:`GIS software` for `free and open-source software`_ (FOSS) MapServer alternatives + + .. _free and open-source software: https://en.wikipedia.org/wiki/Free_and_open-source_software + + To follow these instructions you will need: + + * An AWS account and AWS CLI, **or alternatively**, an `EarthExplorer`_ account + * `GDAL`_ installed + + .. _EarthExplorer: https://earthexplorer.usgs.gov + .. _GDAL: https://gdal.org + + In this example we will download `NAIP`_ imagery and host it using the `MapServer docker image`_ from Docker + Hub. You can download the GeoTIFF imagery from EarthExplorer, or from the Esri-maintained `AWS S3 bucket`_ if + you already have AWS CLI set up: + + .. _NAIP: https://www.usgs.gov/centers/eros/science/usgs-eros-archive-aerial-photography-national-agriculture-imagery-program-naip + .. _MapServer docker image: https://hub.docker.com/r/camptocamp/mapserver + .. _AWS S3 bucket: https://registry.opendata.aws/naip/ + + .. warning:: + This is a **Requester Pays** bucket and the files can be very large so download only what you need. + + .. code-block:: bash + :caption: Example: Downloading a NAIP imagery product from the AWS S3 bucket + + cd ~/gisnav-docker + 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 \ + mapfiles/ + + .. note:: + * The NAIP imagery is in the public domain. However, you must create an EROS account to download + the rasters from EarthExplorer, or use secondary sources such as the AWS S3 bucket mentioned above. The + data is not redistributed in the `gisnav-docker`_ repository to keep its size manageable. + * You do not need an account to browse for product IDs with EarthExplorer. An account is only needed if you + want to download products. + + Once you have the imagery, use GDAL to make a ``naip.vrt`` VRT file out of your downloaded GeoTIFFs: + + .. code-block:: bash + :caption: Use GDAL to create a VRT from TIFF files + + cd mapfiles/ + gdalbuildvrt naip.vrt *.tif + + Once you have your .tif and .vrt files, you can run host them through a MapServer container: + + .. code-block:: bash + :caption: Serve the map layer using the MapServer Docker image + + cd ~/gisnav-docker + export CONTAINER_NAME=gisnav-mapserver + export MAPSERVER_PATH=/etc/mapserver + docker run \ + --name $CONTAINER_NAME \ + -p 80:80 \ + -v $PWD/mapfiles/:$MASERVER_PATH/:ro \ + camptocamp/mapserver + + Test your MapServer WMS service by opening the capabilities XML in your browser: + + .. code-block:: bash + :caption: Launch a WMS GetCapabilities request in Firefox + + firefox "http://localhost:80/?map=/etc/mapserver/wms.map&service=WMS&request=GetCapabilities" + + .. tab-item:: 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. You can follow the `gisnav-docker README.md`_ to set up a WMS MapProxy using + the provided Docker image. + + .. _gisnav-docker README.md: https://github.com/hmakelin/gisnav-docker + + .. note:: + Commercial web-based map services are often `tile-based`_ (as opposed to WMS) because it is more + efficient to serve pre-rendered tiles than to render unique rasters for each individual requested bounding + box. You will need a WMS proxy if you decide to go with a tile-based endpoint. + + .. _tile-based: https://wiki.openstreetmap.org/wiki/Slippy_map_tilenames + + .. warning:: + Many commercial services explicitly prohibit the caching of map tiles in their Terms of Use (ToU), + especially if their business model is based on billing API requests. This is mainly to prevent + disintermediation in case their tiles are redistributed to a large number of end users. + + While caching tiles onboard your own drone is likely not the kind of misuse targeted by such clauses, you + should still make sure you understand the ToU of the service you are using and that it fits your planned + use case. + +GIS software +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +If you want to run your own GIS server or WMS proxy, you may want to consider e.g. these +`free and open-source software`_ (FOSS) options: + + * `MapServer`_ + + * `GeoServer`_ (full-fledged OGC-compliant GIS server) + + * `Mapnik`_ and `MapProxy`_ + +.. _free and open-source software: https://en.wikipedia.org/wiki/Free_and_open-source_software +.. _GeoServer: https://geoserver.org +.. _Mapnik: https://mapnik.org +.. _MapProxy: https://mapproxy.org + +Orthoimagery and DEMs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +If you do not want to use commercial (=not free) high-resolution imagery, various national agencies often provide +country-specific aerial imagery in the public domain or with public-domain-like licensing terms. You should look for +imagery available in `GDAL`_ supported formats with coverage for your area. These may be provided as +downloadable products or through OGC-compliant web services such as WMS or WMTS. Below are just a few examples of +national agencies providing high-resolution orthoimagery that should be suitable for use with GISNav: + +* `USGS High Resolution Orthoimagery`_ (USA) +* `Environment Agency Vertical Aerial Photography`_ (United Kingdom) +* `NLS orthophotos`_ (Finland) + +.. _USGS High Resolution Orthoimagery: https://www.usgs.gov/centers/eros/science/usgs-eros-archive-aerial-photography-high-resolution-orthoimagery-hro +.. _Environment Agency Vertical Aerial Photography: https://www.data.gov.uk/dataset/4921f8a1-d47e-458b-873b-2a489b1c8165/vertical-aerial-photography +.. _NLS orthophotos: https://www.maanmittauslaitos.fi/en/maps-and-spatial-data/expert-users/product-descriptions/orthophotos + +.. note:: + If you have a drone, you can also use readily available `photogrammetry`_ software to create your own maps for your + local region of interest + +.. _photogrammetry: https://en.wikipedia.org/wiki/Photogrammetry + +Rasterizing vector data +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +In some cases useful map data is not directly provided in raster but in vector format. The GISNav SITL service in +the `gisnav-docker`_ repository utilizes vector-format elevation data from `OSM Buildings`_ to determine building +heights in the simulation area to improve accuracy* of pose estimates especially at lower flight altitudes where the +perceived planarity of the terrain is lower. For an example on how the vector data is rasterized using GDAL, +see `this gisnav-docker setup script`_. + +.. note:: + \*The GISNav SITL demo simulation does not actually benefit from the building height data because the simulated + KSQL Airport model buildings are all featureless black blocks. See :ref:`SITL simulation quirks` for more + information. + +.. _OSM Buildings: https://osmbuildings.org/ +.. _this gisnav-docker setup script: https://github.com/hmakelin/gisnav-docker/blob/master/scripts/setup_mapserver.sh \ No newline at end of file diff --git a/docs/pages/developer_guide/sitl_environment/install_gisnav.rst b/docs/pages/developer_guide/sitl_environment/install_gisnav.rst new file mode 100644 index 00000000..3cf53b62 --- /dev/null +++ b/docs/pages/developer_guide/sitl_environment/install_gisnav.rst @@ -0,0 +1,97 @@ +GISNav ROS package +____________________________________________________ +Install ROS 2 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +If you followed the :ref:`Autopilots` SITL install instructions you should already have ROS 2 installed. See +`ROS 2 foxy install instructions`_ if you do not yet have ROS 2 on your machine. You can check your +ROS version with the ``whereis ros2`` command: + +.. code-block:: text + :caption: Check ROS version example output + + hmakelin@hmakelin-Nitro-AN515-54:~$ whereis ros2 + ros2: /opt/ros/foxy/bin/ros2 + +.. _ROS 2 foxy install instructions: https://docs.ros.org/en/foxy/Installation.html + +.. note:: + Once you have your `ROS 2 workspace`_ set up, consider automatically sourcing it in your ``~/.bashrc`` to avoid + manual repetition: + + .. _ROS 2 Workspace: https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html + + .. code-block:: bash + :caption: Source ROS 2 workspace in ~/.bashrc + + echo "source /opt/ros/foxy/setup.bash" >> ~/.bashrc + echo "source ~/colcon_ws/install/setup.bash" >> ~/.bashrc + +Install GISNav +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Install GISNav in your ROS 2 Workspace: + +.. code-block:: bash + :caption: Install GISNav in colcon workspace + + cd ~/colcon_ws + mkdir -p src && cd "$_" + git clone https://github.com/hmakelin/gisnav.git + cd gisnav + pip3 install -r requirements.txt + +.. note:: + If you have followed the `PX4 ROS 2 User Guide`_, your workspace may be called ``px4_ros_com_ros2`` instead of + ``colcon_ws`` + + .. _PX4 ROS 2 User Guide: https://docs.px4.io/main/en/ros/ros2_comm.html + +Download the LoFTR submodule and weights: + +.. code-block:: bash + :caption: Install LoFTR keypoint matcher + + cd ~/colcon_ws/src/gisnav + git submodule update LoFTR + mkdir weights && cd "$_" + pip3 install gdown + gdown https://drive.google.com/uc?id=1M-VD35-qdB5Iw-AtbDBCKC7hPolFW9UY + +.. note:: + The example downloads the dual-softmax (_ds suffix) outdoor weights which are permissively licensed (does not use + SuperGlue) + +Build the GISNav package along with other dependencies you may have in your colcon workspace. If you have already built +the other dependencies (such as ``px4_msgs`` and ``px4_ros_com`` for PX4 configuration) earlier you may want to skip +rebuilding them and build GISNav only to save time: + +.. tab-set:: + + .. tab-item:: With dependencies + :selected: + + .. code-block:: bash + :caption: Build colcon workspace + + cd ~/colcon_ws + colcon build + + .. tab-item:: GISNav only + + .. code-block:: bash + :caption: Build GISNav package only + + cd ~/colcon_ws + colcon build --packages-select gisnav + +Once GISNav is installed, you can try to :ref:`Launch from ROS launch file`. + +Development dependencies +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +The development dependencies are required to :ref:`Generate documentation` and run :ref:`Launch tests`. Install them +with the following commands: + +.. code-block:: bash + :caption: Install Python development dependencies + + cd ~/colcon_ws/src/gisnav + python3 -m pip install -r requirements-dev.txt diff --git a/docs/pages/developer_guide/sitl_environment/prerequisites.rst b/docs/pages/developer_guide/sitl_environment/prerequisites.rst new file mode 100644 index 00000000..2df0e1b0 --- /dev/null +++ b/docs/pages/developer_guide/sitl_environment/prerequisites.rst @@ -0,0 +1,35 @@ +Prerequisites +____________________________________________________ +These instructions assume you are running **Ubuntu 20.04 (Focal Fossa)**, although with small changes other releases +might also work. + +It is strongly recommended that you have an **NVIDIA GPU and CUDA** installed. You can inspect your NVIDIA driver and +CUDA versions with the ``nvidia-smi`` command line utility. If you don't have it installed, follow the `NVIDIA CUDA +Installation Guide for Linux `_. The output of +the ``nvidia-smi`` command should look something like this: + +.. code-block:: text + :caption: Example output of nvidia-smi command + + hmakelin@hmakelin-Nitro-AN515-54:~$ nvidia-smi + Thu Dec 15 12:00:12 2022 + +-----------------------------------------------------------------------------+ + | NVIDIA-SMI 520.61.05 Driver Version: 520.61.05 CUDA Version: 11.8 | + |-------------------------------+----------------------+----------------------+ + | GPU Name Persistence-M| Bus-Id Disp.A | Volatile Uncorr. ECC | + | Fan Temp Perf Pwr:Usage/Cap| Memory-Usage | GPU-Util Compute M. | + | | | MIG M. | + |===============================+======================+======================| + | 0 NVIDIA GeForce ... On | 00000000:01:00.0 On | N/A | + | N/A 73C P8 7W / N/A | 69MiB / 6144MiB | 23% Default | + | | | N/A | + +-------------------------------+----------------------+----------------------+ + + +-----------------------------------------------------------------------------+ + | Processes: | + | GPU GI CI PID Type Process name GPU Memory | + | ID ID Usage | + |=============================================================================| + | 0 N/A N/A 1353 G /usr/lib/xorg/Xorg 22MiB | + | 0 N/A N/A 2210 G /usr/lib/xorg/Xorg 45MiB | + +-----------------------------------------------------------------------------+ diff --git a/docs/pages/developer_guide/sitl_environment/qgroundcontrol.rst b/docs/pages/developer_guide/sitl_environment/qgroundcontrol.rst new file mode 100644 index 00000000..ea6bd372 --- /dev/null +++ b/docs/pages/developer_guide/sitl_environment/qgroundcontrol.rst @@ -0,0 +1,35 @@ +QGroundControl +___________________________________________________ +QGroundControl is the `recommended ground control station for PX4`_ and also `compatible with Ardupilot`_. It is needed +for controlling the drone in the SITL (software-in-the-loop) simulation. + +.. _recommended ground control station for PX4: https://docs.px4.io/main/en/getting_started/px4_basic_concepts.html#qgroundcontrol +.. _compatible with Ardupilot: https://ardupilot.org/copter/docs/common-choosing-a-ground-station.html?highlight=qgroundcontrol#qgroundcontrol + +**Install QGroundControl** by following the `official instructions`_. + +.. _official instructions: https://docs.qgroundcontrol.com/master/en/getting_started/quick_start.html + +You can then run QGroundControl from the directory where you installed it, for example: + +.. code-block:: bash + :caption: Run QGroundControl + + ~/Applications/QGroundControl.AppImage + +.. note:: + You may need to change the QGroundControl app image file permissions and/or extract it before you can run it. + Assuming you downloaded the app image to the ``~/Applications`` folder: + + .. code-block:: bash + :caption: Change file permissions + + cd ~/Applications + chmod +x QGroundControl.AppImage + ./QGroundControl.AppImage + + .. code-block:: bash + :caption: Extract and run + + cd ~/Applications + ./QGroundControl.AppImage --appimage-extract-and-run diff --git a/docs/pages/developer_guide/test/code_coverage.rst b/docs/pages/developer_guide/test/code_coverage.rst new file mode 100644 index 00000000..f78c62a8 --- /dev/null +++ b/docs/pages/developer_guide/test/code_coverage.rst @@ -0,0 +1,12 @@ +Generate code coverage reports +____________________________________________________ +To generate and inspect code coverage you can use ``coverage.py``. See the +`official instructions `_ on how to configure what source files +to measure: + +.. code-block:: bash + :caption: Run and inspect code coverage report for ROS launch tests for PX4 (Fast DDS) launch configuration + + cd ~/colcon_ws + python3 -m coverage run --branch --include */site-packages/gisnav/* src/gisnav/test/test_px4_launch.py + python3 -m coverage report diff --git a/docs/pages/developer_guide/test/launch_tests.rst b/docs/pages/developer_guide/test/launch_tests.rst new file mode 100644 index 00000000..ff329ab7 --- /dev/null +++ b/docs/pages/developer_guide/test/launch_tests.rst @@ -0,0 +1,23 @@ +Launch tests +____________________________________________________ +`Launch tests `_ are provided for smoke testing launch files for common +launch configurations in the :py:mod:`test.launch` package: + +.. tab-set:: + + .. tab-item:: PX4 + :selected: + + .. code-block:: bash + :caption: Run ROS launch tests for PX4 (Fast DDS) launch configuration + + cd ~/colcon_ws + launch_test src/gisnav/test/launch/test_px4_launch.py + + .. tab-item:: ArduPilot + + .. code-block:: bash + :caption: Run ROS launch tests for ArduPilot (MAVROS) launch configuration + + cd ~/colcon_ws + launch_test src/gisnav/test/launch/test_ardupilot_launch.py diff --git a/docs/pages/developer_guide/test/sitl_tests.rst b/docs/pages/developer_guide/test/sitl_tests.rst new file mode 100644 index 00000000..7934c78d --- /dev/null +++ b/docs/pages/developer_guide/test/sitl_tests.rst @@ -0,0 +1,21 @@ +SITL tests +____________________________________________________ +SITL tests are under the ``test/sitl`` folder. They are simple Python scripts: + +.. code-block:: bash + + cd ~/px4_ros_com_ros2/src/gisnav/test/sitl + python sitl_test_mock_gps_node.py + +Flight Log Analysis +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +The flight log generated by the SITL test can be analyzed with the Jupyter notebooks in ``test/ulog_analysis`` folder. +You must first start ``jupyter-notebook``: + +.. code-block:: bash + + cd ~/px4_ros_com_ros2/src/gisnav/test/sitl/ulog_analysis + jupyter-notebook + +The notebook documents the analysis and displays the results. Download the example ULog file from Google Drive `here +`_. diff --git a/docs/pages/developer_guide/test/test_prerequisites.rst b/docs/pages/developer_guide/test/test_prerequisites.rst new file mode 100644 index 00000000..97a493dd --- /dev/null +++ b/docs/pages/developer_guide/test/test_prerequisites.rst @@ -0,0 +1,4 @@ +Test prerequisites +____________________________________________________ +.. note:: + Install the :ref:`Development dependencies` before you attempt to run any tests in this section \ No newline at end of file diff --git a/docs/pages/setup.rst b/docs/pages/setup.rst deleted file mode 100644 index a65b6ec4..00000000 --- a/docs/pages/setup.rst +++ /dev/null @@ -1,483 +0,0 @@ -************************************************** -Setup -************************************************** -This page provides instruction on how to setup a local GISNav development and SITL simulation environment. - -`Dockerized environments `_ are also available. Take a look at the -Dockerfiles to see how everything is set up. These instructions should closely mirror what is included in the -Dockerfiles with only minor differences. - -Prerequisites -=================================================== - -* These instructions assume you are running **Ubuntu 20.04 (Focal Fossa)**, although with small changes other releases - might also work. - -* It is strongly recommended that you have an **NVIDIA GPU and CUDA** installed. You can inspect your NVIDIA driver and - CUDA versions with the ``nvidia-smi`` command line utility. If you don't have it installed, follow the `NVIDIA CUDA - Installation Guide for Linux `_. - -.. _PX4 Autopilot: - -PX4 Autopilot -=================================================== -PX4 **v1.13** is currently supported by GISNav. Earlier versions might work, but the :class:`.MockGPSNode` demo requires -v1.13. - -Follow the PX4 instructions to setup your `Ubuntu Development Environment -`_ with `Fast DDS -`_. - -Once you have installed PX4 Autopilot, you can try out the Gazebo simulation to make sure everything is working -correctly. The following command should pop out a Gazebo window with a Typhoon H480 drone sitting somewhere in the -vicinity of San Carlos (KSQL) airport: - -.. code-block:: bash - - cd ~/PX4-Autopilot - make px4_sitl_rtps gazebo_typhoon_h480__ksql_airport - -.. note:: - The initial build may take several minutes. - -Mock GPS Node Demo Configuration -___________________________________________________ -If you are planning to use :class:`.MockGPSNode`, you should adjust the following PX4 parameters to make GISNav work -better either through the PX4 shell, through QGroundControl, or in the -``~/PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480`` file before making the build: - -.. warning:: - This configuration is intended for simulation use only - -.. code-block:: - - param set-default NAV_ACC_RAD 20.0 - param set-default MPC_YAWRAUTO_MAX 10.0 - - param set-default COM_POS_FS_DELAY 5 - - param set-default EKF2_GPS_P_NOISE 10 - param set-default EKF2_GPS_V_NOISE 3 - - param set-default SENS_GPS_MASK 2 - -.. note:: - This is a sample configuration that seems to work, but you may want to experiment with the parameters. - - It is important to make the waypoint turns softer and/or to reduce the yaw rate especially if the camera has some - pitch (is not completely nadir-facing) to ensure the field of view does not move or rotate* too quickly for GISNav. - Otherwise GISNav may lose track of position for long enough for the position delay failsafe to trigger before GISNav - can find the drone again. Increasing the position failsafe delay helps if your GPU is a bit slower or GISNav for some - reason cannot produce a position estimate for a number of subsequent frames. However as a failsafe parameter it - 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 - 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 - default)` - -.. _ROS 2 Workspace: - -ROS 2 Workspace -___________________________________________________ -GISNav requires ROS 2 to communicate with PX4 Autopilot and is therefore structured as a ROS 2 package. - -Follow the `PX4 instructions to setup ROS 2 and the PX4-ROS 2 bridge -`_. - -Once you have your ROS 2 workspace set up, consider automatically sourcing it in your ``~/.bashrc`` to avoid -manual repetition: - -.. code-block:: bash - - echo "source /opt/ros/foxy/setup.bash" >> ~/.bashrc - echo "source ~/px4_ros_com_ros2/install/setup.bash" >> ~/.bashrc - -.. note:: - The PX4 tutorial uses ``px4_ros_com_ros2`` for the workspace name, while the ``gisnav-docker`` container image uses - ``colcon_ws``. - -.. _PX4-ROS 2 Bridge: - -PX4-ROS 2 Bridge -___________________________________________________ -The default configuration of the PX4-ROS 2 bridge is not sufficient for GISNav. The bridge must be reconfigured and -the ``micrortps_agent`` re-generated. - -To reconfigure the bridge, see the `ROS 2 Offboard Control Example -`_ on how to edit the -``urtps_bridge_topics.yaml`` file in the ``PX4-Autopilot/msg/tools`` and ``px4_ros_com_ros2/src/px4_ros_com/templates`` -folders. You must configure the following send and receive flags for the following topics: - -.. list-table:: ``urtps_bridge_topics.yaml`` - :header-rows: 1 - - * - PX4-Autopilot/msg/tools - - px4_ros_com_ros2/src/px4_ros_com/templates - * - .. code-block:: yaml - - - msg: vehicle_local_position - send: true - ... - - msg: vehicle_global_position - send: true - ... - - msg: vehicle_attitude - send: true - ... - - msg: gimbal_device_set_attitude - send: true - ... - - msg: sensor_gps - receive: true - - .. code-block:: yaml - - - msg: VehicleLocalPosition - send: true - ... - - msg: VehicleGlobalPosition - send: true - ... - - msg: VehicleAttitude - send: true - ... - - msg: GimbalDeviceSetAttitude - send: true - ... - - msg: SensorGps - receive: true - -.. note:: - * The ``SensorGps`` topic is used by :class:`.MockGPSNode` and is optional if you are only using :class:`.BaseNode`. - Remember to add any other topics here that you might be using if you are extending :class:`.BaseNode`. - * The `Dockerfile for the SITL image - `_ uses the - `configure_urtps_bridge_topics.py - `_ - script to automatically configure the above topics before building the PX4 SITL target. - -PX4-ROS 2 Bridge Troubleshooting -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Ensure you have your new workspace sourced before moving on to next steps: - -.. code-block:: bash - - cd ~/px4_ros_com_ros2 - source /opt/ros/foxy/setup.bash - source install/setup.bash - -You can check whether your new configuration works by running ``micrortps_agent`` and inspecting the console output: - -.. code-block:: bash - - micrortps_agent -t UDP - -If your new topics are not listed, you can try cleaning both the ``px4_ros_com_ros2`` workspace and your PX4 build -before rebuilding again: - -.. code-block:: bash - :caption: Clean ROS 2 workspace - - cd ~/px4_ros_com_ros2/scripts - ./clean_all.bash - -.. code-block:: bash - :caption: Clean PX4 build - - cd ~/PX4-Autopilot - make clean - -.. note:: - *Unverified*: - When GISNav is running, it will try to exit cleanly when ``Ctrl+C`` is pressed. However, if the combination is - mashed quickly in succession the clean exit may fail and leave some subscriptions hanging. In this case you may - want to restart ``micrortps_agent``. - -gscam -___________________________________________________ - -The ``typhoon_h480`` build target for Gazebo SITL supports UDP `video streaming -`_ . Here we will use ``gscam`` to publish the -UDP video stream to ROS 2 to make it accessible to GISNav: - -Install ``gscam`` and dependencies: - -.. code-block:: bash - - sudo apt-get install -y gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl ros-foxy-gscam - -The GISNav repository includes a sample camera configuration that we will use. Run ``gscam`` in a dedicated bash shell -with the provided configuration files: - -.. code-block:: bash - - cd ~/px4_ros_com_ros2 - ros2 run gscam gscam_node --ros-args --params-file src/gisnav/test/assets/gscam_params.yaml \ - -p camera_info_url:=file://$PWD/src/gisnav/test/assets/camera_calibration.yaml - -.. seealso:: - See - `How to Calibrate a Monocular Camera `_ - on how to create a custom camera calibration file if you do not want to use the provided example - -gscam Troubleshooting -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -.. note:: - *Unverified*: - When GISNav is running, it will try to exit cleanly when ``Ctrl+C`` is pressed. However, if the combination is - mashed quickly in succession the clean exit may fail and leave some subscriptions hanging. In this case you may - want to restart ``gscam``. - -.. _ArduPilot: - -ArduPilot -=================================================== -ArduPilot is supported as an alternative to `PX4 Autopilot`_. The following tutorials should get you started with an -ArduPilot SITL simulation environment: - -* `Setting up SITL on Linux `_ -* `Using Gazebo simulator with SITL `_ -* `Connecting with ROS `_ - -As of ``gisnav`` v0.61, :class:`.MockGPSNode` can be used in the ArduPilot SITL simulation included in the -`gisnav-docker `_ image. The included ``gazebo-iris`` model only has a static -camera. Because the camera is not stabilized, it likely won't be reliable enough to act as a full replacement for GPS in -ArduPilot's mission mode, while loitering will work. Use the following command to start the ``mock_gps_node`` with the -ArduPilot bridge: - -.. code-block:: bash - - ros2 run gisnav mock_gps_node --mavros --ros-args --log-level info \ - --params-file src/gisnav/config/typhoon_h480__ksql_airport_ardupilot.yaml - - -.. note:: - You may have to enable virtual joystick from QGroundControl settings and have it centered to maintain altitude in - ArduPilot's Loiter mode in the SITL simulation. - -.. _QGroundControl: - -QGroundControl -=================================================== -QGroundControl is a PX4-compatible ground control station software with a graphical user interface. It is needed -for controlling the drone in the SITL (software-in-the-loop) simulation. - -Install QGroundControl by following the `official instructions -`_. - -You can then run QGroundControl from the directory where you installed it, for example: - -.. code-block:: bash - - ~/Applications/QGroundControl.AppImage - -QGroundControl Troubleshooting -___________________________________________________ - -You may need to change the file permissions and/or extract it before running it: - -.. code-block:: bash - :caption: Change file permissions - - cd ~/Applications - chmod +x QGroundControl.AppImage - ./QGroundControl.AppImage - -.. code-block:: bash - :caption: Extract and run - - cd ~/Applications - ./QGroundControl.AppImage --appimage-extract-and-run - -.. _`WMS endpoint`: - -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` 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: - -.. code-block:: yaml - :caption: Example YAML configuration of WMS ROS parameters - - my_node: - ros__parameters: - wms: - url: 'http://localhost:80/?map=/etc/mapserver/wms.map' - version: '1.1.1' - layers: ['Imagery'] - srs: 'EPSG:4326' # don't change this setting, internal logic may often implicitly assume EPSG:4326 - request_timeout: 10 - image_format: 'image/jpeg' - -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 `_ to set -up a WMS MapProxy using the provided Docker image. - -.. note:: - Commercial web-based map services are often - `tile-based `_ (as opposed to WMS) because it is more - efficient to serve pre-computed tiles than to compute unique rasters for each individual requested bounding box. - You will need a WMS proxy if you decide to go with a tile-based endpoint. - -.. warning:: - Many commercial services explicitly prohibit the caching of map tiles in their Terms of Use (ToU), especially if - their business model is based on billing API requests. This is mainly to prevent disintermediation in case their - tiles are redistributed to a large number of end users. - - While caching tiles onboard your own drone is likely not the kind of misuse targeted by such clauses, you should - still make sure you understand the ToU of the service you are using and that it fits your planned use case. - -Self-hosted WMS Server -___________________________________________________ -The benefit of a self-hosted WMS service is that you can embed it onboard the drone and not rely on an internet -connection. - -If you want to run your own WMS server, you may want to consider e.g. these options: - - * `MapServer `_ - - * `GeoServer `_ (full-fledged - `OGC-compliant `_ GIS server) - - * `Mapnik `_ and `MapProxy `_ - -If you do not want to use commercial (=not free) high-resolution imagery, various national agencies often provide -country-specific aerial imagery in the public domain or with public-domain-like licensing terms. You should look for -imagery available in `GDAL `_ supported formats with coverage for your area. - -.. note:: - You can even create your own maps for the flight area using the same drone and camera you are going to be - navigating with and host them on your own GIS server. - -MapServer with preloaded maps for :class:`.MockGPSNode` demo -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Here we provide an example on how to host your own maps using MapServer. If you are fine with using maps for the -:class:`.MockGPSNode` demo only, then you can simply use the `gisnav-docker -`_ repository. Otherwise see the instructions below. - -To follow these instructions you will need: - -* An AWS account and AWS CLI, **or alternatively**, an `EarthExplorer `_ account -* `GDAL `_ - -For the :class:`.MockGPSNode` demo, you can use `NAIP -`_ -imagery and the `MapServer docker image `_ from Docker Hub. You can -download the GeoTIFF imagery from EarthExplorer, or from the Esri-maintained `AWS S3 Requester Pays bucket -`_ if you already have AWS CLI set up: - -.. warning:: - This is a **Requester Pays** bucket and the files can be very large so download only what you need. - -.. code-block:: bash - :caption: Example: Downloading a NAIP imagery product from the AWS S3 bucket - - cd ~/gisnav-docker - 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 \ - mapfiles/ - -.. note:: - * The USDA FSA NAIP imagery is licensed under public domain with attribution requested. However, you must create an - EROS account to download the rasters from EarthExplorer, or use secondary sources such as the AWS S3 bucket - mentioned above. The data is not redistributed in the `gisnav-docker `_ - repository to keep its size manageable. - * You do not need an account to browse for product IDs with EarthExplorer. An account is needed if you want to - download products. - -Use GDAL to make a ``naip.vrt`` VRT file out of your downloaded GeoTIFFs: - -.. code-block:: bash - - cd mapfiles/ - gdalbuildvrt naip.vrt *.tif - -Once you have your .tif and .vrt files, you can run a ``mapserver`` container: - -.. code-block:: bash - - cd ~/gisnav-docker - export CONTAINER_NAME=gisnav-mapserver - export MAPSERVER_PATH=/etc/mapserver - docker run \ - --name $CONTAINER_NAME \ - -p 80:80 \ - -v $PWD/mapfiles/:$MASERVER_PATH/:ro \ - camptocamp/mapserver - -Test your MapServer WMS service by opening the capabilities XML in your browser: - -.. code-block:: bash - - firefox "http://localhost:80/?map=/etc/mapserver/wms.map&service=WMS&request=GetCapabilities" - -GISNav -=================================================== - -Install GISNav in your `ROS 2 Workspace`_: - -.. code-block:: bash: - - cd ~/px4_ros_com_ros2 - mkdir -p src && cd "$_" - git clone https://github.com/hmakelin/gisnav.git - cd gisnav - pip3 install -r requirements.txt - pip3 install -r requirements-dev.txt - -Download the LoFTR submodule and weights: - -.. code-block:: bash - - cd ~/px4_ros_com_ros2/src/gisnav - git submodule update LoFTR - pip3 install gdown - mkdir weights && cd "$_" - gdown https://drive.google.com/uc?id=1M-VD35-qdB5Iw-AtbDBCKC7hPolFW9UY - -.. note:: - The example downloads the dual-softmax (_ds suffix) outdoor weights which are permissively licensed (does not use - SuperGlue) - -Build the GISNav package: - -.. code-block:: bash: - - cd ~/px4_ros_com_ros2 - colcon build --packages-select gisnav - -Once GISNav is installed, you can run the included :class:`.MockGPSNode` either directly with ``ros2 run``: - -.. code-block:: bash - :caption: Run GISNav with PX4 microRTPS bridge - - cd ~/px4_ros_com_ros2 - ros2 run gisnav mock_gps_node --ros-args --log-level info \ - --params-file src/gisnav/config/typhoon_h480__ksql_airport.yaml - -.. code-block:: bash - :caption: Run GISNav with ArduPilot MAVROS - - cd ~/px4_ros_com_ros2 - ros2 run gisnav mock_gps_node --mavros --ros-args --log-level info \ - --params-file src/gisnav/config/typhoon_h480__ksql_airport_ardupilot.yaml - -Or using the provided launch file: - -.. code-block:: bash - - cd ~/px4_ros_com_ros2 - ros2 launch gisnav mock_gps_node.launch.py - diff --git a/gisnav/__init__.py b/gisnav/__init__.py index e69de29b..7cfcae11 100644 --- a/gisnav/__init__.py +++ b/gisnav/__init__.py @@ -0,0 +1 @@ +"""ROS 2 package for estimating airborne drone global position by matching video to map retrieved from onboard GIS server.""" \ No newline at end of file diff --git a/gisnav/__main__.py b/gisnav/__main__.py deleted file mode 100644 index 21847a63..00000000 --- a/gisnav/__main__.py +++ /dev/null @@ -1,52 +0,0 @@ -import io -import cProfile -import pstats -import rclpy -import sys - -from ament_index_python.packages import get_package_share_directory - -from gisnav.nodes.mock_gps_node import MockGPSNode - - -def main(args=None): - """Starts and terminates the ROS 2 node. - - Also starts cProfile profiling in debugging mode. - - :param args: Any args for initializing the rclpy node - :return: - """ - package_name = __name__.rsplit('.')[-2] # should match package.xml declared package_name - if __debug__: - pr = cProfile.Profile() - pr.enable() - else: - pr = None - - mock_gps_node = None - try: - rclpy.init(args=args) - mock_gps_node = MockGPSNode('mock_gps_node', get_package_share_directory(package_name), - px4_micrortps='--mavros' not in sys.argv) - rclpy.spin(mock_gps_node) - except KeyboardInterrupt as e: - print(f'Keyboard interrupt received:\n{e}') - if pr is not None: - # Print out profiling stats - pr.disable() - s = io.StringIO() - ps = pstats.Stats(pr, stream=s).sort_stats(pstats.SortKey.CUMULATIVE) - ps.print_stats(40) - print(s.getvalue()) - finally: - if mock_gps_node is not None: - mock_gps_node.destroy_timers() - mock_gps_node.unsubscribe_topics() - mock_gps_node.terminate_pools() - mock_gps_node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/gisnav/assertions.py b/gisnav/assertions.py index fee2225c..c68967db 100644 --- a/gisnav/assertions.py +++ b/gisnav/assertions.py @@ -1,5 +1,5 @@ """Common assertions for convenience""" -from typing import Optional, Any, Union, Sequence, Collection, Tuple, get_args +from typing import Any, Union, Sequence, Collection, Tuple import numpy as np @@ -9,7 +9,6 @@ def assert_type(value: object, type_: Any) -> None: :param value: Object to check :param type_: Type to be asserted - :return: """ assert isinstance(value, type_), f'Type {type(value)} provided when {type_} was expected.' @@ -19,43 +18,39 @@ def assert_ndim(value: np.ndarray, ndim: int) -> None: :param value: Numpy array to check :param ndim: Required number of dimensions - :return: """ assert value.ndim == ndim, f'Unexpected number of dimensions: {value.ndim} ({ndim} expected).' -def assert_len(value: Union[Sequence, Collection], len_: int): +def assert_len(value: Union[Sequence, Collection], len_: int) -> None: """Asserts a specific length for a sequence or a collection (e.g. a list). :param value: Sequence or collection to check :param len_: Required length - :return: """ assert len(value) == len_, f'Unexpected length: {len(value)} ({len_} expected).' -def assert_shape(value: np.ndarray, shape: tuple): +def assert_shape(value: np.ndarray, shape: tuple) -> None: """Asserts a specific shape for np.ndarray. :param value: Numpy array to check :param shape: Required shape - :return: """ assert value.shape == shape, f'Unexpected shape: {value.shape} ({shape} expected).' -def assert_pose(pose: Tuple[np.ndarray, np.ndarray]): +def assert_pose(pose: Tuple[np.ndarray, np.ndarray]) -> None: """Asserts that provided tuple is a valid pose (r, t) :param pose: Tuple consisting of a rotation (3, 3) and translation (3, 1) numpy arrays with valid values - :return: True if pose tuple is correctly formatted and valid """ r, t = pose assert_rotation_matrix(r) assert_shape(t, (3, 1)) -def assert_rotation_matrix(r: np.ndarray): +def assert_rotation_matrix(r: np.ndarray) -> None: """Asserts that matrix is a valid rotation matrix Provided matrix of shape (3, 3) with valid values diff --git a/gisnav/autopilots/ardupilot_mavros.py b/gisnav/autopilots/ardupilot_mavros.py deleted file mode 100644 index 05c27f08..00000000 --- a/gisnav/autopilots/ardupilot_mavros.py +++ /dev/null @@ -1,356 +0,0 @@ -"""Adapter for ArduPilot MAVROS bridge""" -import rclpy -import time -import numpy as np -from typing import Optional, Callable, get_args - -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, HomePosition - -from gisnav.assertions import assert_type -from gisnav.data import TimePair, Attitude, Position, Altitude -from gisnav.geo import GeoPoint - -from gisnav.autopilots.autopilot import Autopilot - - -class ArduPilotMAVROS(Autopilot): - """ArduPilot MAVROS bridge adapter - - .. seealso:: - `MAVROS topic name to message definition mapping `_ - """ - - ROS_TOPICS = { - '/mavros/local_position/pose': { - Autopilot._TOPICS_MSG_KEY: PoseStamped, - Autopilot._TOPICS_QOS_KEY: rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value - }, - '/mavros/global_position/global': { - 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 - }, - } - """ROS topics to subscribe""" - - def __init__(self, node: rclpy.node.Node, image_callback: Callable[[Image], None]) -> None: - """Initialize class - - :param node: Parent node that handles the ROS subscriptions - :param image_callback: Callback function for camera image - """ - super().__init__(node, image_callback) - - self._local_position_pose = None - self._global_position_global = None - self._mount_control_command = None - self._home_position_home = None - - self._time_sync = None - - self._setup_subscribers() - - # region Properties - @property - def _local_position_pose(self) -> Optional[PoseStamped]: - """Local position pose received via MAVROS.""" - return self.__local_position_pose - - @_local_position_pose.setter - def _local_position_pose(self, value: Optional[PoseStamped]) -> None: - assert_type(value, get_args(Optional[PoseStamped])) - self.__local_position_pose = value - - @property - def _global_position_global(self) -> Optional[NavSatFix]: - """Global positiion received via MAVROS.""" - return self.__global_position_global - - @_global_position_global.setter - 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.""" - return self.__mount_control_command - - @_mount_control_command.setter - def _mount_control_command(self, value: Optional[MountControl]) -> None: - assert_type(value, get_args(Optional[MountControl])) - self.__mount_control_command = value - - @property - def _time_sync(self) -> Optional[TimePair]: - """A :class:`gisnav.data.TimePair` with local and foreign (EKF2) timestamps in microseconds - - The pair will contain the local system time and the foreign EKF2 time received from autopilot via ROS. The pair - can then at any time be used to locally estimate the EKF2 system time. - """ - return self.__time_sync - - @_time_sync.setter - def _time_sync(self, value: Optional[TimePair]) -> None: - assert_type(value, get_args(Optional[TimePair])) - self.__time_sync = value - # endregion - - # region Private - def _sync_timestamps(self, ekf2_timestamp_usec: int) -> None: - """Synchronizes local timestamp with PX4 EKF2's reference time - - This synchronization is triggered in the :meth:`._local_position_pose` and therefore expected to be done at - high frequency. - - .. seealso: - :py:attr:`._time_sync` - - :param ekf2_timestamp_usec: Time since foreign system start in microseconds - """ - assert_type(ekf2_timestamp_usec, int) - now_usec = time.time() * 1e6 - self._time_sync = TimePair(now_usec, ekf2_timestamp_usec) - - def _setup_subscribers(self) -> None: - """Creates and stores subscribers ROS topics""" - for topic_name, d in self.ROS_TOPICS.items(): - class_ = d.get(Autopilot._TOPICS_MSG_KEY, None) - assert class_ is not None, f'Message definition not provided for {topic_name}.' - qos = d.get(Autopilot._TOPICS_QOS_KEY, rclpy.qos.QoSPresetProfiles.SYSTEM_DEFAULT.value) - callback_name = f'_{"_".join(topic_name.split("/")[-2:]).lower()}_callback' # TODO make topic name parsing less brittle - callback = getattr(self, callback_name, None) - assert callback is not None, f'Missing callback implementation for {callback_name}.' - self._subscribe(topic_name, class_, callback, qos) - - def _local_position_pose_callback(self, msg: PoseStamped) -> None: - """Handles latest :class:`geometry_msgs.msg.PoseStamped` message - - Uses the EKF2 system time in the message to synchronize local system time. - - :param msg: :class:`class:`geometry_msgs.msg.PoseStamped` message via MAVROS - :return: - """ - assert hasattr(msg, 'header') and hasattr(msg.header, 'stamp') and hasattr(msg.header.stamp, 'sec') and \ - hasattr(msg.header.stamp, 'nanosec') - self._local_position_pose = msg - usec = msg.header.stamp.sec + int(msg.header.stamp.nanosec / 1e3) - self._sync_timestamps(usec) - - def _global_position_global_callback(self, msg: NavSatFix) -> None: - """Handles latest :class:`sensor_msgs.msg.NavSatFix` message - - :param msg: :class:`sensor_msgs.msg.NavSatFix` message via MAVROS - :return: - """ - self._global_position_global = msg - - def _mount_control_command_callback(self, msg: MountControl) -> None: - """Handles latest :class:`mavros_msgs.msg.MountControl` message - - :param msg: :class:`mavros_msgs.msg.MountControl` via MAVROS - :return: - """ - 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` - - .. note:: - Assumes message is expired if its age cannot be determined - - :return: True if the message is too old - """ - if not hasattr(message, 'timestamp') or self.synchronized_time is None or \ - self.synchronized_time > message.timestamp + self.TELEMETRY_EXPIRATION_LIMIT: - return True - else: - return False - # endregion - - # region Public - @property - def synchronized_time(self) -> Optional[int]: - """Estimated autopilot EKF2 system reference time in microseconds or None if not available - - .. seealso: - :py:attr:`._time_sync` and :meth:`._sync_timestamps` - - :return: Estimated autpilot timestamp in usec or None if not available - """ - if self._time_sync is None: - return None - else: - now_usec = time.time() * 1e6 - assert now_usec > self._time_sync.local,\ - f'Current timestamp {now_usec} was unexpectedly smaller than timestamp stored earlier for ' \ - f'synchronization {self._time_sync.local}.' - ekf2_timestamp_usec = int(self._time_sync.foreign + (now_usec - self._time_sync.local)) - return ekf2_timestamp_usec - - @property - def attitude(self) -> Optional[Attitude]: - """Vehicle attitude in NED frame as an :class:`.Attitude` instance - - .. note:: - MAVROS local position plugin returns orientation in ENU frame, it is converted to NED here - """ - if self._local_position_pose is not None \ - and hasattr(self._local_position_pose, 'pose') \ - and hasattr(self._local_position_pose.pose, 'orientation') \ - and hasattr(self._local_position_pose.pose.orientation, 'x') \ - and hasattr(self._local_position_pose.pose.orientation, 'y') \ - and hasattr(self._local_position_pose.pose.orientation, 'z') \ - and hasattr(self._local_position_pose.pose.orientation, 'w'): - - attitude_ned_q = np.array([self._local_position_pose.pose.orientation.y, - self._local_position_pose.pose.orientation.x, - -self._local_position_pose.pose.orientation.z, - self._local_position_pose.pose.orientation.w]) - - # Convert ENU->NED + re-center yaw - attitude_ned = Rotation.from_quat(attitude_ned_q) * Rotation.from_rotvec(np.array([0, 0, np.pi / 2])) - - return Attitude(attitude_ned.as_quat(), extrinsic=True) - else: - return None - - @property - def gimbal_attitude(self) -> Optional[np.ndarray]: - """Gimbal attitude as an :class:`.Attitude` instance or None if not available""" - # TODO - return None - - @property - def gimbal_set_attitude(self) -> Optional[np.ndarray]: - """Gimbal set attitude in NED frame as an :class:`.Attitude` instance or None if not available - - .. note:: - * Assumes 2-axis (roll & pitch/tilt stabilized) gimbal - * Gimbal actual attitude may not match the set attitude if gimbal has not stabilized itself - """ - return None - - @property - def altitude_amsl(self) -> Optional[float]: - """Vehicle altitude in meters above mean sea level (AMSL) or None if not available""" - 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 - self._egm96.height(self.global_position.lat, - self.global_position.lon) - else: - return None - - @property - def altitude_ellipsoid(self) -> Optional[float]: - """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 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 - - @property - def altitude_home(self) -> Optional[float]: - """Vehicle altitude in meters above home (starting) position or None if not available - - .. seealso:: - `Altitude definitions `_ - """ - 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 - - @property - 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 `_ - """ - 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 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 diff --git a/gisnav/autopilots/autopilot.py b/gisnav/autopilots/autopilot.py deleted file mode 100644 index f3357a52..00000000 --- a/gisnav/autopilots/autopilot.py +++ /dev/null @@ -1,312 +0,0 @@ -"""Module with abstract base class for autopilot (PX4, Ardupilot etc.) adapters""" -import numpy as np -import rclpy - -from abc import ABC, abstractmethod -from typing import Optional, Callable, get_args - -from pygeodesy.geoids import GeoidPGM - -from sensor_msgs.msg import Image, CameraInfo - -from gisnav.data import Attitude, CameraData, ImageData, Dim, Altitude, Snapshot, Position -from gisnav.geo import GeoPoint -from gisnav.assertions import assert_type - - -class Autopilot(ABC): - """Abstract base class definining autopilot bridge interface - - .. note:: - * Different altitudes against WGS 84 ellipsoid altitudes are defined in this parent class and based on a - correction to the corresponding AMSL altitude - """ - - # Keys for topics dictionary that map microRTPS bridge topics to subscribers and message definitions - _TOPICS_MSG_KEY = 'message' - _TOPICS_SUBSCRIBER_KEY = 'subscriber' - _TOPICS_QOS_KEY = 'qos' - - # TODO: use this for all properties (make wrapper) - TELEMETRY_EXPIRATION_LIMIT = 1e6 - """Expiration period in usec for vehicle state (if telemetry data is older than this it should not be used)""" - - CAMERA_INFO_TOPIC = 'camera/camera_info' - """ROS camera info (:class:`.sensor_msgs.msg.CameraInfo`) topic to subscribe to""" - - IMAGE_RAW_TOPIC = 'camera/image_raw' - """ROS image raw (:class:`.sensor_msgs.msg.Image`) topic to subscribe to""" - - def __init__(self, node: rclpy.node.Node, on_image_callback: Callable[[ImageData], None]) -> None: - """Initializes autopilot adapter - - :param node: Parent node that handles the ROS subscriptions - :param on_image_callback: Callback function for camera image - """ - self._node = node - self._topics = {} - - # Subscribe to camera data topic - self.camera_data = None - self._subscribe(self.CAMERA_INFO_TOPIC, CameraInfo, self._camera_info_callback, - rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value) - - # Image topic callback gets access to base node through provided on_image_callback - self._subscribe(self.IMAGE_RAW_TOPIC, Image, on_image_callback, rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value) - - # TODO: make configurable / use shared folder home path instead - self._egm96 = GeoidPGM('/usr/share/GeographicLib/geoids/egm96-5.pgm', kind=-3) - - @property - def _node(self) -> rclpy.node.Node: - """Parent :class:`.BaseNode` that handles the subscriptions (and holds a handle to this adapter)""" - return self.__node - - @_node.setter - def _node(self, value: rclpy.node.Node) -> None: - assert_type(value, rclpy.node.Node) - self.__node = value - - @property - def _topics(self) -> dict: - """Dictionary that stores all rclpy subscribers.""" - return self.__topics - - @_topics.setter - def _topics(self, value: dict) -> None: - assert_type(value, dict) - self.__topics = value - - @property - def _egm96(self) -> GeoidPGM: - """Geoid for getting ellipsoid altitudes from alt (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 - `_ - """ - return self.__egm96 - - @_egm96.setter - def _egm96(self, value: GeoidPGM) -> None: - assert_type(value, GeoidPGM) - self.__egm96 = value - - @property - def camera_data(self) -> Optional[CameraData]: - """CameraInfo received via the PX4-ROS 2 bridge as a :class:`.CameraData` instance.""" - return self.__camera_data - - @camera_data.setter - def camera_data(self, value: Optional[CameraData]) -> None: - assert_type(value, get_args(Optional[CameraData])) - self.__camera_data = value - - def _subscribe(self, topic_name: str, class_: type, callback: Callable, qos: rclpy.qos.QoSProfile) -> None: - """Subscribes to ROS topic - - :param topic_name: Name of the microRTPS topic - :param class_: Message definition class type (e.g. px4_msgs.msg.VehicleLocalPosition) - :param callback: Callback function for the topic - :param qos: Subscription quality of service profile - :return: The subscriber instance - """ - self._topics.update({topic_name: {self._TOPICS_SUBSCRIBER_KEY: - self._node.create_subscription(class_, topic_name, callback, qos)}}) - - def _unsubscribe(self, topic_name: str): - """Unsubscribes from ROS topic - - :param topic_name: Name of ROS topic to unsubscribe - """ - for topic_name, v in self._topics.items(): - subscriber = v.get(topic_name, {}).get(self._TOPICS_SUBSCRIBER_KEY, None) - if subscriber is not None: - subscriber.destroy() - - def unsubscribe_all(self): - """Destroys all subscribers""" - for k, _ in self._topics.items(): - self._unsubscribe(k) - - def _camera_info_callback(self, msg: CameraInfo) -> None: - """Handles latest :class:`px4_msgs.msg.CameraInfo` message - - .. note:: - Checks that intrisic matrix and height/width information is received and then destroys subscription ( - assumes camera info is static) - - :param msg: :class:`px4_msgs.msg.CameraInfo` message from the PX4-ROS 2 bridge - """ - if not all(hasattr(msg, attr) for attr in ['k', 'height', 'width']): - # TODO: check that k and height/width match - return None - else: - self.camera_data = CameraData(k=msg.k.reshape((3, 3)), dim=Dim(msg.height, msg.width)) - camera_info_sub = self._topics.get(self.CAMERA_INFO_TOPIC, {}).get(self._TOPICS_SUBSCRIBER_KEY, None) - if camera_info_sub is not None: - # Assume camera info is static, destroy subscription - camera_info_sub.destroy() - - @property - @abstractmethod - def synchronized_time(self) -> Optional[None]: - """Estimated foreign (autopilot) timestamp in microseconds or None if not available""" - pass - - @property - @abstractmethod - def attitude(self) -> Optional[Attitude]: - """Vehicle attitude as an :class:`.Attitude` instance or None if not available""" - pass - - @property - @abstractmethod - def altitude_home(self) -> Optional[float]: - """Vehicle altitude in meters above home (starting) position or None if not available - - .. seealso:: - `Altitude definitions `_ - """ - pass - - def altitude_agl(self, terrain_altitude_amsl: Optional[float]) -> Optional[float]: - """Vehicle altitude in meters above ground level (AGL) or None if not available - - .. seealso:: - `Altitude definitions ` - - :param terrain_altitude_amsl: Terrain altitude AMSL at current location according to DEM (optional) - :return: Vehicle altitude in meters above ground level (AGL) - """ - if self.altitude_amsl is not None: - if terrain_altitude_amsl is not None: - return self.altitude_amsl - terrain_altitude_amsl - elif self.home_altitude_amsl is not None: - # TODO: assumes home is at ground level and that ground is flat --> not strictly true - return self.altitude_amsl - self.home_altitude_amsl - else: - return None - else: - return None - - @property - @abstractmethod - def altitude_amsl(self) -> Optional[float]: - """Vehicle altitude in meters above mean sea level (AMSL) or None if not available - - .. seealso:: - `Altitude definitions `_ - """ - pass - - @property - def altitude_ellipsoid(self) -> Optional[float]: - """Vehicle altitude in meters above WGS 84 ellipsoid or None if not available - - .. note:: - Looks like at least PX4 SITL :class:`VehicleGlobalPosition` message has an invalid alt_ellipsoid value: - ``alt_ellipsoid: -1844674428928.0``. Ellipsoid altitude is therefore calculated from altitude AMSL using - egm96 geoid instead, which should apply regardless of what autopilot is used. - - .. seealso:: - `Altitude definitions `_ - """ - 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 - - @property - @abstractmethod - def home_altitude_amsl(self) -> Optional[float]: - """Home (starting position) altitude in meters above mean sea level (AMSL) or None if not available - - .. seealso:: - `Altitude definitions `_ - """ - pass - - @property - def home_altitude_ellipsoid(self) -> Optional[float]: - """Home (starting position) altitude in meters above WGS 84 ellipsoid or None if not available - - .. seealso:: - `Altitude definitions `_ - """ - if self.home_altitude_amsl is not None and self.global_position is not None: - return self.home_altitude_amsl + self._egm96.height(self.global_position.lat, self.global_position.lon) - else: - return None - - @property - @abstractmethod - def gimbal_attitude(self) -> Optional[np.ndarray]: - """Gimbal attitude quaternion in (x, y, z, w) format in NED frame or None if not available - - .. note:: - This is the same format that for example SciPy uses, while e.g. the PX4-ROS 2 bridge uses (w, x, y, z) - """ - pass - - @property - @abstractmethod - def gimbal_set_attitude(self) -> Optional[np.ndarray]: - """Gimbal set attitude quaternion in (x, y, z, w) format in NED frame or None if not available - - .. note:: - * This is the same format that for example SciPy uses, while e.g. the PX4-ROS 2 bridge uses (w, x, y, z) - * Gimbal actual attitude may not match the set attitude if e.g. gimbal has not yet stabilized itself - """ - pass - - @property - @abstractmethod - def global_position(self) -> Optional[GeoPoint]: - """Vehicle global position as a :class:`.GeoPoint`""" - pass - - @property - @abstractmethod - def local_frame_origin(self) -> Optional[Position]: - """Vehicle local frame origin as a :class:`.Position`""" - pass - - @property - def snapshot(self) -> Snapshot: - """Snapshot of all other properties for convenience""" - global_position = self.global_position - terrain_altitude_amsl = self._node._terrain_altitude_amsl_at_position(global_position) - home_altitude_amsl = self.home_altitude_amsl - return Snapshot( - synchronized_time=self.synchronized_time, - attitude=self.attitude, - gimbal_attitude=self.gimbal_attitude, - gimbal_set_attitude=self.gimbal_set_attitude, - global_position=global_position, - local_frame_origin=self.local_frame_origin, - altitude=Altitude( - amsl=self.altitude_amsl, - agl=self.altitude_agl, - ellipsoid=self.altitude_ellipsoid, - home=self.altitude_home - ), - terrain_altitude=Altitude( - amsl=terrain_altitude_amsl, - agl=0, - ellipsoid=terrain_altitude_amsl + self._egm96.height(global_position.lat, global_position.lon) if terrain_altitude_amsl is not None else None, - home=terrain_altitude_amsl - home_altitude_amsl if terrain_altitude_amsl is not None and home_altitude_amsl is not None else None, - ), - home_altitude=Altitude( - amsl=home_altitude_amsl, - agl=home_altitude_amsl - terrain_altitude_amsl if terrain_altitude_amsl is not None else None, - ellipsoid=self.home_altitude_ellipsoid, - home=0 - ), - ) \ No newline at end of file diff --git a/gisnav/autopilots/px4_micrortps.py b/gisnav/autopilots/px4_micrortps.py deleted file mode 100644 index 52d89b88..00000000 --- a/gisnav/autopilots/px4_micrortps.py +++ /dev/null @@ -1,330 +0,0 @@ -"""Adapter for PX4-ROS 2 bridge""" -import rclpy -import time -import numpy as np -from typing import Optional, Tuple, Callable, get_args - -from scipy.spatial.transform import Rotation -from sensor_msgs.msg import CameraInfo, Image -from px4_msgs.msg import VehicleAttitude, VehicleLocalPosition, VehicleGlobalPosition, GimbalDeviceAttitudeStatus, \ - GimbalDeviceSetAttitude - -from gisnav.assertions import assert_type -from gisnav.data import TimePair, Attitude, Altitude, Position -from gisnav.geo import GeoPoint - -from gisnav.autopilots.autopilot import Autopilot - - -class PX4microRTPS(Autopilot): - """PX4-ROS 2 microRTPS bridge adapter""" - - ROS_TOPICS = { - '/fmu/vehicle_attitude/out': { - Autopilot._TOPICS_MSG_KEY: VehicleAttitude, - Autopilot._TOPICS_QOS_KEY: rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value - }, - '/fmu/vehicle_local_position/out': { - Autopilot._TOPICS_MSG_KEY: VehicleLocalPosition, - Autopilot._TOPICS_QOS_KEY: rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value - }, - '/fmu/vehicle_global_position/out': { - Autopilot._TOPICS_MSG_KEY: VehicleGlobalPosition, - Autopilot._TOPICS_QOS_KEY: rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value - }, - '/fmu/gimbal_device_set_attitude/out': { - Autopilot._TOPICS_MSG_KEY: GimbalDeviceSetAttitude, - Autopilot._TOPICS_QOS_KEY: rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value - } - } - """ROS topics to subscribe""" - - def __init__(self, node: rclpy.node.Node, image_callback: Callable[[Image], None]) -> None: - """Initialize class - - :param node: Parent node that handles the ROS subscriptions - :param image_callback: Callback function for camera image - """ - super().__init__(node, image_callback) - - self._setup_subscribers() - - self._vehicle_local_position = None - self._vehicle_global_position = None - self._vehicle_attitude = None - self._gimbal_device_set_attitude = None - self._gimbal_attitude = None - self._time_sync = None - - # region Properties - @property - def _vehicle_local_position(self) -> Optional[VehicleLocalPosition]: - """VehicleLocalPosition received via the PX4-ROS 2 bridge.""" - return self.__vehicle_local_position - - @_vehicle_local_position.setter - def _vehicle_local_position(self, value: Optional[VehicleLocalPosition]) -> None: - assert_type(value, get_args(Optional[VehicleLocalPosition])) - self.__vehicle_local_position = value - - @property - def _vehicle_attitude(self) -> Optional[VehicleAttitude]: - """VehicleAttitude received via the PX4-ROS 2 bridge.""" - return self.__vehicle_attitude - - @_vehicle_attitude.setter - def _vehicle_attitude(self, value: Optional[VehicleAttitude]) -> None: - assert_type(value, get_args(Optional[VehicleAttitude])) - self.__vehicle_attitude = value - - @property - def _vehicle_global_position(self) -> Optional[VehicleGlobalPosition]: - """VehicleGlobalPosition received via the PX4-ROS 2 bridge.""" - return self.__vehicle_global_position - - @_vehicle_global_position.setter - def _vehicle_global_position(self, value: Optional[VehicleGlobalPosition]) -> None: - assert_type(value, get_args(Optional[VehicleGlobalPosition])) - self.__vehicle_global_position = value - - @property - def _gimbal_device_set_attitude(self) -> Optional[GimbalDeviceSetAttitude]: - """GimbalDeviceSetAttitude received via the PX4-ROS 2 bridge.""" - return self.__gimbal_device_set_attitude - - @_gimbal_device_set_attitude.setter - def _gimbal_device_set_attitude(self, value: Optional[GimbalDeviceSetAttitude]) -> None: - assert_type(value, get_args(Optional[GimbalDeviceSetAttitude])) - self.__gimbal_device_set_attitude = value - - @property - def _time_sync(self) -> Optional[TimePair]: - """A :class:`gisnav.data.TimePair` with local and foreign (EKF2) timestamps in microseconds - - The pair will contain the local system time and the foreign EKF2 time received from autopilot via ROS. The pair - can then at any time be used to locally estimate the EKF2 system time. - """ - return self.__time_sync - - @_time_sync.setter - def _time_sync(self, value: Optional[TimePair]) -> None: - assert_type(value, get_args(Optional[TimePair])) - self.__time_sync = value - # endregion - - # region Private - def _sync_timestamps(self, ekf2_timestamp_usec: int) -> None: - """Synchronizes local timestamp with PX4 EKF2's reference time - - This synchronization is triggered in the :meth:`._vehicle_local_position_callback` and therefore expected to be - done at high frequency. - - .. seealso: - :py:attr:`._time_sync` - - :param ekf2_timestamp_usec: Time since PX4 EKF2 system start in microseconds - """ - assert_type(ekf2_timestamp_usec, int) - now_usec = time.time() * 1e6 - self._time_sync = TimePair(now_usec, ekf2_timestamp_usec) - - def _setup_subscribers(self) -> None: - """Creates and stores subscribers for microRTPS bridge topics""" - for topic_name, d in self.ROS_TOPICS.items(): - class_ = d.get(Autopilot._TOPICS_MSG_KEY, None) - assert class_ is not None, f'Message definition not provided for {topic_name}.' - qos = d.get(Autopilot._TOPICS_QOS_KEY, rclpy.qos.QoSPresetProfiles.SYSTEM_DEFAULT.value) - callback_name = f'_{topic_name.split("/")[2].lower()}_callback' # TODO make topic name parsing less brittle - callback = getattr(self, callback_name, None) - assert callback is not None, f'Missing callback implementation for {callback_name}.' - self._subscribe(topic_name, class_, callback, qos) - - def _vehicle_local_position_callback(self, msg: VehicleLocalPosition) -> None: - """Handles latest :class:`px4_msgs.msg.VehicleLocalPosition` message - - Uses the EKF2 system time in the message to synchronize local system time. - - :param msg: :class:`px4_msgs.msg.VehicleLocalPosition` message from the PX4-ROS 2 bridge - :return: - """ - assert_type(msg.timestamp, int) - self._vehicle_local_position = msg - self._sync_timestamps(self._vehicle_local_position.timestamp) - - def _vehicle_attitude_callback(self, msg: VehicleAttitude) -> None: - """Handles latest :class:`px4_msgs.msg.VehicleAttitude` message - - :param msg: :class:`px4_msgs.msg.VehicleAttitude` message from the PX4-ROS 2 bridge - :return: - """ - self._vehicle_attitude = msg - - def _vehicle_global_position_callback(self, msg: VehicleGlobalPosition) -> None: - """Handles latest :class:`px4_msgs.msg.VehicleGlobalPosition` message - - :param msg: :class:`px4_msgs.msg.VehicleGlobalPosition` message from the PX4-ROS 2 bridge - :return: - """ - self._vehicle_global_position = msg - - def _gimbal_device_set_attitude_callback(self, msg: GimbalDeviceSetAttitude) -> None: - """Handles latest :class:`px4_msgs.msg.GimbalDeviceSetAttitude` message - - :param msg: :class:`px4_msgs.msg.GimbalDeviceSetAttitude` message from the PX4-ROS 2 bridge - :return: - """ - self._gimbal_device_set_attitude = msg - - def _is_expired(self, message) -> bool: - """Returns True if the given message is older than :py:attr:`._TELEMETRY_EXPIRATION_LIMIT` - - .. note:: - Assumes message is expired if its age cannot be determined - - :return: True if the message is too old - """ - if not hasattr(message, 'timestamp') or self.synchronized_time is None or \ - self.synchronized_time > message.timestamp + self.TELEMETRY_EXPIRATION_LIMIT: - return True - else: - return False - # endregion - - # region Public - @property - def synchronized_time(self) -> Optional[int]: - """Estimated autopilot EKF2 system reference time in microseconds or None if not available - - .. seealso: - :py:attr:`._time_sync` and :meth:`._sync_timestamps` - - :return: Estimated autpilot timestamp in usec or None if not available - """ - if self._time_sync is None: - return None - else: - now_usec = time.time() * 1e6 - assert now_usec > self._time_sync.local,\ - f'Current timestamp {now_usec} was unexpectedly smaller than timestamp stored earlier for ' \ - f'synchronization {self._time_sync.local}.' - ekf2_timestamp_usec = int(self._time_sync.foreign + (now_usec - self._time_sync.local)) - return ekf2_timestamp_usec - - @property - def attitude(self) -> Optional[Attitude]: - """Vehicle attitude in NED frame as an :class:`.Attitude` instance""" - if self._vehicle_attitude is not None and hasattr(self.__vehicle_attitude, 'q'): - return Attitude(np.append(self._vehicle_attitude.q[1:4], self._vehicle_attitude.q[0]), extrinsic=True) - else: - return None - - @property - def gimbal_attitude(self) -> Optional[np.ndarray]: - """Gimbal attitude as an :class:`.Attitude` instance or None if not available""" - return None - - @property - def gimbal_set_attitude(self) -> Optional[np.ndarray]: - """Gimbal set attitude in NED frame as an :class:`.Attitude` instance or None if not available - - .. note:: - * Assumes 2-axis (roll & pitch/tilt stabilized) gimbal - * Gimbal actual attitude may not match the set attitude if gimbal has not stabilized itself - """ - if self._vehicle_attitude is None or self._gimbal_device_set_attitude is None: - return None - - yaw_mask = np.array([1, 0, 0, 1]) # Gimbal roll & pitch/tilt is assumed stabilized so only need yaw/pan - vehicle_yaw = self._vehicle_attitude.q * yaw_mask - - gimbal_set_attitude_frd = self._gimbal_device_set_attitude.q - - # SciPy expects (x, y, z, w) while PX4 messages are (w, x, y, z) - vehicle_yaw = Rotation.from_quat(np.append(vehicle_yaw[1:], vehicle_yaw[0])) - gimbal_set_attitude_frd = Rotation.from_quat(np.append(gimbal_set_attitude_frd[1:], gimbal_set_attitude_frd[0])) - - gimbal_set_attitude_ned = vehicle_yaw * gimbal_set_attitude_frd - - return Attitude(gimbal_set_attitude_ned.as_quat(), extrinsic=True) - - @property - def altitude_home(self) -> Optional[float]: - """Vehicle altitude in meters above home (starting) position or None if not available - - .. seealso:: - `Altitude definitions `_ - """ - # Check for existence of z attribute even if z_valid is True, just in case - if self._vehicle_local_position is not None \ - and hasattr(self._vehicle_local_position, 'z_valid') \ - and hasattr(self._vehicle_local_position, 'z') \ - and self._vehicle_local_position.z_valid: - # TODO Harmonized conventions: In this case assumption has been that up is positive altitude --> '-' - return -self._vehicle_local_position.z - - @property - def altitude_amsl(self) -> Optional[float]: - """Vehicle altitude in meters above mean sea level (AMSL) or None if not available - - .. seealso:: - `Altitude definitions `_ - """ - if self._vehicle_global_position is not None and hasattr(self._vehicle_global_position, 'alt'): - return self._vehicle_global_position.alt - else: - return None - - @property - 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 `_ - """ - if self._vehicle_local_position is not None and hasattr(self._vehicle_local_position, 'ref_alt'): - return self._vehicle_local_position.ref_alt - else: - return None - - @property - def global_position(self) -> Optional[GeoPoint]: - """Returns vehicle global position as a :class:`.GeoPoint`""" - if self._vehicle_global_position is not None and hasattr(self._vehicle_global_position, 'lat') \ - and hasattr(self._vehicle_global_position, 'lon'): - return GeoPoint(self._vehicle_global_position.lon, self._vehicle_global_position.lat, crs='epsg:4326') - else: - return None - - @property - def local_frame_origin(self) -> Optional[Position]: - """Returns vehicle local frame origin as a :class:`.Position` - - .. warning:: - Assumes local frame origin is on ground (starting location) - """ - if self._vehicle_local_position is not None \ - and self.global_position is not None \ - and hasattr(self._vehicle_local_position, 'ref_lat') \ - and hasattr(self._vehicle_local_position, 'ref_lon') \ - and hasattr(self._vehicle_local_position, 'ref_lon'): - - # TODO: self.home_altitude is redundant - altitude = Altitude( - agl=0, # local frame origin assumed on ground level - amsl=self._vehicle_local_position.ref_alt, - ellipsoid=self._vehicle_local_position.ref_alt + self._egm96.height(self.global_position.lat, - self.global_position.lon), - home=0 - ) - position = Position( - xy=GeoPoint(self._vehicle_local_position.ref_lon, self._vehicle_local_position.ref_lat, crs='epsg:4326'), - altitude=altitude, - attitude=None, - timestamp=None - ) - return position - else: - return None diff --git a/gisnav/data.py b/gisnav/data.py index 2961b4b7..0c036d97 100644 --- a/gisnav/data.py +++ b/gisnav/data.py @@ -13,6 +13,7 @@ except DataValueError as dve: self.get_logger().warn(f'Error determining vehicle position:\n{dve},\n{traceback.print_exc()}.') return None + """ from __future__ import annotations # Python version 3.7+ @@ -24,17 +25,19 @@ warnings.filterwarnings(action='ignore', category=UserWarning, message='Gimbal lock detected.') from xml.etree import ElementTree -from typing import Optional +from typing import Optional, Tuple from dataclasses import dataclass, field from collections import namedtuple -from multiprocessing.pool import AsyncResult from scipy.spatial.transform import Rotation +from shapely.geometry import box +from geographic_msgs.msg import GeoPoint from gisnav.assertions import assert_type, assert_ndim, assert_shape, assert_len -from gisnav.geo import GeoPoint, GeoTrapezoid, GeoValueError +from gisnav.geo import GeoPt, GeoTrapezoid, GeoValueError Dim = namedtuple('Dim', 'height width') TimePair = namedtuple('TimePair', 'local foreign') +BBox = namedtuple('BBox', 'left bottom right top') # noinspection PyClassHasNoInit @@ -45,7 +48,7 @@ class Position: .. note:: (x, y, z) coordinates are in ENU frame """ - xy: GeoPoint # XY coordinates (e.g. longitude & latitude in WGS84) + xy: GeoPt # XY coordinates (e.g. longitude & latitude in WGS84) altitude: Altitude attitude: Optional[Attitude] # attitude in NED frame timestamp: Optional[int] # Reference timestamp of position @@ -90,7 +93,7 @@ def to_esd(self) -> Attitude: :return: Attitude in SED frame """ - nadir_pitch = np.array([0, np.sin(np.pi/4), 0, np.sin(np.pi/4)]) # Adjust origin to nadir facing camera + nadir_pitch = np.array([0, np.sin(np.pi / 4), 0, np.sin(np.pi / 4)]) # Adjust origin to nadir facing camera r = Rotation.from_quat(self.q) * Rotation.from_quat(nadir_pitch) q = r.as_quat() q = np.array([q[1], -q[0], q[2], -q[3]]) # NED to ESD @@ -158,39 +161,40 @@ def __post_init__(self): @dataclass(frozen=True) class MapData(_ImageHolder): """Keeps map frame related data in one place and protects it from corruption.""" - bbox: GeoSquare + bbox: BBox elevation: Optional[Img] = None # Optional elevation raster def __post_init__(self): """Post-initialization validity check.""" if self.elevation is not None: assert self.elevation.arr.shape[0:2], self.image.arr.shape[0:2] - assert_ndim(self.elevation.arr, 2) # Grayscale image expected + assert_ndim(self.elevation.arr, 2) # Grayscale image expected # noinspection PyClassHasNoInit @dataclass(frozen=True) class ContextualMapData(_ImageHolder): - """Contains the rotated and cropped map image for _match estimation""" - image: Img = field(init=False) # This is the cropped and rotated map which is same size as the camera frames + """Contains the rotated and cropped map image pose estimation""" + image: Img = field(init=False) # Cropped and rotated map which is the same size as the camera frames elevation: Optional[Img] = field(init=None) # Rotated elevation raster (optional) in meters - rotation: float # radians - crop: Dim # Same value will also be found at image.dim (but not at initialization) - map_data: MapData # This is the original (square) map with padding + rotation: float # radians + crop: Dim # Same value will also be found at image.dim (but not at initialization) + map_data: MapData # This is the original (square) map with padding pix_to_wgs84: np.ndarray = field(init=False) - mock_data: bool = False # Indicates that this was used for field of view guess (mock map data) - altitude_scaling: Optional[float] = None # altitude scaling (elevation raster meters -> camera pixels) + mock_data: bool = False # Indicates that this was used for field of view guess (mock map data) + altitude_scaling: Optional[float] = None # altitude scaling (elevation raster meters -> camera pixels) + # TODO: update docs - only one transformation is returned def _pix_to_wgs84(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: - """Returns tuple of affine 2D transformation matrix for converting matched pixel coordinates to WGS84 coordinates - along with intermediate transformations + """Returns tuple of affine 2D transformation matrix for converting matched pixel coordinates to WGS84 + coordinates along with intermediate transformations - These transformations can be used to reverse the rotation and cropping that :func:`~rotate_and_crop_map` did to + These transformations can be used to reverse the rotation and cropping that :func:`._rotate_and_crop_map` did to the original map. - :return: Tuple containing 2D affinre transformations from 1. pixel coordinates to WGS84, 2. from original unrotated - and uncropped map pixel coordinates to WGS84, 3. from rotated map coordinates to unrotated map coordinates, and 4. - from cropped map coordinates to uncropped (but still rotated) map pixel coordinates. + :return: Tuple containing 2D affine transformations from 1. pixel coordinates to WGS84, 2. from original + unrotated and uncropped map pixel coordinates to WGS84, 3. from rotated map coordinates to unrotated map + coordinates, and 4. from cropped map coordinates to uncropped (but still rotated) map pixel coordinates. """ map_dim_arr = np.array(self.map_data.image.dim) img_dim_arr = np.array(self.image.dim) @@ -206,14 +210,16 @@ def _pix_to_wgs84(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray] uncropped_to_unrotated = np.vstack((rotation, rotation_padding)) src_corners = create_src_corners(*self.map_data.image.dim) - dst_corners = self.map_data.bbox.to_crs('epsg:4326').coords # .reshape(-1, 1, 2) + coords = np.array(box(*self.map_data.bbox).exterior.coords) + gt = GeoTrapezoid(coords) # .reshape(-1, 1, 2) + dst_corners = gt.square_coords dst_corners = np.flip(dst_corners, axis=1) # from ENU frame to WGS 84 axis order unrotated_to_wgs84 = cv2.getPerspectiveTransform(np.float32(src_corners).squeeze(), np.float32(dst_corners).squeeze()) # Ratio of boundaries in pixels and meters -> Altitude (z) scaling factor - vertical_scaling = abs(self.map_data.bbox.meter_length / \ - (2*self.map_data.image.dim.width + 2*self.map_data.image.dim.height)) + vertical_scaling = abs(gt.meter_length / \ + (2 * self.map_data.image.dim.width + 2 * self.map_data.image.dim.height)) pix_to_wgs84_ = unrotated_to_wgs84 @ uncropped_to_unrotated @ pix_to_uncropped @@ -222,9 +228,7 @@ def _pix_to_wgs84(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray] return pix_to_wgs84_ # , unrotated_to_wgs84, uncropped_to_unrotated, pix_to_uncropped def _rotate_and_crop_map(self, elevation: bool = False) -> np.ndarray: - """Rotates map counter-clockwise and then crops a dimensions-sized part from the middle. - - Map needs padding so that a circle with diameter of the diagonal of the img_size rectangle is enclosed in map. + """Rotates map counter-clockwise and then crops a dimensions-sized part from the middle :param elevation: Set True to do rotation on elevation raster instead :return: Rotated and cropped map raster @@ -248,7 +252,7 @@ def _rotate_and_crop_map(self, elevation: bool = False) -> np.ndarray: @staticmethod def _crop_center(img: np.ndarray, dimensions: Dim) -> np.ndarray: - """Crops dimensions sized part from center. + """Crops dimensions sized part from center :param img: Image to crop :param dimensions: Dimensions of area to crop (not of image itself) @@ -257,9 +261,8 @@ def _crop_center(img: np.ndarray, dimensions: Dim) -> np.ndarray: cx, cy = tuple(np.array(img.shape[0:2]) / 2) img_cropped = img[math.floor(cy - dimensions.height / 2):math.floor(cy + dimensions.height / 2), math.floor(cx - dimensions.width / 2):math.floor(cx + dimensions.width / 2)] - assert ( - img_cropped.shape[0:2] == dimensions.height, dimensions.width), 'Something went wrong when cropping the ' \ - 'map raster. ' + assert (img_cropped.shape[0:2] == dimensions.height, dimensions.width), \ + 'Something went wrong when cropping the map raster.' return img_cropped def __post_init__(self): @@ -280,44 +283,6 @@ class ImagePair: ref: ContextualMapData -# noinspection PyClassHasNoInit -@dataclass(frozen=True) -class _AsyncQuery: - """Abstract base class of a structure that stores a :py:class:`multiprocessing.pool.AsyncResult` instance along - with its input data - - The intention is to keep the result of the query in the same place along with the inputs so that they can be - easily reunited again in the callback function. - - .. note:: - You should not try to instantiate this class directly. Use child classes instead. - """ - result: AsyncResult - - -# noinspection PyClassHasNoInit -@dataclass(frozen=True) -class AsyncPoseQuery(_AsyncQuery): - """Stores a :py:class:`multiprocessing.pool.AsyncResult` instance along with its input data - - The :meth:`.PoseEstimator.worker` interface expects an image_pair (query, reference images and camera intrinsics matrix) - and an input_data context as arguments (along with a guess which is not stored since it is no longer needed after - the _match estimation). - """ - image_pair: ImagePair - input_data: InputData - - -# noinspection PyClassHasNoInit -@dataclass(frozen=True) -class AsyncWMSQuery(_AsyncQuery): - """Stores a :py:class:`multiprocessing.pool.AsyncResult` instance along with its input data - - The :meth:`.WMSClient.worker` expects the :class:`.GeoSquare` bounds as input it is needed here - """ - geobbox: GeoSquare - - # noinspection PyClassHasNoInit @dataclass(frozen=True) class Pose: @@ -345,21 +310,6 @@ def __iter__(self): yield item -# noinspection PyClassHasNoInit -@dataclass(frozen=True) -class Snapshot: - """Snapshot of vehicle and environment state""" - synchronized_time: Optional[int] - attitude: Optional[Attitude] - gimbal_attitude: Optional[Attitude] - gimbal_set_attitude: Optional[Attitude] - global_position: Optional[GeoPoint] - altitude: Optional[Altitude] - home_altitude: Optional[Altitude] - terrain_altitude: Optional[Altitude] - local_frame_origin: Optional[Position] - - # noinspection PyClassHasNoInit @dataclass(frozen=True) class Altitude: @@ -371,7 +321,7 @@ class Altitude: home: Above home or starting location .. note:: - Altitude AGL should always be known (cannot be None) + TODO: Altitude AGL should always be known (cannot be None) .. seealso:: `Altitude definitions `_ @@ -381,31 +331,19 @@ class Altitude: ellipsoid: Optional[float] home: Optional[float] + # TODO #def __post_init__(self): # """Post-initialization validity checks""" # assert self.agl is not None - - -# noinspection PyClassHasNoInit -@dataclass(frozen=True) -class InputData: - """InputData of vehicle state and other variables needed for postprocessing pose estimates""" - r_guess: Optional[np.ndarray] - """Camera expected attitude""" - - snapshot: Snapshot - """Snapshot of vehicle and environment state""" - - # noinspection PyClassHasNoInit @dataclass class FOV: """Camera field of view related attributes""" fov_pix: np.ndarray fov: Optional[GeoTrapezoid] - c: GeoPoint + c: GeoPt c_pix: np.ndarray scaling: float = field(init=False) @@ -444,13 +382,16 @@ def __post_init__(self): class FixedCamera: """WGS84-fixed camera attributes - Collects field of view and map_match under a single structure that is intended to be stored in input data context as - visual odometry fix reference. Includes the needed map_match and pix_to_wgs84 transformation for the vo fix. + # TODO: refactor this class out - it was used earlier for visual odometry but is now redundant + + Collects field of view and map_match under a single structure. """ image_pair: ImagePair pose: Pose timestamp: int - snapshot: Snapshot + terrain_altitude_amsl: Optional[float] + terrain_altitude_ellipsoid: Optional[float] + home_position: Optional[GeoPoint] fov: FOV = field(init=False) position: Position = field(init=False) h: np.ndarray = field(init=False) @@ -472,7 +413,7 @@ def _estimate_fov(self) -> Optional[FOV]: fov = FOV(fov_pix=fov_pix, fov=GeoTrapezoid(np.flip(fov_wgs84, axis=2), crs='epsg:4326'), c_pix=c_pix, - c=GeoPoint(*c_wgs84.squeeze()[::-1], crs='epsg:4326') + c=GeoPt(*c_wgs84.squeeze()[::-1], crs='epsg:4326') ) return fov except GeoValueError as _: @@ -482,7 +423,7 @@ def _estimate_fov(self) -> Optional[FOV]: # Could not create a valid FOV return None - def _estimate_attitude(self) -> np.ndarray: + def _estimate_attitude(self) -> Attitude: """Estimates gimbal (not vehicle) attitude in NED frame .. note:: @@ -496,7 +437,6 @@ def _estimate_attitude(self) -> np.ndarray: assert not np.isnan(rT).any() gimbal_estimated_attitude = Rotation.from_matrix(rT) # rotated map pixel frame - #gimbal_estimated_attitude *= Rotation.from_rotvec(-(np.pi/2) * np.array([0, 1, 0])) # camera body gimbal_estimated_attitude *= Rotation.from_rotvec( self.image_pair.ref.rotation * np.array([0, 0, 1])) # unrotated map pixel frame @@ -512,7 +452,7 @@ def _get_fov_and_c(img_arr_shape: Tuple[int, int], h_mat: np.ndarray) -> Tuple[n :param img_arr_shape: Image array shape tuple (height, width) :param h_mat: Homography matrix - :return: Tuple of FOV corner coordinates and prinicpal point np.ndarrays + :return: Tuple of FOV corner coordinates and principal point np.ndarrays """ assert_type(img_arr_shape, tuple) assert_len(img_arr_shape, 2) @@ -542,13 +482,12 @@ def _estimate_position(self, terrain_altitude_amsl: Optional[float], terrain_alt :param terrain_altitude_amsl: Optional ground elevation above AMSL in meters :param terrain_altitude_ellipsoid: Optional ground elevation above WGS 84 ellipsoid in meters :param crs: CRS to use for the Position - :return: Camera position + :return: Camera position or None if not available """ assert self.fov is not None # Call _estimate_fov before _estimate_position! # Translation in WGS84 (and altitude or z-axis translation in meters above ground) assert_type(self.image_pair.ref, ContextualMapData) # need pix_to_wgs84 t_wgs84 = self.image_pair.ref.pix_to_wgs84 @ np.append(self.camera_position[0:2], 1) - #t_wgs84[2] = -self.image_pair.ref.pix_to_wgs84[2][2] * self.camera_position[2] # In NED frame z-coordinate is negative above ground, make altitude >0 t_wgs84[2] = -self.fov.scaling * self.camera_position[2] # In NED frame z-coordinate is negative above ground, make altitude >0 # Check that we have all the values needed for a global position @@ -566,7 +505,7 @@ def _estimate_position(self, terrain_altitude_amsl: Optional[float], terrain_alt home=None # TODO ) position = Position( - xy=GeoPoint(lon, lat, crs), # lon-lat order + xy=GeoPt(lon, lat, crs), # lon-lat order altitude=altitude, attitude=self._estimate_attitude(), timestamp=self.image_pair.qry.timestamp @@ -583,8 +522,8 @@ def __post_init__(self): raise DataValueError('Please provide valid image pair.') img = self.image_pair.qry - if self.snapshot.terrain_altitude.amsl is not None and self.snapshot.terrain_altitude.ellipsoid is None or \ - self.snapshot.terrain_altitude.amsl is not None and self.snapshot.terrain_altitude.ellipsoid is None: + if self.terrain_altitude_amsl is not None and self.terrain_altitude_ellipsoid is None or \ + self.terrain_altitude_amsl is not None and self.terrain_altitude_ellipsoid is None: raise DataValueError('Please provide terrain altitude in both AMSL and above WGS 84 ellipsoid.') object.__setattr__(self, 'h', img.camera_data.k @ np.delete(self.pose.e, 2, 1)) # Remove z-column, making the matrix square @@ -601,8 +540,8 @@ def __post_init__(self): raise DataValueError('Could not initialize a valid FixedCamera.') try: - position = self._estimate_position(self.snapshot.terrain_altitude.amsl, - self.snapshot.terrain_altitude.ellipsoid) + position = self._estimate_position(self.terrain_altitude_amsl, + self.terrain_altitude_ellipsoid) if position is not None: object.__setattr__(self, 'position', position) else: @@ -619,6 +558,9 @@ def __post_init__(self): raise DataValueError(f'pose.t {self.pose.t} & pose.t {self.pose.t} have values too large compared to ' \ f'(cx, cy, fx): {reference}.') + # Fix home position + if self.home_position is not None: + object.__setattr__(self.position.altitude, 'home', self.position.altitude.ellipsoid - self.home_position.altitude) # noinspection PyClassHasNoInit @dataclass(frozen=True) diff --git a/gisnav/geo.py b/gisnav/geo.py index c0ff281c..b607c77e 100644 --- a/gisnav/geo.py +++ b/gisnav/geo.py @@ -1,6 +1,6 @@ """Module containing classes that wrap :class:`geopandas.GeoSeries` for convenience""" from __future__ import annotations - +from typing import TYPE_CHECKING, Tuple import math import numpy as np import warnings @@ -11,6 +11,9 @@ from shapely.geometry import Point, Polygon, box from gisnav.assertions import assert_len, assert_type +if TYPE_CHECKING: + # CameraData used as type hint only - avoid ImportError due to circular import + from .data import CameraData class _GeoObject(ABC): @@ -19,7 +22,7 @@ class _GeoObject(ABC): Each instance wraps a :class:`geopandas.GeoSeries` instance of length one, i.e. containing a single Shapely shape. The GeoSeries class is versatile and operates on an entire series, so this wrapper is provided to only expose specific functionality that is needed in the application as well as provide an abstraction for a 'GeoObject' i.e. a - GeoSeries of length 1. For example, it is conceptually more convenient to handle a 'GeoPoint' object than a + GeoSeries of length 1. For example, it is conceptually more convenient to handle a 'GeoPt' object than a GeoSeries of length 1 with a single Shapely Point in it. """ @@ -43,7 +46,7 @@ def coords(self) -> np.ndarray: def to_crs(self, crs: str) -> _GeoObject: # TODO: return None? Misleading this way """Converts to provided CRS - :return: The same GeoPoint instance transformed to new CRS + :return: The same GeoPt instance transformed to new CRS """ if self._geoseries.crs != crs: self._geoseries = self._geoseries.to_crs(crs) @@ -68,12 +71,12 @@ class _GeoPolygon(_GeoObject): """Abstract base class for :class:`_GeoObject`s that contain a Shapely Polygon""" @property - def center(self) -> GeoPoint: + def center(self) -> GeoPt: """Returns center point of the polygon""" - return GeoPoint(*self._geoseries.centroid[0].coords[0], crs=self.crs) + return GeoPt(*self._geoseries.centroid[0].coords[0], crs=self.crs) @property - def bounds(self) -> Tuple[4*(float,)]: + def bounds(self) -> Tuple[4 * (float,)]: """Returns (left, bottom, right, top) or (minx, miny, maxx, maxy) formatted tuple (e.g. for WMS GetMap)""" return self._geoseries[0].bounds @@ -106,9 +109,9 @@ def coords(self) -> np.ndarray: assert_len(exterior_coords, 4) return exterior_coords - def intersection(self, box: _GeoPolygon) -> GeoTrapezoid: + def intersection(self, box_: _GeoPolygon) -> GeoTrapezoid: """Returns the intersection with the given :class:`._GeoPolygon`""" - return GeoTrapezoid(self._geoseries.intersection(box._geoseries)) + return GeoTrapezoid(self._geoseries.intersection(box_._geoseries)) def __post_init__(self): """Post-initialization validity checks @@ -118,7 +121,7 @@ def __post_init__(self): assert_type(self._geoseries[0], Polygon) -class GeoPoint(_GeoObject): +class GeoPt(_GeoObject): """Wrapper for :class:`geopandas.GeoSeries` that constrains it to a 2D Point (a geographical coordinate pair)""" def __init__(self, x: float, y: float, crs: str = _GeoObject.DEFAULT_CRS): """Creates a geographical coordinate pair @@ -163,7 +166,7 @@ def spherical_adjustment(self): class GeoSquare(_GeoPolygon): """Wrapper for :class:`geopandas.GeoSeries` that constrains it to a (square shaped) bounding box""" - def __init__(self, center: GeoPoint, radius: float, crs: str = _GeoObject.DEFAULT_CRS): + def __init__(self, center: GeoPt, radius: float, crs: str = _GeoObject.DEFAULT_CRS): """Creates a square bounding box by enveloping a circle of given radius at given center :param center: Center of the bounding box @@ -195,7 +198,7 @@ def coords(self) -> np.ndarray: class GeoTrapezoid(_GeoPolygon): """Wrapper for :class:`geopandas.GeoSeries` that constrains it to a (convex isosceles) trapezoid - Used to represents camera field of view projected to ground plane. + Used to represent camera field of view projected to ground plane. """ def __init__(self, corners: np.ndarray, crs: str = _GeoObject.DEFAULT_CRS): """Creates a square bounding box by enveloping a circle of given radius inside @@ -208,8 +211,6 @@ def __init__(self, corners: np.ndarray, crs: str = _GeoObject.DEFAULT_CRS): def __post_init__(self): """Post-initialization validity checks""" - #if not (self._geoseries[0].is_valid and self._is_convex_isosceles_trapezoid()): - # raise GeoValueError(f'Not a valid convex isosceles trapezoid: {self._geoseries[0]}') if not (self._geoseries[0].is_valid and not self._is_convex()): raise GeoValueError(f'Not a valid convex trapezoid: {self._geoseries[0]}') @@ -220,35 +221,35 @@ def _is_convex(self) -> bool: """ return self._geoseries[0].bounds == self._geoseries[0].convex_hull.bounds - #def _is_convex_isosceles_trapezoid(self, diagonal_length_tolerance: float = 0.1) -> bool: - # """Returns True if the quadrilateral is a convex isosceles trapezoid - # - # .. note:: - # If the estimated field of view (FOV) is not a convex isosceles trapezoid, it is a sign that (1) the match - # was bad or (2) the gimbal the camera is mounted on has not had enough time to stabilize (camera has - # non-zero roll). Matches where the FOV is not a convex isosceles trapezoid should be rejected assuming we - # can't tell (1) from (2) and that it is better to wait for a good position estimate than to use a bad one. - # - # .. seealso:: - # :func:`.create_src_corners` for the assumed order of the quadrilateral corners. - # - # :param diagonal_length_tolerance: Tolerance for relative length difference between trapezoid diagonals - # :return: True if the quadrilateral is a convex isosceles trapezoid - # """ - # ul, ll, lr, ur = tuple(map(lambda pt: pt.squeeze().tolist(), self.coords)) - # - # # Check convexity (exclude self-crossing trapezoids) - # # Note: inverted y-axis, origin at upper left corner of image - # if not (ul[0] < ur[0] and ul[1] < ll[1] and lr[0] > ll[0] and lr[1] > ur[1]): - # return False - # - # # Check diagonals same length within tolerance - # ul_lr_length = math.sqrt((ul[0] - lr[0]) ** 2 + (ul[1] - lr[1]) ** 2) - # ll_ur_length = math.sqrt((ll[0] - ur[0]) ** 2 + (ll[1] - ur[1]) ** 2) - # if abs((ul_lr_length / ll_ur_length) - 1) > diagonal_length_tolerance: - # return False - # - # return True + @property + def square_coords(self) -> np.ndarray: + """Returns a numpy array of the corners coordinates of the bbox + + Order should be top-left, bottom-left, bottom-right, top-right (same as + :meth:`gisnav.data.create_src_corners`). + """ + corners = box(*self.bounds).exterior.coords + corners = np.array([ + corners[2], # tl + corners[3], # bl + corners[0], # br + corners[1] # tr + ]) + + return corners + + +def get_dynamic_map_radius(camera_data: CameraData, max_map_radius: int, altitude: float) -> float: + """Returns map radius that adjusts for camera altitude to be used for new map requests + + :param camera_data: Camera data + :param max_map_radius: Max map radius + :param altitude: Altitude of camera in meters + :return: Suitable map radius in meters + """ + hfov = 2 * math.atan(camera_data.dim.width / (2 * camera_data.fx)) + map_radius = 1.5 * hfov * altitude # Arbitrary padding of 50% + return min(map_radius, max_map_radius) class GeoValueError(ValueError): diff --git a/gisnav/nodes/__init__.py b/gisnav/nodes/__init__.py index e69de29b..fd2cadbe 100644 --- a/gisnav/nodes/__init__.py +++ b/gisnav/nodes/__init__.py @@ -0,0 +1,89 @@ +"""Entry points for ROS 2 nodes + +All ROS 2 nodes are defined in dedicated modules to keep individual file size down. They are imported here to package +namespace for convenience. For example: + +.. code-block:: + + #from gisnav.nodes.pose_estimator_node import PoseEstimatorNode + from gisnav.nodes import PoseEstimatorNode + +Node names are hard-coded inside the static public node entrypoints defined here. Other node initialization arguments +are provided via ROS 2 launch arguments. +""" +import rclpy +import cProfile +import pstats +import io + +from .px4_node import PX4Node +from .ardupilot_node import ArduPilotNode +from .mock_gps_node import MockGPSNode +from .map_node import MapNode +from .bbox_node import BBoxNode +from .pose_estimation_node import PoseEstimationNode + + +def _run(constructor: rclpy.node.Node, *args, **kwargs): + """Spins up a ROS 2 node + + :param constructor: Node constructor + :param *args: Node constructor args + :param **kwargs: Node constructor kwargs + :return: + """ + if __debug__: + profile = cProfile.Profile() + profile.enable() + else: + profile = None + + node = None + try: + rclpy.init() + node = constructor(*args, **kwargs) + rclpy.spin(node) + except KeyboardInterrupt as e: + print(f'Keyboard interrupt received:\n{e}') + if profile is not None: + assert __debug__ + # Print out cProfile stats + profile.disable() + s = io.StringIO() + stats = pstats.Stats(profile, stream=s).sort_stats(pstats.SortKey.CUMULATIVE) + stats.print_stats(40) + print(s.getvalue()) + finally: + if node is not None: + node.destroy_node() + rclpy.shutdown() + + +def run_px4_node(): + """Spins up a :class:`.PX4Node`""" + _run(PX4Node, 'px4_node') + + +def run_ardupilot_node(): + """Spins up a :class:`.ArduPilotNode`""" + _run(ArduPilotNode, 'ardupilot_node') + + +def run_mock_gps_node(): + """Spins up a :class:`.MockGPSNode`""" + _run(MockGPSNode, 'mock_gps_node') + + +def run_bbox_node(): + """Spins up a :class:`.BBoxNode`""" + _run(BBoxNode, 'bbox_node') + + +def run_map_node(): + """Spins up a :class:`.MapNode`""" + _run(MapNode, 'map_node') + + +def run_pose_estimation_node(): + """Spins up a :class:`.PoseEstimationNode`""" + _run(PoseEstimationNode, 'pose_estimation_node') diff --git a/gisnav/nodes/ardupilot_node.py b/gisnav/nodes/ardupilot_node.py new file mode 100644 index 00000000..16f7a494 --- /dev/null +++ b/gisnav/nodes/ardupilot_node.py @@ -0,0 +1,190 @@ +"""Module that contains the ArduPilot adapter ROS 2 node.""" +from typing import Optional, List, Tuple, Union + +import numpy as np +from scipy.spatial.transform import Rotation +from rclpy.qos import QoSPresetProfiles +from geometry_msgs.msg import Quaternion, PoseStamped +from geographic_msgs.msg import GeoPoint, GeoPointStamped, GeoPose, GeoPoseStamped +from mavros_msgs.msg import Altitude, HomePosition, MountControl +from sensor_msgs.msg import NavSatFix + +from . import messaging +from .base.autopilot_node import AutopilotNode + + +class ArduPilotNode(AutopilotNode): + """ROS 2 node that acts as an adapter for ArduPilot vis MAVROS""" + + ROS_PARAM_DEFAULTS = [] + """List containing ROS parameter name, default value and read_only flag tuples""" + + def __init__(self, name: str) -> None: + """Initializes the ROS 2 node + + :param name: Name of the node + """ + super().__init__(name) + + self._vehicle_nav_sat_fix = None + self._vehicle_nav_sat_fix_sub = self.create_subscription(NavSatFix, + '/mavros/global_position/global', + self._vehicle_nav_sat_fix_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._vehicle_pose_stamped = None + self._vehicle_pose_stamped_sub = self.create_subscription(PoseStamped, + '/mavros/local_position/pose', + self._vehicle_pose_stamped_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._home_position = None + self._home_position_sub = self.create_subscription(HomePosition, + '/mavros/home_position/home', + self._home_position_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._mount_control = None + self._mount_control_sub = self.create_subscription(MountControl, + '/mavros/mount_control/command', + self._mount_control_callback, + QoSPresetProfiles.SENSOR_DATA.value) + + # region ROS subscriber callbacks + def _vehicle_nav_sat_fix_callback(self, msg: NavSatFix) -> None: + """Handles latest :class:`mavros_msgs.msg.NavSatFix` message + + Calls :meth:`.publish_vehicle_geopose` and :meth:`.publish_vehicle_altitude` because the contents of those + messages are affected by an updated :class:`mavros_msgs.msg.NavSatFix` message. + + :param msg: :class:`mavros_msgs.msg.NavSatFix` message from MAVROS + """ + self._vehicle_nav_sat_fix = msg + self.publish_vehicle_geopose() + self.publish_vehicle_altitude() + self.publish_gimbal_quaternion() # TODO: temporarily assuming static camera so publishing gimbal quat here + + def _vehicle_pose_stamped_callback(self, msg: PoseStamped) -> None: + """Handles latest :class:`mavros_msgs.msg.PoseStamped` message + + Calls :meth:`.publish_vehicle_geopose` because the content of that message is affected by an updated + :class:`mavros_msgs.msg.PoseStamped` message. + + :param msg: :class:`mavros_msgs.msg.PoseStamped` message from MAVROS + """ + self._vehicle_pose_stamped = msg + self.publish_vehicle_geopose() + #self.publish_vehicle_altitude() # Needed? This is mainly about vehicle attitude + + def _home_position_callback(self, msg: HomePosition) -> None: + """Handles latest :class:`mavros_msgs.msg.HomePosition` message + + Calls :meth:`.publish_home_geopoint` because the content of that message is affected by an updated + :class:`mavros_msgs.msg.HomePosition` message. + + :param msg: :class:`mavros_msgs.msg.HomePosition` message from MAVROS + """ + self._home_position = msg + self.publish_home_geopoint() + + def _mount_control_callback(self, msg: MountControl) -> None: + """Handles latest :class:`mavros_msgs.msg.MountControl` message + + Calls :meth:`.publish_gimbal_quaternion` because the content of that message is affected by an updated + :class:`mavros_msgs.msg.MountControl` message. + + :param msg: :class:`mavros_msgs.msg.MountControl` message from MAVROS + """ + self._mount_control = msg + self.publish_gimbal_quaternion() + # endregion ROS subscriber callbacks + + # region computed attributes + @property + def vehicle_geopose(self) -> Optional[GeoPoseStamped]: + """Vehicle pose as :class:`geographic_msgs.msg.GeoPoseStamped` message or None if not available""" + if self._vehicle_nav_sat_fix is not None and self._vehicle_pose_stamped is not None: + # Position + latitude, longitude = self._vehicle_nav_sat_fix.latitude, self._vehicle_nav_sat_fix.longitude + altitude = self._vehicle_nav_sat_fix.altitude + + # Convert ENU->NED + re-center yaw + enu_to_ned = Rotation.from_euler('XYZ', np.array([np.pi, 0, np.pi / 2])) + attitude_ned = Rotation.from_quat(messaging.as_np_quaternion(self._vehicle_pose_stamped.pose.orientation)) \ + * enu_to_ned.inv() + rpy = attitude_ned.as_euler('XYZ', degrees=True) + rpy[0] = (rpy[0] + 180) % 360 + attitude_ned = Rotation.from_euler('XYZ', rpy, degrees=True) + attitude_ned = attitude_ned.as_quat() + orientation = messaging.as_ros_quaternion(attitude_ned) + + return GeoPoseStamped(header=messaging.create_header('base_link'), + pose=GeoPose( + position=GeoPoint(latitude=latitude, longitude=longitude, altitude=altitude), + orientation=orientation) + ) + else: + self.get_logger().warn('NavSatFix and/or PoseStamped message not yet received, cannot determine vehicle ' + 'geopose.') + return None + + @property + def _vehicle_altitude_local(self) -> Optional[float]: + """Returns z coordinate from :class:`sensor_msgs.msg.PoseStamped` message or None if not available""" + if self._vehicle_pose_stamped is not None: + return self._vehicle_pose_stamped.pose.position.z + else: + self.get_logger().warn('VehicleLocalPosition message not yet received, cannot determine vehicle local ' + 'altitude.') + return None + + @property + def vehicle_altitude(self) -> Optional[Altitude]: + """Vehicle altitude as :class:`mavros_msgs.msg.Altitude` message or None if not available""" + if self._vehicle_nav_sat_fix is not None and self.egm96_height is not None \ + and self.terrain_altitude is not None: + vehicle_altitude_amsl = self._vehicle_nav_sat_fix.altitude - self.egm96_height.data + vehicle_altitude_terrain = vehicle_altitude_amsl - self.terrain_altitude.amsl + local = self._vehicle_altitude_local if self._vehicle_altitude_local is not None else np.nan + altitude = Altitude( + header=messaging.create_header('base_link'), + amsl=vehicle_altitude_amsl, + local=local, # TODO: home altitude ok? see https://mavlink.io/en/messages/common.html#ALTITUDE + relative=-local, + terrain=vehicle_altitude_terrain, + bottom_clearance=np.nan + ) + return altitude + else: + self.get_logger().warn(f'NavSatFix {self._vehicle_nav_sat_fix} and/or terrain Altitude ' + f'{self.terrain_altitude} and/or EGM 96 height {self.egm96_height } message not yet ' + f'received, cannot determine vehicle altitude.') + return None + + @property + def gimbal_quaternion(self) -> Optional[Quaternion]: + """Gimbal orientation as :class:`geometry_msgs.msg.Quaternion` message or None if not available + + .. note:: + Current implementation assumes camera is facing directly down from vehicle body + """ + # TODO: assumes static nadir facing camera, do proper implementation + if self.vehicle_geopose is not None: + vehicle_attitude = Rotation.from_quat(messaging.as_np_quaternion(self.vehicle_geopose.pose.orientation)) + gimbal_rpy = vehicle_attitude.as_euler('xyz', degrees=True) + gimbal_rpy[1] -= 90 + gimbal_attitude = Rotation.from_euler('xyz', gimbal_rpy, degrees=True) + return messaging.as_ros_quaternion(gimbal_attitude.as_quat()) + else: + self.get_logger().warn('Vehicle GeoPose unknown, cannot determine gimbal quaternion for static camera.') + return None + + @property + def home_geopoint(self) -> Optional[GeoPointStamped]: + """Home position as :class:`geographic_msgs.msg.GeoPointStamped` message or None if not available""" + if self._home_position is not None: + return GeoPointStamped(header=messaging.create_header('base_link'), + position=GeoPoint(latitude=self._home_position.geo.latitude, + longitude=self._home_position.geo.longitude, + altitude=self._home_position.geo.altitude)) + else: + self.get_logger().warn('HomePosition message not yet received, cannot determine home geopoint.') + return None + # endregion computed attributes diff --git a/gisnav/nodes/base/__init__.py b/gisnav/nodes/base/__init__.py new file mode 100644 index 00000000..b64e88c9 --- /dev/null +++ b/gisnav/nodes/base/__init__.py @@ -0,0 +1 @@ +"""Abstract base classes used by ROS nodes""" \ No newline at end of file diff --git a/gisnav/nodes/base/autopilot_node.py b/gisnav/nodes/base/autopilot_node.py new file mode 100644 index 00000000..a513f448 --- /dev/null +++ b/gisnav/nodes/base/autopilot_node.py @@ -0,0 +1,149 @@ +"""Module that contains the AutopilotNode abstract base class ROS 2 node.""" +from abc import ABC, abstractmethod +from typing import Optional + +from rclpy.qos import QoSPresetProfiles + +from mavros_msgs.msg import Altitude +from geographic_msgs.msg import GeoPointStamped, GeoPoseStamped +from geometry_msgs.msg import Quaternion +from std_msgs.msg import Float32 + +from gisnav.nodes import messaging +from gisnav.nodes.base.base_node import BaseNode + + +class AutopilotNode(BaseNode, ABC): + """A ROS 2 abstract base class for nodes that provide a stable internal interface to autopilot telemetry + + .. note:: + This abstract base class is intended for package internal use only. It should be extended by nodes that adapt it + to the ROS topics provided by any given autopilot platform, for example PX4 or ArduPilot. + """ + def __init__(self, name: str) -> None: + """Initializes the ROS 2 node. + + :param name: Name of the node + """ + super().__init__(name) + + # Publishers + # Use name mangling to protect these from being overwritten by extending classes + self.__vehicle_geopose_pub = self.create_publisher(GeoPoseStamped, + messaging.ROS_TOPIC_VEHICLE_GEOPOSE, + QoSPresetProfiles.SENSOR_DATA.value) + self.__vehicle_altitude_pub = self.create_publisher(Altitude, + messaging.ROS_TOPIC_VEHICLE_ALTITUDE, + QoSPresetProfiles.SENSOR_DATA.value) + self.__gimbal_quaternion_pub = self.create_publisher(Quaternion, + messaging.ROS_TOPIC_GIMBAL_QUATERNION, + QoSPresetProfiles.SENSOR_DATA.value) + self.__home_geopoint_pub = self.create_publisher(GeoPointStamped, + messaging.ROS_TOPIC_HOME_GEOPOINT, + QoSPresetProfiles.SENSOR_DATA.value) + + # Subscribers + # terrain_altitude and egm96_height properties intended to be used by extending classes -> no name mangling + self.terrain_altitude = None + self.__terrain_altitude_sub = self.create_subscription(Altitude, + messaging.ROS_TOPIC_TERRAIN_ALTITUDE, + self.__terrain_altitude_callback, + QoSPresetProfiles.SENSOR_DATA.value) + + self.egm96_height = None + self.__egm96_height_sub = self.create_subscription(Float32, + messaging.ROS_TOPIC_EGM96_HEIGHT, + self.__egm96_height_callback, + QoSPresetProfiles.SENSOR_DATA.value) + + # region public properties + @property + def terrain_altitude(self) -> Optional[Altitude]: + """Terrain altitude + + Needed by implementing classes to generate vehicle :class:`geographic_msgs.msg.GeoPoseStamped` message + """ + return self.__terrain_altitude + + @terrain_altitude.setter + def terrain_altitude(self, value: Optional[Altitude]) -> None: + self.__terrain_altitude = value + + @property + def egm96_height(self) -> Optional[Float32]: + """EGM96 geoid height + + Needed by implementing classes to generate vehicle :class:`geographic_msgs.msg.GeoPoseStamped` and + :class:`mavros_msgs.msg.Altitude` messages + """ + return self.__egm96_height + + @egm96_height.setter + def egm96_height(self, value: Optional[Float32]) -> None: + self.__egm96_height = value + # endregion public properties + + # region ROS subscriber callbacks + def __terrain_altitude_callback(self, msg: Altitude) -> None: + """Handles terrain altitude message""" + self.__terrain_altitude = msg + + def __egm96_height_callback(self, msg: Float32) -> None: + """Handles ellipsoid height message""" + self.__egm96_height = msg + # endregion ROS subscriber callbacks + + # region publish hooks + @property + @abstractmethod + def vehicle_geopose(self) -> Optional[GeoPoseStamped]: + """Vehicle pose as :class:`geographic_msgs.msg.GeoPoseStamped` message or None if not available""" + pass + + def publish_vehicle_geopose(self) -> None: + """Publishes vehicle :class:`geographic_msgs.msg.GeoPoseStamped`""" + if self.vehicle_geopose is not None: + self.__vehicle_geopose_pub.publish(self.vehicle_geopose) + else: + self.get_logger().warn('Vehicle geopose unknown, skipping publishing.') + + @property + @abstractmethod + def vehicle_altitude(self) -> Optional[Altitude]: + """Vehicle altitude as :class:`mavros_msgs.msg.Altitude` message or None if not available""" + pass + + def publish_vehicle_altitude(self) -> None: + """Publishes vehicle :class:`mavros_msgs.msg.Altitude`""" + if self.vehicle_altitude is not None: + self.__vehicle_altitude_pub.publish(self.vehicle_altitude) + else: + self.get_logger().warn('Vehicle altitude unknown, skipping publishing.') + + @property + @abstractmethod + def gimbal_quaternion(self) -> Optional[Quaternion]: + """Gimbal orientation as :class:`geometry_msgs.msg.Quaternion` message or None if not available""" + pass + + def publish_gimbal_quaternion(self) -> None: + """Publishes gimbal :class:`geometry_msgs.msg.Quaternion` orientation""" + # TODO: NED or ENU? ROS convention is ENU but current implementation is actually NED? + if self.gimbal_quaternion is not None: + self.__gimbal_quaternion_pub.publish(self.gimbal_quaternion) + else: + self.get_logger().warn('Gimbal quaternion unknown, skipping publishing.') + + @property + @abstractmethod + def home_geopoint(self) -> Optional[GeoPointStamped]: + """Home position as :class:`geographic_msgs.msg.GeoPointStamped` message or None if not available""" + pass + + def publish_home_geopoint(self) -> None: + """Publishes home :class:`.geographic_msgs.msg.GeoPointStamped`""" + if self.home_geopoint is not None: + self.__home_geopoint_pub.publish(self.home_geopoint) + else: + self.get_logger().warn('Home geopoint unknown, skipping publishing.') + # endregion publish hooks diff --git a/gisnav/nodes/base/base_node.py b/gisnav/nodes/base/base_node.py new file mode 100644 index 00000000..1864e977 --- /dev/null +++ b/gisnav/nodes/base/base_node.py @@ -0,0 +1,66 @@ +"""Abstract base class implementing shared logic for all nodes""" +from abc import ABC, abstractmethod +from typing import Union, List, Tuple +import time + +from rclpy.node import Node +from rclpy.exceptions import ParameterAlreadyDeclaredException +from rcl_interfaces.msg import ParameterDescriptor + + +class BaseNode(ABC, Node): + """Abstract base class for all nodes implementing shared logic + + .. note:: + This class is intended to be extended by nodes in the same package and should not be imported directly in any + other package. + """ + @property + @abstractmethod + def ROS_PARAM_DEFAULTS(self) -> List[Tuple[str, Union[int, float, str, bool, List[str]], bool]]: + """List containing ROS parameter name, default value and read_only flag tuples + + The ROS parameter defaults are declared as an abstract property to encourage all implementing classes to define + the ROS parameters they use in one place. When defined as a class constant it also gets picked up by Sphinx + autodoc so that the list of ROS parameters for each node can be conveniently checked in the API docs. + + .. note:: + The property name is all upper-case to suggest it should be declared as class constant variable (for + documentation reasons stated above) instead of as a property. + """ + pass + + def __init__(self, name: str): + """Initializes the node + + :param name: Node name + """ + super().__init__(name, allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) + self.__declare_ros_params() + + @property + def sec(self) -> int: + """Returns system time in seconds""" + return int(time.time()) + + @property + def usec(self) -> int: + """Returns system time in microseconds""" + return int(time.time_ns() / 1e3) + + def __declare_ros_params(self) -> None: + """Declares ROS parameters from :py:attr:`.ROS_PARAM_DEFAULTS` + + Does not override ROS parameters that are already declared (e.g. from YAML file on launch). + """ + # Declare parameters one by one because declare_parameters will not declare remaining parameters if it + # raises a ParameterAlreadyDeclaredException + for param_tuple in self.ROS_PARAM_DEFAULTS: + param, default_value, read_only = param_tuple + try: + self.declare_parameter(param, default_value, ParameterDescriptor(read_only=read_only)) + self.get_logger().info(f'Using default value "{default_value}" for ROS parameter "{param}".') + except ParameterAlreadyDeclaredException as _: + # This means parameter is already declared (e.g. from a YAML file) + value = self.get_parameter(param).value + self.get_logger().info(f'ROS parameter "{param}" already declared with value "{value}".') diff --git a/gisnav/nodes/base/camera_subscriber_node.py b/gisnav/nodes/base/camera_subscriber_node.py new file mode 100644 index 00000000..1469e508 --- /dev/null +++ b/gisnav/nodes/base/camera_subscriber_node.py @@ -0,0 +1,104 @@ +"""Abstract base class for :class:`sensor_msgs.msg.CameraInfo and :class:`sensor_msgs.msg.Image` subscribers""" +from abc import abstractmethod +from typing import Optional, Tuple + +import numpy as np +from rclpy.qos import QoSPresetProfiles +from sensor_msgs.msg import CameraInfo, Image + +from .base_node import BaseNode +from gisnav.data import Dim, CameraData +from gisnav.assertions import assert_type + + +class CameraSubscriberNode(BaseNode): + """Abstract base class for :class:`sensor_msgs.msg.CameraInfo and :class:`sensor_msgs.msg.Image` subscribers + + Extending classes must implement the :class:`sensor_msgs.msg.Image` callback. :class:`sensor_msgs.msg.CameraInfo` + messages are automatically consumed and the contained information provided through the :py:attr:`.camera_data` + property for convenience. + + .. note:: + This class is intended to be extended by nodes in the same package and should not be imported directly in any + other package. + """ + + # TODO: move these to messaging.py? + ROS_CAMERA_INFO_TOPIC = 'camera/camera_info' + """:class:`sensor_msgs.msg.CameraInfo` topic to subscribe to""" + + ROS_IMAGE_TOPIC = 'camera/image_raw' + """:class:`sensor_msgs.msg.Image` topic to subscribe to""" + + def __init__(self, name: str): + """Initializes the node + + :param name: Node name + """ + super().__init__(name) + + self.__camera_info = None + self.__camera_info_sub = self.create_subscription(CameraInfo, + self.ROS_CAMERA_INFO_TOPIC, + self.__camera_info_callback, + QoSPresetProfiles.SENSOR_DATA.value) + + self.__image_sub = self.create_subscription(Image, + self.ROS_IMAGE_TOPIC, + self.image_callback, + QoSPresetProfiles.SENSOR_DATA.value) + + def __camera_info_callback(self, msg: CameraInfo) -> None: + """Handles latest :class:`sensor_msgs.msg.CameraInfo` message + + Not intended to be implemented by extending classes. Camera info is stored and provided to extending class + through :py:attr:`.camera_data` property. + + :param msg: Latest :class:`sensor_msgs.msg.CameraInfo` message + """ + self.__camera_info = msg + + @abstractmethod + def image_callback(self, msg: Image) -> None: + """Handles the latest :class:`sensor_msgs.msg.Image` message + + Must be implemented by extending classes (unlike callback for :class:`sensor_msgs.msg.CameraInfo`). + """ + pass + + @property + def img_dim(self) -> Optional[Dim]: + """Image resolution from latest :class:`px4_msgs.msg.CameraInfo` message, None if not available""" + if self.camera_data is not None: + return self.camera_data.dim + else: + self.get_logger().warn('Camera data was not available, returning None as declared image size.') + return None + + @property + def map_size_with_padding(self) -> Optional[Tuple[int, int]]: + """Padded map size tuple (height, width) or None if the information is not available. + + Because the deep learning models used for predicting matching keypoints or poses between camera image frames + and map rasters are not assumed to be rotation invariant in general, the map rasters are rotated based on + camera yaw so that they align with the camera images. To keep the scale of the map after rotation the same, + black corners would appear unless padding is used. Retrieved maps therefore have to be squares with the side + lengths matching the diagonal of the camera frames so that scale is preserved and no black corners appear in + the map rasters after arbitrary 2D rotation. The height and width will both be equal to the diagonal of the + declared (:py:attr:`.img_dim`) camera frame dimensions. + """ + if self.img_dim is None: + self.get_logger().warn(f'Dimensions not available - returning None as map size.') + return None + diagonal = int(np.ceil(np.sqrt(self.img_dim.width ** 2 + self.img_dim.height ** 2))) + assert_type(diagonal, int) + return diagonal, diagonal + + @property + def camera_data(self) -> Optional[CameraData]: + """Camera intrinsics or None if not available""" + if not all(hasattr(self.__camera_info, attr) for attr in ['k', 'height', 'width']): + return None + else: + camera_info = self.__camera_info + return CameraData(camera_info.k.reshape((3, 3)), dim=Dim(camera_info.height, camera_info.width)) diff --git a/gisnav/nodes/base_node.py b/gisnav/nodes/base_node.py deleted file mode 100644 index c1f02a1c..00000000 --- a/gisnav/nodes/base_node.py +++ /dev/null @@ -1,1445 +0,0 @@ -"""Module that contains the BaseNode ROS 2 node.""" -import sys -import rclpy -import traceback -import math -import numpy as np -import cv2 -import importlib -import os -import yaml -import torch - -from abc import ABC, abstractmethod -from multiprocessing.pool import Pool -#from multiprocessing.pool import ThreadPool as Pool # Rename 'Pool' to keep same interface -from typing import Optional, Union, Tuple, get_args, Callable -from rclpy.node import Node -from rcl_interfaces.msg import ParameterDescriptor -from cv_bridge import CvBridge -from scipy.spatial.transform import Rotation -from sensor_msgs.msg import Image - -from gisnav.data import Dim, ImageData, MapData, Attitude, DataValueError, InputData, ImagePair, AsyncPoseQuery, \ - AsyncWMSQuery, ContextualMapData, FixedCamera, Img, Pose, Position, Altitude -from gisnav.geo import GeoPoint, GeoSquare, GeoTrapezoid -from gisnav.assertions import assert_type, assert_len, assert_ndim, assert_shape -from gisnav.pose_estimators.pose_estimator import PoseEstimator -from gisnav.wms import WMSClient -from gisnav.autopilots.autopilot import Autopilot - - -class BaseNode(Node, ABC): - """ROS 2 node that publishes position estimate based on visual match of drone video to map of same location""" - - # Process counts for multiprocessing pools - _WMS_PROCESS_COUNT = 1 # should be 1 - _MATCHER_PROCESS_COUNT = 1 # should be 1 - - # Encoding of input video (input to CvBridge) - # e.g. gscam2 only supports bgr8 so this is used to override encoding in image header - _IMAGE_ENCODING = 'bgr8' - - # Altitude in meters used for DEM request to get local origin elevation - _DEM_REQUEST_ALTITUDE = 100 - - # region ROS Parameter Defaults - ROS_D_WMS_URL = 'http://localhost:80/wms' - """Default WMS server endpoint URL""" - - ROS_D_WMS_VERSION = '1.3.0' - """Default WMS server version""" - - ROS_D_WMS_LAYERS = ['imagery'] - """Default WMS request layers parameter - - .. note:: - The combined layers should cover the flight area of the vehicle at high resolution. Typically this list would - have just one layer for high resolution aerial or satellite imagery. - """ - - ROS_D_WMS_ELEVATION_LAYERS = [] - """Default WMS request elevation layers parameter - - .. note:: - This is an optional elevation layer that makes the pose estimation more accurate especially when flying at low - altitude. It is assumed to be grayscale. - """ - - ROS_D_WMS_STYLES = [''] - """Default WMS request styles parameter - - Must be same length as :py:attr:`.ROS_D_WMS_LAYERS`. Use empty strings for server default styles. - """ - - ROS_D_WMS_SRS = 'EPSG:4326' - """Default WMS request SRS parameter""" - - ROS_D_WMS_REQUEST_TIMEOUT = 10 - """Default WMS client request timeout in seconds""" - - ROS_D_WMS_IMAGE_FORMAT = 'image/jpeg' - """Default WMS client request image format""" - - ROS_D_MISC_AUTOPILOT = 'gisnav.autopilots.px4_micrortps.PX4microRTPS' - """Default autopilot adapter""" - - ROS_D_STATIC_CAMERA = False - """Default value for static camera flag (true for static camera facing down from vehicle body)""" - - ROS_D_MISC_ATTITUDE_DEVIATION_THRESHOLD = 10 - """Magnitude of allowed attitude deviation of estimate from expectation in degrees""" - - ROS_D_MISC_MAX_PITCH = 30 - """Default maximum camera pitch from nadir in degrees for attempting to estimate pose against reference map - - .. seealso:: - :py:attr:`.ROS_D_MAP_UPDATE_MAX_PITCH` - :py:attr:`.ROS_D_MAP_UPDATE_GIMBAL_PROJECTION` - """ - - ROS_D_MISC_MIN_MATCH_ALTITUDE = 80 - """Default minimum ground altitude in meters under which matches against map will not be attempted""" - - ROS_D_MAP_UPDATE_UPDATE_DELAY = 1 - """Default delay in seconds for throttling WMS GetMap requests - - When the camera is mounted on a gimbal and is not static, this delay should be set quite low to ensure that whenever - camera field of view is moved to some other location, the map update request will follow very soon after. The field - of view of the camera projected on ground generally moves *much faster* than the vehicle itself. - - .. note:: - This parameter provides a hard upper limit for WMS GetMap request frequency. Even if this parameter is set low, - WMS GetMap requests will likely be much less frequent because they will throttled by the conditions set in - :meth:`._should_update_map` (private method - see source code for reference). - """ - - ROS_D_MAP_UPDATE_GIMBAL_PROJECTION = True - """Default flag to enable map updates based on expected center of field of view (FOV) projected onto ground - - When this flag is enabled, map rasters are retrieved for the expected center of the camera FOV instead of the - expected position of the vehicle, which increases the chances that the FOV is fully contained in the map raster. - This again increases the chances of getting a good pose estimate. - - .. seealso:: - :py:attr:`.ROS_D_MISC_MAX_PITCH` - :py:attr:`.ROS_D_MAP_UPDATE_MAX_PITCH` - """ - - ROS_D_MAP_UPDATE_MAX_PITCH = 30 - """Default maximum camera pitch from nadir in degrees for attempting to update the stored map - - This limit only applies when camera field of view (FOV) projection is enabled. This value will prevent unnecessary - WMS GetMap requests when the camera is looking far into the horizon and it would be unrealistic to get a good pose - estimate against a map. - - .. seealso:: - :py:attr:`.ROS_D_MISC_MAX_PITCH` - :py:attr:`.ROS_D_MAP_UPDATE_GIMBAL_PROJECTION` - """ - - ROS_D_MAP_UPDATE_MAX_MAP_RADIUS = 400 - """Default maximum map radius (half of map width) in meters for WMS GetMap request bounding boxes - - This limit prevents unintentionally requesting very large maps if camera field of view is projected far into the - horizon. This may happen e.g. if :py:attr:`.ROS_D_MAP_UPDATE_MAX_PITCH` is set too high relative to the camera's - vertical view angle. If the WMS server back-end needs to e.g. piece the large map together from multiple files the - request might timeout in any case. - """ - - ROS_D_MAP_UPDATE_UPDATE_MAP_AREA_THRESHOLD = 0.85 - """Default map bounding box area intersection threshold that prevents a new map from being retrieved - - This prevents unnecessary WMS GetMap requests to replace an old map with a new map that covers almost the same area. - """ - - ROS_D_POSE_ESTIMATOR_CLASS = 'gisnav.pose_estimators.loftr.LoFTRPoseEstimator' - """Default :class:`.PoseEstimator` to use for estimating pose camera pose against reference map""" - - ROS_D_POSE_ESTIMATOR_PARAMS_FILE = 'config/loftr_params.yaml' - """Default parameter file with args for the default :class:`.PoseEstimator`'s :meth:`.PoseEstimator.initializer`""" - - ROS_D_DEBUG_EXPORT_POSITION = '' # 'position.json' - """Default filename for exporting GeoJSON containing estimated field of view and position - - Set to '' to disable - """ - - ROS_D_DEBUG_EXPORT_PROJECTION = '' # 'projection.json' - """Default filename for exporting GeoJSON containing projected field of view (FOV) and FOV center - - Set to '' to disable - """ - - read_only = ParameterDescriptor(read_only=True) - _ROS_PARAMS = [ - ('wms.url', ROS_D_WMS_URL, read_only), - ('wms.version', ROS_D_WMS_VERSION, read_only), - ('wms.layers', ROS_D_WMS_LAYERS), - ('wms.styles', ROS_D_WMS_STYLES), - ('wms.srs', ROS_D_WMS_SRS), - ('wms.request_timeout', ROS_D_WMS_REQUEST_TIMEOUT), - ('misc.autopilot', ROS_D_MISC_AUTOPILOT, read_only), - ('misc.static_camera', ROS_D_STATIC_CAMERA, read_only), - ('misc.attitude_deviation_threshold', ROS_D_MISC_ATTITUDE_DEVIATION_THRESHOLD), - ('misc.max_pitch', ROS_D_MISC_MAX_PITCH), - ('misc.min_match_altitude', ROS_D_MISC_MIN_MATCH_ALTITUDE), - ('map_update.update_delay', ROS_D_MAP_UPDATE_UPDATE_DELAY, read_only), - ('map_update.gimbal_projection', ROS_D_MAP_UPDATE_GIMBAL_PROJECTION), - ('map_update.max_map_radius', ROS_D_MAP_UPDATE_MAX_MAP_RADIUS), - ('map_update.update_map_area_threshold', ROS_D_MAP_UPDATE_UPDATE_MAP_AREA_THRESHOLD), - ('map_update.max_pitch', ROS_D_MAP_UPDATE_MAX_PITCH), - ('pose_estimator.class', ROS_D_POSE_ESTIMATOR_CLASS, read_only), - ('pose_estimator.params_file', ROS_D_POSE_ESTIMATOR_PARAMS_FILE, read_only), - ('debug.export_position', ROS_D_DEBUG_EXPORT_POSITION), - ('debug.export_projection', ROS_D_DEBUG_EXPORT_PROJECTION), - ] - """ROS parameter configuration to declare - - .. note:: - Some parameters are declared read_only and cannot be changed at runtime because there is currently no way to - reinitialize the WMS client, pose estimator, Kalman filter, WMS map update timer, nor the autopilot or ROS - subscriptions. - """ - # endregion - - def __init__(self, name: str, package_share_dir: str) -> None: - """Initializes the ROS 2 node. - - :param name: Name of the node - :param package_share_dir: Package share directory file path - """ - assert_type(name, str) - assert_type(package_share_dir, str) - super().__init__(name, allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) - self.__declare_ros_params() - - self._package_share_dir = package_share_dir - - self._wms_query = None - self._wms_pool = None # Lazy setup - - self._map_update_timer = self._setup_map_update_timer() - - # Setup pose estimation pool - pose_estimator_params_file = self.get_parameter('pose_estimator.params_file').get_parameter_value().string_value - self._pose_estimator, self._pose_estimator_pool = self._setup_pose_estimation_pool(pose_estimator_params_file) - self._pose_estimation_query = None # Must check for None when using this - - # Autopilot bridge - ap = self.get_parameter('misc.autopilot').get_parameter_value().string_value or self.ROS_D_MISC_AUTOPILOT - ap: Autopilot = self._load_autopilot(ap) - # TODO: implement passing init args, kwargs - self._bridge = ap(self, self._image_raw_callback) - - # Converts image_raw to cv2 compatible image - self._cv_bridge = CvBridge() - - # Initialize remaining properties (does not include computed properties) - self._map_input_data = None - self._map_input_data_prev = None - self._fixed_camera_prev = None - # self._image_data = None # Not currently used / needed - self._map_data = None - self._pose_guess = None - self._origin_dem_altitude = None - - # region Properties - @property - def _package_share_dir(self) -> str: - """ROS 2 package share directory""" - return self.__package_share_dir - - @_package_share_dir.setter - def _package_share_dir(self, value: str) -> None: - assert_type(value, str) - self.__package_share_dir = value - - @property - def _pose_guess(self) -> Optional[Pose]: - """Stores rotation and translation vectors for use by :func:`cv2.solvePnPRansac`. - - Assumes previous solution to the PnP problem will be close to the new solution. - """ - return self.__pose_guess - - @_pose_guess.setter - def _pose_guess(self, value: Optional[Pose]) -> None: - assert_type(value, get_args(Optional[Pose])) - self.__pose_guess = value - - @property - def _pose_estimator(self) -> PoseEstimator: - """Dynamically loaded :class:`.PoseEstimator`""" - return self.__pose_estimator - - @_pose_estimator.setter - def _pose_estimator(self, value: Optional[PoseEstimator]) -> None: - assert issubclass(value, get_args(Optional[PoseEstimator])) - self.__pose_estimator = value - - @property - def _wms_pool(self) -> Optional[Pool]: - """Web Map Service client for fetching map rasters.""" - return self.__wms_pool - - @_wms_pool.setter - def _wms_pool(self, value: Optional[Pool]) -> None: - assert_type(value, get_args(Optional[Pool])) - self.__wms_pool = value - - @property - def _wms_query(self) -> Optional[AsyncWMSQuery]: - """Asynchronous results from a WMS client request.""" - return self.__wms_results - - @_wms_query.setter - def _wms_query(self, value: Optional[AsyncWMSQuery]) -> None: - assert_type(value, get_args(Optional[AsyncWMSQuery])) - self.__wms_results = value - - @property - def _map_update_timer(self) -> rclpy.timer.Timer: - """Timer for throttling map update WMS requests.""" - return self.__map_update_timer - - @_map_update_timer.setter - def _map_update_timer(self, value: rclpy.timer.Timer) -> None: - assert_type(value, rclpy.timer.Timer) - self.__map_update_timer = value - - @property - def _pose_estimator_pool(self) -> Pool: - """Pool for running a :class:`.PoseEstimator` in dedicated thread (or process)""" - return self.__pose_estimator_pool - - @_pose_estimator_pool.setter - def _pose_estimator_pool(self, value: Pool) -> None: - assert_type(value, Pool) - self.__pose_estimator_pool = value - - @ property - def _map_data(self) -> Optional[MapData]: - """The map raster from the WMS server response along with supporting metadata.""" - return self.__map_data - - @_map_data.setter - def _map_data(self, value: Optional[MapData]) -> None: - assert_type(value, get_args(Optional[MapData])) - self.__map_data = value - - @property - def _map_input_data(self) -> InputData: - """Inputs stored at time of launching a new asynchronous match that are needed for processing its results.""" - return self.__map_input_data - - @_map_input_data.setter - def _map_input_data(self, value: Optional[InputData]) -> None: - assert_type(value, get_args(Optional[InputData])) - self.__map_input_data = value - - @property - def _pose_estimation_query(self) -> Optional[AsyncPoseQuery]: - """Asynchronous results and input from a pose estimation thread or process.""" - return self.__pose_estimation_query - - @_pose_estimation_query.setter - def _pose_estimation_query(self, value: Optional[AsyncPoseQuery]) -> None: - assert_type(value, get_args(Optional[AsyncPoseQuery])) - self.__pose_estimation_query = value - - @property - def _cv_bridge(self) -> CvBridge: - """CvBridge that decodes incoming PX4-ROS 2 bridge images to cv2 images.""" - return self.__cv_bridge - - @_cv_bridge.setter - def _cv_bridge(self, value: CvBridge) -> None: - assert_type(value, CvBridge) - self.__cv_bridge = value - - @property - def _map_input_data_prev(self) -> Optional[InputData]: - """Previous map input data""" - return self.__map_input_data_prev - - @_map_input_data_prev.setter - def _map_input_data_prev(self, value: Optional[InputData]) -> None: - assert_type(value, get_args(Optional[InputData])) - self.__map_input_data_prev = value - - @property - def _fixed_camera_prev(self) -> Optional[FixedCamera]: - """Previous estimated :class:`.FixedCamera`""" - return self.__fixed_camera_prev - - @_fixed_camera_prev.setter - def _fixed_camera_prev(self, value: Optional[FixedCamera]) -> None: - assert_type(value, get_args(Optional[FixedCamera])) - self.__fixed_camera_prev = value - - @property - def _autopilot(self) -> Autopilot: - """Autopilot bridge adapter""" - return self.__autopilot - - @_autopilot.setter - def _autopilot(self, value: Autopilot) -> None: - assert_type(value, Autopilot) - self.__autopilot = value - - @property - def _origin_dem_altitude(self) -> Optional[float]: - """Elevation layer (DEM) altitude at local frame origin""" - return self.__origin_dem_altitude - - @_origin_dem_altitude.setter - def _origin_dem_altitude(self, value: Optional[float]) -> None: - assert_type(value, get_args(Optional[float])) - self.__origin_dem_altitude = value - # endregion - - # region Computed Properties - @property - def _altitude_scaling(self) -> Optional[float]: - """Returns camera focal length divided by camera altitude in meters.""" - alt_agl = self._bridge.altitude_agl(self._terrain_altitude_amsl_at_position(self._bridge.global_position)) - if self._bridge.camera_data is not None and alt_agl is not None: - return self._bridge.camera_data.fx / alt_agl - else: - self.get_logger().warn('Could not estimate elevation scale because camera focal length and/or vehicle ' - 'altitude is unknown.') - return None - - @property - def _r_guess(self) -> Optional[np.ndarray]: - """Gimbal rotation matrix guess (based on :class:`px4_msgs.GimbalDeviceSetAttitude` message) - - .. note:: - Should be roughly same as rotation matrix stored in :py:attr:`._pose_guess`, even though it is derived via - a different route. If gimbal is not stabilized to its set position, the rotation matrix will be different. - """ - static_camera = self.get_parameter('misc.static_camera').get_parameter_value().bool_value - if self._bridge.gimbal_set_attitude is None: - if not static_camera: - self.get_logger().warn('Gimbal set attitude not available, will not provide pose guess.') - return None - else: - if self._bridge.attitude is not None: - attitude = self._bridge.attitude.as_rotation() - attitude *= Rotation.from_euler('XYZ', [0, -np.pi/2, 0]) - return Attitude(attitude.as_quat()).to_esd().r - else: - self.get_logger().warn('Vehicle attitude not available, will not provide pose guess for static ' - 'camera.') - return None - else: - assert_type(self._bridge.gimbal_set_attitude, Attitude) - gimbal_set_attitude = self._bridge.gimbal_set_attitude.to_esd() # Need coordinates in image frame, not NED - return gimbal_set_attitude.r - - @property - def _wms_results_pending(self) -> bool: - """True if there are pending results""" - return not self._wms_query.result.ready() if self._wms_query is not None else False - - @property - def _is_gimbal_projection_enabled(self) -> bool: - """True if map rasters should be retrieved for projected field of view instead of vehicle position - - If this is set to false, map rasters are retrieved for the vehicle's global position instead. This is typically - fine as long as the camera is not aimed too far in to the horizon and has a relatively wide field of view. For - best results, this should be on to ensure the field of view is fully contained within the area of the retrieved - map raster. - - .. note:: - If you know your camera will be nadir-facing, disabling ``map_update.gimbal_projection`` may improve - performance by a small amount. - """ - gimbal_projection_flag = self.get_parameter('map_update.gimbal_projection').get_parameter_value().bool_value - if type(gimbal_projection_flag) is bool: - return gimbal_projection_flag - else: - # Default behavior (safer) - self.get_logger().warn(f'Could not read gimbal projection flag: {gimbal_projection_flag}. Assume False.') - return False - - @property - def _map_size_with_padding(self) -> Optional[Tuple[int, int]]: - """Padded map size tuple (height, width) or None if the information is not available. - - Because the deep learning models used for predicting matching keypoints or poses between camera image frames - and map rasters are not assumed to be rotation invariant in general, the map rasters are rotated based on - camera yaw so that they align with the camera images. To keep the scale of the map after rotation the same, - black corners would appear unless padding is used. Retrieved maps therefore have to be squares with the side - lengths matching the diagonal of the camera frames so that scale is preserved and no black corners appear in - the map rasters after rotation. The height and width will both be equal to the diagonal of the declared - (:py:attr:`._bridge.camera_data`) camera frame dimensions. - """ - if self._img_dim is None: - self.get_logger().warn(f'Dimensions not available - returning None as map size.') - return None - diagonal = math.ceil(math.sqrt(self._img_dim.width ** 2 + self._img_dim.height ** 2)) - assert_type(diagonal, int) - return diagonal, diagonal - - @property - def _img_dim(self) -> Optional[Dim]: - """Image resolution from latest :class:`px4_msgs.msg.CameraInfo` message, None if not available""" - if self._bridge.camera_data is not None: - return self._bridge.camera_data.dim - else: - self.get_logger().warn('Camera data was not available, returning None as declared image size.') - return None - - @property - def _vehicle_position(self) -> Optional[Position]: - """Vehicle position guess in WGS84 coordinates and altitude in meters above ground, None if not available""" - if self._bridge.global_position is not None: - assert_type(self._bridge.global_position, get_args(Optional[GeoPoint])) - - crs = 'epsg:4326' - if self._bridge.attitude is None: - self.get_logger().warn('Vehicle attitude not yet available, cannot determine vehicle Position.') - return None - - try: - position = Position( - xy=self._bridge.global_position, - altitude=Altitude( - agl=self._bridge.altitude_agl(self._terrain_altitude_amsl_at_position(self._bridge.global_position)), - amsl=self._bridge.altitude_amsl, - ellipsoid=self._bridge.altitude_ellipsoid, - home=self._bridge.altitude_home - ), - attitude=self._bridge.attitude, - timestamp=self._bridge.synchronized_time - ) - return position - except DataValueError as dve: - self.get_logger().warn(f'Error determining vehicle position:\n{dve},\n{traceback.print_exc()}.') - return None - else: - return None - - @property - def _estimation_inputs(self) -> Optional[Tuple[InputData, ContextualMapData]]: - """Returns snapshot of vehicle state and reference map data required for pose estimation - - Pose estimation is asynchronous, so this property provides a way of taking snapshot of the required input - arguments to :meth:`._estimate`. - """ - input_data = InputData( - r_guess=self._r_guess, - snapshot=self._bridge.snapshot - ) - - static_camera = self.get_parameter('misc.static_camera').get_parameter_value().bool_value - - # Get cropped and rotated map - if self._bridge.gimbal_set_attitude is not None and not static_camera: - camera_yaw = self._bridge.gimbal_set_attitude.yaw - assert_type(camera_yaw, float) - assert -np.pi <= camera_yaw <= np.pi, f'Unexpected gimbal yaw value: {camera_yaw} ([-pi, pi] expected).' - else: - static_camera = self.get_parameter('misc.static_camera').get_parameter_value().bool_value - if not static_camera: - self.get_logger().warn(f'Camera yaw unknown, cannot estimate pose.') - return - else: - self.get_logger().debug(f'Assuming zero yaw relative to vehicle body for static nadir-facing camera.') - assert self._bridge.attitude is not None and hasattr(self._bridge.attitude, 'yaw') - camera_yaw = self._bridge.attitude.yaw - - contextual_map_data = ContextualMapData(rotation=camera_yaw, map_data=self._map_data, crop=self._img_dim, - altitude_scaling=self._altitude_scaling) - return input_data, contextual_map_data - # endregion - - # region Initialization - def __declare_ros_params(self) -> None: - """Declares ROS parameters""" - # Need to declare parameters one by one since declare_parameters will not declare remaining parameters if it - # raises a ParameterAlreadyDeclaredException - for param_tuple in self._ROS_PARAMS: - try: - self.declare_parameter(*param_tuple) - self.get_logger().debug(f'Using default value {param_tuple[1]} for ROS parameter {param_tuple[0]}') - except rclpy.exceptions.ParameterAlreadyDeclaredException as _: - # This means parameter is declared from YAML file - pass - - def _setup_wms_pool(self) -> Pool: - """Returns WMS pool - - .. note:: - Declare ROS parameters before calling this method - """ - self.get_logger().info('Setting up WMS pool.') - url = self.get_parameter('wms.url').get_parameter_value().string_value - version = self.get_parameter('wms.version').get_parameter_value().string_value - timeout = self.get_parameter('wms.request_timeout').get_parameter_value().integer_value - assert_type(url, str) - assert_type(version, str) - assert_type(timeout, int) - pool = Pool(self._WMS_PROCESS_COUNT, initializer=WMSClient.initializer, - initargs=(url, version, timeout)) - - return pool - - def _setup_pose_estimation_pool(self, params_file: str) -> Tuple[type, Pool]: - """Returns the pose estimator type along with an initialized pool - - :param params_file: Parameter file with pose estimator full class path and initialization arguments - :return: Tuple containing the class type and the initialized pose estimation pool - """ - # Use 'spawn', see: https://pytorch.org/docs/stable/notes/multiprocessing.html#cuda-in-multiprocessing - try: - torch.multiprocessing.set_start_method('spawn') - except RuntimeError as _: - # context has already been set - pass - pose_estimator_params = self._load_config(params_file) - module_name, class_name = pose_estimator_params.get('class_name', '').rsplit('.', 1) - pose_estimator = self._import_class(class_name, module_name) - pose_estimator_pool = Pool(self._MATCHER_PROCESS_COUNT, initializer=pose_estimator.initializer, - initargs=(pose_estimator, *pose_estimator_params.get('args', []),)) # TODO: handle missing args, do not use default value - - return pose_estimator, pose_estimator_pool - - def _load_config(self, yaml_file: str) -> dict: - """Loads config from the provided YAML file - - :param yaml_file: Path to the yaml file - :return: The loaded yaml file as dictionary - """ - assert_type(yaml_file, str) - with open(os.path.join(self._package_share_dir, yaml_file), 'r') as f: - # noinspection PyBroadException - try: - config = yaml.safe_load(f) - self.get_logger().info(f'Loaded config:\n{config}.') - return config - except Exception as e: - self.get_logger().error(f'Could not load config file {yaml_file} because of unexpected exception.') - raise - - def _setup_map_update_timer(self) -> rclpy.timer.Timer: - """Sets up a timer to throttle map update requests - - Initially map updates were triggered in VehicleGlobalPosition message callbacks, but were moved to a separate - timer since map updates may be needed even if the EKF2 filter does not publish a global position reference (e.g. - when GPS fusion is turned off in the EKF2_AID_MASK parameter). - - :return: The timer instance - """ - timer_period = self.get_parameter('map_update.update_delay').get_parameter_value().integer_value - assert_type(timer_period, int) - if not 0 <= timer_period: - error_msg = f'Map update delay must be >0 seconds ({timer_period} provided).' - self.get_logger().error(error_msg) - raise ValueError(error_msg) - timer = self.create_timer(timer_period, self._map_update_timer_callback) - return timer - - def _map_update_timer_callback(self) -> None: - """Attempts to update the stored map at regular intervals - - Also gets DEM for local frame origin if needed (see :meth:`._should_request_dem_ - - Calls :meth:`._update_map` if the center and altitude coordinates for the new map raster are available and the - :meth:`._should_update_map` check passes. - - New map is retrieved based on a guess of the vehicle's global position. If - :py:attr:`._is_gimbal_projection_enabled`, the center of the projected camera field of view is used instead of - vehicle position to ensure the field of view is best contained in the new map raster. - """ - if self._should_request_dem_for_local_frame_origin(): - assert self._vehicle_position is not None - map_radius = self._get_dynamic_map_radius(self._DEM_REQUEST_ALTITUDE) - map_candidate = GeoSquare(self._vehicle_position.xy, map_radius) - self._wms_query = self._request_new_map(map_candidate) - - if self._vehicle_position is None: - self.get_logger().warn(f'Could not determine vehicle approximate global position and therefore cannot ' - f'update map.') - return - - if self._is_gimbal_projection_enabled: - projected_center = self._guess_fov_center(self._vehicle_position) - if projected_center is None: - self.get_logger().warn('Could not project field of view center. Using vehicle position for map center ' - 'instead.') - else: - projected_center = None - - map_update_altitude = self._bridge.altitude_agl(self._terrain_altitude_amsl_at_position(self._bridge.global_position)) - if map_update_altitude is None: - self.get_logger().warn('Cannot determine altitude AGL, skipping map update.') - return None - if map_update_altitude <= 0: - self.get_logger().warn(f'Map update altitude {map_update_altitude} should be > 0, skipping map update.') - return None - map_radius = self._get_dynamic_map_radius(map_update_altitude) - map_candidate = GeoSquare(projected_center if projected_center is not None else self._vehicle_position.xy, - map_radius) - - if self._should_request_new_map(map_candidate): - self._wms_query = self._request_new_map(map_candidate) - else: - self.get_logger().debug('Needed map not different enough to request new map yet, ' - 'or previous results are not ready.') - - def _import_class(self, class_name: str, module_name: str) -> type: - """Dynamically imports class from given module if not yet imported - - :param class_name: Name of the class to import - :param module_name: Name of module that contains the class - :return: Imported class - """ - if module_name not in sys.modules: - self.get_logger().info(f'Importing module {module_name}.') - importlib.import_module(module_name) - imported_class = getattr(sys.modules[module_name], class_name, None) - assert imported_class is not None, f'{class_name} was not found in module {module_name}.' - return imported_class - - def _load_autopilot(self, autopilot: str) -> Callable: - """Returns :class:`.Autopilot` instance from provided class path - - :param autopilot: Full path of :class:`.Autopilot` adapter class - :return: Initialized :class:`.Autopilot` instance - """ - assert_type(autopilot, str) - module_name, class_name = autopilot.rsplit('.', 1) - class_ = self._import_class(class_name, module_name) - return class_ - # endregion - - #region WMSWorkerCallbacks - def _wms_pool_worker_callback(self, result: Tuple[np.ndarray, Optional[np.ndarray]]) -> None: - """Handles result from :meth:`gisnav.wms.worker`. - - Saves received result to :py:attr:`~_map_data. The result should be a collection containing a single - :class:`~data.MapData`. - - .. note:: - The result could be either from a regular map update request, or it could be a DEM for local frame origin - - :param result: Results from the asynchronous call - :return: - """ - map_, elevation = result - assert_type(map_, np.ndarray) - if elevation is not None: - assert_ndim(elevation, 2) - assert_shape(elevation, map_.shape[0:2]) - - # Should already have received camera info so _map_size_with_padding should not be None - assert map_.shape[0:2] == self._map_size_with_padding, 'Decoded map is not of specified size.' - - elevation = Img(elevation) if elevation is not None else None - map_data = MapData(bbox=self._wms_query.geobbox, image=Img(map_), elevation=elevation) - self.get_logger().info(f'Map received for bbox: {map_data.bbox.bounds}.') - - self._map_data = map_data - - # TODO: assumes that this local_frame_origin is the starting location, same that was used for the request - # --> not strictly true even if it works for the simulation - # DO AFTER MAP DATA IS SET - terrain_altitude_at_position needs it - if self._origin_dem_altitude is None and self._elevation_enabled(): - # TODO: assumes that local frame origin is withing this map, not necessarily true since same callback is used - # --> Have separate WMS callback for local origin DEM ref request - local_frame_origin = self._bridge.local_frame_origin - if local_frame_origin is not None: - self._origin_dem_altitude = self._terrain_altitude_at_position(local_frame_origin.xy) - - - def _wms_pool_worker_error_callback(self, e: BaseException) -> None: - """Handles errors from WMS pool worker. - - :param e: Exception returned by the worker - :return: - """ - # TODO: handle IOError separately? - # These are *probably* connection related exceptions from requests library. They do not seem to be part of - # OWSLib public API so WMSClient does not handle them (in case OWSLib devs change it). Handling them would - # require direct dependency to requests. Log exception as error here and move on. - self.get_logger().error(f'Something went wrong with WMS process:\n{e},\n{traceback.print_exc()}.') - #endregion - - def _image_raw_callback(self, msg: Image) -> None: - """Handles latest :class:`px4_msgs.msg.Image` message - - :param msg: The :class:`px4_msgs.msg.Image` message from the PX4-ROS 2 bridge - """ - # Estimate EKF2 timestamp first to get best estimate - if self._bridge.synchronized_time is None: - self.get_logger().warn('Image frame received but could not estimate EKF2 system time, skipping frame.') - return None - - assert_type(msg, Image) - cv_image = self._cv_bridge.imgmsg_to_cv2(msg, self._IMAGE_ENCODING) - - # Check that image dimensions match declared dimensions - if self._img_dim is not None: - cv_img_shape = cv_image.shape[0:2] - assert cv_img_shape == self._img_dim, f'Converted cv_image shape {cv_img_shape} did not match '\ - f'declared image shape {self._img_dim}.' - - if self._bridge.camera_data is None: - self.get_logger().warn('Camera data not yet available, skipping frame.') - return None - - image_data = ImageData(image=Img(cv_image), frame_id=msg.header.frame_id, - timestamp=self._bridge.synchronized_time, camera_data=self._bridge.camera_data) - - if self._should_estimate(image_data.image.arr): - assert self._pose_estimation_query is None or self._pose_estimation_query.result.ready() - assert self._map_data is not None - assert hasattr(self._map_data, 'image'), 'Map data unexpectedly did not contain the image data.' - - inputs, contextual_map_data = self._estimation_inputs - self._map_input_data = inputs - image_pair = ImagePair(image_data, contextual_map_data) - self._estimate(image_pair, inputs) - - # region Map Updates - def _elevation_enabled(self) -> bool: - """Returns True if DEM is in use - - :param: True if elevation_layers ROS 2 param is set - """ - elevation_layers = self.get_parameter('wms.elevation_layers').get_parameter_value().string_array_value - return elevation_layers and not (len(elevation_layers) == 1 and not elevation_layers[0]) - - def _terrain_altitude_at_position(self, position: Optional[GeoPoint]) -> Optional[float]: - """Raw terrain altitude from DEM if available, or None if not available - - :param position: Position to query - :return: Raw altitude in DEM coordinate frame and units - """ - if self._elevation_enabled() and self._map_data is not None and position is not None: - elevation = self._map_data.elevation.arr - bbox = self._map_data.bbox - polygon = bbox._geoseries[0] - # position = self._bridge.global_position - point = position._geoseries[0] - - if polygon.contains(point): - h, w = elevation.shape[0:2] - assert h, w == self._img_dim - left, bottom, right, top = bbox.bounds - x = w * (position.lon - left) / (right - left) - y = h * (position.lat - bottom) / (top - bottom) - try: - dem_elevation = elevation[int(np.floor(y)), int(np.floor(x))] - except IndexError as _: - # TODO: might be able to handle this - self.get_logger().warn('Position seems to be outside current elevation raster, cannot compute ' - 'terrain altitude.') - return None - - return float(dem_elevation) - else: - # Should not happen - self.get_logger().warn('Did not have elevation raster for current location or local frame origin ' - 'altitude was unknwon, cannot compute terrain altitude.') - return None - - return None - - def _terrain_altitude_amsl_at_position(self, position: Optional[GeoPoint]): - """Terrain altitude in meters AMSL accroding to DEM if available, or None if not available - - :param position: Position to query - :return: Terrain altitude AMSL in meters at position - """ - dem_elevation = self._terrain_altitude_at_position(position) - local_frame_origin = self._bridge.local_frame_origin - if dem_elevation is not None and self._origin_dem_altitude is not None and local_frame_origin is not None: - elevation_relative = dem_elevation - self._origin_dem_altitude - elevation_amsl = elevation_relative + local_frame_origin.altitude.amsl - return float(elevation_amsl) - - return None - - def _guess_fov_center(self, origin: Position) -> Optional[GeoPoint]: - """Guesses WGS84 coordinates of camera field of view (FOV) projected on ground from given origin - - Triggered by :meth:`._map_update_timer_callback` when gimbal projection is enabled to determine center - coordinates for next WMS GetMap request. - - :param origin: Camera position - :return: Center of the projected FOV, or None if not available - """ - static_camera = self.get_parameter('misc.static_camera').get_parameter_value().bool_value - - if self._bridge.gimbal_set_attitude is None: - if not static_camera: - self.get_logger().warn('Gimbal set attitude not available, cannot project gimbal FOV.') - return None - else: - if self._bridge.attitude is not None: - attitude = self._bridge.attitude.as_rotation() - attitude *= Rotation.from_euler('XYZ', [0, -np.pi/2, 0]) - gimbal_set_attitude = Attitude(attitude.as_quat()).to_esd().r - else: - self.get_logger().warn('Vehicle attitude not available, will not provide pose guess for static ' - 'camera.') - return None - else: - assert_type(self._bridge.gimbal_set_attitude, Attitude) - gimbal_set_attitude = self._bridge.gimbal_set_attitude.to_esd() # Need coordinates in image frame, not NED - - assert gimbal_set_attitude is not None - - if self._bridge.camera_data is None: - self.get_logger().warn('Camera data not available, could not create a mock pose to generate a FOV guess.') - return None - - translation = -gimbal_set_attitude.r @ np.array([self._bridge.camera_data.cx, self._bridge.camera_data.cy, - -self._bridge.camera_data.fx]) - - try: - pose = Pose(gimbal_set_attitude.r, translation.reshape((3, 1))) - except DataValueError as e: - self.get_logger().warn(f'Pose input values: {gimbal_set_attitude.r}, {translation} were invalid: {e}.') - return None - - try: - mock_fixed_camera = FixedCamera(pose=pose, image_pair=self._mock_image_pair(origin), - snapshot=self._bridge.snapshot, - timestamp=self._bridge.synchronized_time) - except DataValueError as _: - self.get_logger().warn(f'Could not create a valid mock projection of FOV.') - return None - - if __debug__: - export_projection = self.get_parameter('debug.export_projection').get_parameter_value().string_value - if export_projection != '': - self._export_position(mock_fixed_camera.fov.c, mock_fixed_camera.fov.fov, export_projection) - - return mock_fixed_camera.fov.fov.to_crs('epsg:4326').center - - def _request_new_map(self, bbox: GeoSquare) -> AsyncWMSQuery: - """Instructs the WMS client to request a new map from the WMS server - - :param bbox: Bounding box of map requested map - """ - if self._map_size_with_padding is None: - self.get_logger().warn('Map size not yet available - skipping WMS request.') - return - - if self._wms_pool is None: - self._wms_pool = self._setup_wms_pool() - - # Build and send WMS request - layers = self.get_parameter('wms.layers').get_parameter_value().string_array_value - elevation_layers = self.get_parameter('wms.elevation_layers').get_parameter_value().string_array_value - styles = self.get_parameter('wms.styles').get_parameter_value().string_array_value - srs_str = self.get_parameter('wms.srs').get_parameter_value().string_value - image_format = self.get_parameter('wms.image_format').get_parameter_value().string_value - assert all(isinstance(x, str) for x in layers) - assert all(isinstance(x, str) for x in styles) - assert_len(styles, len(layers)) - assert_type(srs_str, str) - assert_type(image_format, str) - assert self._wms_query is None or self._wms_query.result.ready(), f'New map was requested while previous ' \ - f'results were not yet ready.' - self.get_logger().info(f'Requesting map for bbox: {bbox.bounds}, layers: {layers}, srs: {srs_str}, format: ' - f'{image_format}.') - args = [layers, styles, bbox.bounds, self._map_size_with_padding, srs_str, image_format] - if len(elevation_layers) > 0: - args.append(elevation_layers) - - return AsyncWMSQuery( - result=self._wms_pool.apply_async( - WMSClient.worker, - tuple(args), - callback=self._wms_pool_worker_callback, - error_callback=self._wms_pool_worker_error_callback - ), - geobbox=bbox - ) - - def _previous_map_too_close(self, bbox: GeoSquare) -> bool: - """Returns True previous map is too close to new requested one - - This check is made to avoid retrieving a new map that is almost the same as the previous map. Relaxing map - update constraints should not improve accuracy of position estimates unless the map is so old that the field of - view either no longer completely fits inside (vehicle has moved away or camera is looking in other direction) - or is too small compared to the size of the map (vehicle altitude has significantly decreased). - - :param bbox: Bounding box of new map candidate - :return: True if previous map is too close. - """ - assert_type(bbox, GeoSquare) - if self._map_data is not None: - previous_map_data = self._map_data - area_threshold = self.get_parameter('map_update.update_map_area_threshold').get_parameter_value().double_value - intersection1 = bbox.intersection(previous_map_data.bbox).area / bbox.area - intersection2 = previous_map_data.bbox.intersection(bbox).area / previous_map_data.bbox.area - ratio = min(intersection1, intersection2) - - if ratio > area_threshold: - return True - - return False - - def _should_request_new_map(self, bbox: GeoSquare) -> bool: - """Returns True if a new map should be requested to replace the old map - - Map is updated unless (1) there is a previous map that is close enough to provided center and has radius - that is close enough to new request, (2) previous WMS request is still processing, or (3) camera pitch is too - large and gimbal projection is used so that map center would be too far or even beyond the horizon. - - :param bbox: Bounding box of new map candidate - :return: True if new map should be requested - """ - assert_type(bbox, GeoSquare) - - if self._wms_results_pending: - return False - - if self._previous_map_too_close(bbox): - return False - - 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_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 - - return True - - def _should_request_dem_for_local_frame_origin(self) -> bool: - """Returns True if a new map should be requested to determine elevation value for local frame origin - - DEM value for local frame origin is needed if elevation layer is used in order to determine absolute altitude - of altitude estimates (GISNav estimates altitude against DEM, or assumes altitude at 0 if no DEM is provided). - - :return: True if new map should be requested - """ - elevation_layers = self.get_parameter('wms.elevation_layers').get_parameter_value().string_array_value - if not elevation_layers or (len(elevation_layers) == 1 and not elevation_layers[0]): - return False - - if self._origin_dem_altitude is not None: - return False - - if self._bridge.local_frame_origin is None: - return False - - if self._wms_results_pending: - self.get_logger().warn(f'Should request DEM to determine local frame origin altitude but WMS pool has ' - f'results pending.') - return False - - return False - - def _get_dynamic_map_radius(self, altitude: Union[int, float]) -> int: - """Returns map radius that adjusts for camera altitude to be used for new map requests - - :param altitude: Altitude of camera in meters - :return: Suitable map radius in meters - """ - assert_type(altitude, get_args(Union[int, float])) - max_map_radius = self.get_parameter('map_update.max_map_radius').get_parameter_value().integer_value - - if self._bridge.camera_data is not None: - hfov = 2 * math.atan(self._bridge.camera_data.dim.width / (2 * self._bridge.camera_data.fx)) - map_radius = 1.5*hfov*altitude # Arbitrary padding of 50% - else: - # Update map before CameraInfo has been received - self.get_logger().warn(f'Could not get camera data, using guess for map width.') - map_radius = 3*altitude # Arbitrary guess - - if map_radius > max_map_radius: - self.get_logger().warn(f'Dynamic map radius {map_radius} exceeds max map radius {max_map_radius}, using ' - f'max radius {max_map_radius} instead.') - map_radius = max_map_radius - - return map_radius - # endregion - - # region Mock Image Pair - def _mock_image_pair(self, origin: Position) -> Optional[ImagePair]: - """Creates mock :class:`.ImagePair` for guessing projected FOV needed for map requests, or None if not available - - The mock image pair will be paired with a pose guess to compute the expected field of view. The expected field - of view is used to request a new map that overlaps with what the camera is looking at. - - .. seealso: - :meth:`._mock_map_data` and :meth:`._mock_image_data` - - :param origin: Vehicle position - :return: Mock image pair that can be paired with a pose guess to generate a FOV guess, or None if not available - """ - image_data = self._mock_image_data() - map_data = self._mock_map_data(origin) - if image_data is None or map_data is None: - self.get_logger().warn('Missing required inputs for generating mock image PAIR.') - return None - contextual_map_data = ContextualMapData(rotation=0, crop=image_data.image.dim, map_data=map_data, - mock_data=True) - image_pair = ImagePair(image_data, contextual_map_data) - return image_pair - - # TODO: make property? - def _mock_image_data(self) -> Optional[ImageData]: - """Creates mock :class:`.ImageData` for guessing projected FOV for map requests, or None if not available - - .. seealso: - :meth:`._mock_map_data` and :meth:`._mock_image_pair` - """ - if self._img_dim is None or self._bridge.synchronized_time is None or self._bridge.camera_data is None: - self.get_logger().warn('Missing required inputs for generating mock image DATA.') - return None - - image_data = ImageData(image=Img(np.zeros(self._img_dim)), - frame_id='mock_image_data', # TODO - timestamp=self._bridge.synchronized_time, - camera_data=self._bridge.camera_data) - return image_data - - def _mock_map_data(self, origin: Position) -> Optional[MapData]: - """Creates mock :class:`.MapData` for guessing projected FOV needed for map requests, or None if not available - - The mock image pair will be paired with a pose guess to compute the expected field of view. The expected field - of view is used to request a new map that overlaps with what the camera is looking at. - - .. seealso: - :meth:`._mock_image_pair` and :meth:`._mock_image_data` - - :param origin: Vehicle position - :return: Mock map data with mock images but with real expected bbox, or None if not available - """ - assert_type(origin, Position) - if self._bridge.camera_data is None or self._map_size_with_padding is None: - self.get_logger().warn('Missing required inputs for generating mock MAP DATA.') - return None - - # Scaling factor of image pixels := terrain_altitude - scaling = (self._map_size_with_padding[0]/2) / self._bridge.camera_data.fx - altitude = self._bridge.altitude_agl(self._terrain_altitude_amsl_at_position(self._bridge.global_position)) - if altitude is None: - self.get_logger().warn('Cannot determine altitude AGL, skipping mock map data.') - return - if altitude < 0: - self.get_logger().warn(f'Altitude AGL {altitude} was negative, skipping mock map data.') - return - radius = scaling * altitude - - assert_type(origin.xy, GeoPoint) - bbox = GeoSquare(origin.xy, radius) - map_data = MapData(bbox=bbox, image=Img(np.zeros(self._map_size_with_padding))) - return map_data - # endregion - - # region Pose Estimation Callbacks - def _pose_estimation_worker_error_callback(self, e: BaseException) -> None: - """Error callback for matching worker""" - self.get_logger().error(f'Pose estimator encountered an unexpected exception:\n{e}\n{traceback.print_exc()}.') - - def _pose_estimation_worker_callback(self, result: Optional[Pose]) -> None: - """Callback for :meth:`.PoseEstimator.worker` - - Retrieves latest :py:attr:`._pose_estimation_query.input_data` and uses it to call :meth:`._compute_output`. - The input data is needed so that the post-processing is done using the same state information that was used for - initiating the pose estimation in the first place. For example, camera pitch may have changed since then, - and current camera pitch should therefore not be used for processing the matches. - - :param result: Pose result from WMS worker, or None if pose could not be estimated - """ - if result is not None: - try: - pose = Pose(*result) - - if self._pose_estimation_query.image_pair.ref.elevation is not None: - # Compute DEM value at estimated position - # This is in camera intrinsic (pixel) units with origin at whatever the DEM uses - # For example, the USGS DEM uses NAVD 88 - x, y = -pose.t.squeeze()[0:2] - x, y = int(x), int(y) - elevation = self._pose_estimation_query.image_pair.ref.elevation.arr[y, x] - pose = Pose(pose.r, pose.t - elevation) - except DataValueError as _: - self.get_logger().warn(f'Estimated pose was not valid, skipping this frame.') - return None - except IndexError as __: - # TODO: might be able to handle this - self.get_logger().warn(f'Estimated pose was not valid, skipping this frame.') - return None - - self._pose_guess = pose - else: - self.get_logger().warn(f'Worker did not return a pose, skipping this frame.') - return None - - try: - image_pair = self._pose_estimation_query.image_pair - input_data = self._pose_estimation_query.input_data - fixed_camera = FixedCamera(pose=pose, image_pair=image_pair, snapshot=input_data.snapshot, - timestamp=image_pair.qry.timestamp) - except DataValueError as _: - self.get_logger().warn(f'Could not estimate a valid camera position, skipping this frame.') - return None - - if not self._is_valid_estimate(fixed_camera, self._pose_estimation_query.input_data): - self.get_logger().warn('Estimate did not pass post-estimation validity check, skipping this frame.') - return None - - assert fixed_camera is not None - # noinspection PyUnreachableCode - if __debug__: - # Visualize projected FOV estimate - fov_pix = fixed_camera.fov.fov_pix - ref_img = fixed_camera.image_pair.ref.image.arr - map_with_fov = cv2.polylines(ref_img.copy(), - [np.int32(fov_pix)], True, - 255, 3, cv2.LINE_AA) - - img = np.vstack((map_with_fov, fixed_camera.image_pair.qry.image.arr)) - cv2.imshow("Projected FOV", img) - cv2.waitKey(1) - - # Export GeoJSON - export_geojson = self.get_parameter('debug.export_position').get_parameter_value().string_value - if export_geojson != '': - self._export_position(fixed_camera.position.xy, fixed_camera.fov.fov, export_geojson) - - self.publish(fixed_camera) - - self._map_input_data_prev = self._map_input_data - self._fixed_camera_prev = fixed_camera - # endregion - - # region Pose Estimation - 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 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 - """ - # Check condition (1) - that _map_data exists - if self._map_data is None: - self.get_logger().debug(f'No reference map available. Skipping pose estimation.') - return False - - # Check condition (2) - that a request is not already running - if not (self._pose_estimation_query is None or self._pose_estimation_query.result.ready()): - self.get_logger().debug(f'Previous pose estimation results pending. Skipping pose estimation.') - return False - - # 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_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 - min_alt = self.get_parameter('misc.min_match_altitude').get_parameter_value().integer_value - altitude = self._bridge.altitude_agl(self._terrain_altitude_amsl_at_position(self._bridge.global_position)) - if altitude is None: - self.get_logger().warn('Cannot determine altitude AGL, skipping map update.') - return None - if not isinstance(min_alt, int) or altitude is None or altitude < min_alt: - self.get_logger().warn(f'Assumed altitude {altitude} was lower than minimum threshold for matching ' - f'({min_alt}) or could not be determined. Skipping pose estimation.') - return False - - return True - - def _estimate(self, image_pair: ImagePair, input_data: InputData) -> None: - """Instructs the pose estimator to estimate the pose between the image pair - - :param image_pair: The image pair to estimate the pose from - :param input_data: Input data context - :return: - """ - assert self._pose_estimation_query is None or self._pose_estimation_query.result.ready() - pose_guess = None if self._pose_guess is None else tuple(self._pose_guess) - - # Scale elevation raster from meters to camera native pixels - elevation = image_pair.ref.elevation.arr if image_pair.ref.elevation is not None else None - if elevation is not None: - elevation = elevation * image_pair.ref.altitude_scaling if image_pair.ref.altitude_scaling is not None \ - else None - - self._pose_estimation_query = AsyncPoseQuery( - result=self._pose_estimator_pool.apply_async( - self._pose_estimator.worker, - (image_pair.qry.image.arr, image_pair.ref.image.arr, image_pair.qry.camera_data.k, pose_guess, - elevation), - callback=self._pose_estimation_worker_callback, - error_callback=self._pose_estimation_worker_error_callback - ), - image_pair=image_pair, - input_data=input_data - ) - - def _is_valid_estimate(self, fixed_camera: FixedCamera, input_data: InputData) -> bool: - """Returns True if the estimate is valid - - Compares computed estimate to guess based on set gimbal device attitude. This will reject estimates made when - the gimbal was not stable (which is strictly not necessary), which is assumed to filter out more inaccurate - estimates. - """ - static_camera = self.get_parameter('misc.static_camera').get_parameter_value().bool_value - if static_camera: - vehicle_attitude = self._bridge.attitude - if vehicle_attitude is None: - self.get_logger().warn('Gimbal attitude was not available, cannot do post-estimation validity check' - 'for static camera.') - return False - - # Add vehicle roll & pitch (yaw handled separately through map rotation) - #r_guess = Attitude(Rotation.from_rotvec([vehicle_attitude.roll, vehicle_attitude.pitch - np.pi/2, 0]) - # .as_quat()).to_esd().as_rotation() - r_guess = Attitude(Rotation.from_euler('XYZ', [vehicle_attitude.roll, vehicle_attitude.pitch - np.pi / 2, - 0]).as_quat()).to_esd().as_rotation() - - if input_data.r_guess is None and not static_camera: - self.get_logger().warn('Gimbal attitude was not available, cannot do post-estimation validity check.') - return False - - if not static_camera: - r_guess = Rotation.from_matrix(input_data.r_guess) - # Adjust for map rotation - camera_yaw = fixed_camera.image_pair.ref.rotation - camera_yaw = Rotation.from_euler('xyz', [0, 0, camera_yaw], degrees=False) - r_guess *= camera_yaw - - r_estimate = Rotation.from_matrix(fixed_camera.pose.r) - - magnitude = Rotation.magnitude(r_estimate * r_guess.inv()) - - threshold = self.get_parameter('misc.attitude_deviation_threshold').get_parameter_value().integer_value - threshold = np.radians(threshold) - - if magnitude > threshold: - self.get_logger().warn(f'Estimated rotation difference to expected was too high (magnitude ' - f'{np.degrees(magnitude)}).') - return False - - #roll = r_guess.as_euler('xyz', degrees=True)[0] - #if roll > threshold/2: # TODO: have separate configurable threshold - # self.get_logger().warn(f'Estimated roll difference to expected was too high (magnitude ' - # f'{roll}).') - # return False - - return True - - - # endregion - - # region Shared Logic - 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 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 - - :param max_pitch: The limit for the pitch in degrees from nadir over which it will be considered too high - :return: True if pitch is too high - """ - assert_type(max_pitch, get_args(Union[int, float])) - 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: - if not static_camera: - self.get_logger().warn('Gimbal attitude was not available, assuming camera pitch too high.') - return True - else: - if self._bridge.attitude is None: - self.get_logger().warn('Vehicle attitude was not available, assuming static camera pitch too high.') - return True - else: - pitch = max(self._bridge.attitude.pitch, self._bridge.attitude.roll) - - assert pitch is not None - if pitch > max_pitch: - self.get_logger().warn(f'Camera pitch {pitch} is above limit {max_pitch}.') - return True - - return False - - def _export_position(self, position: GeoPoint, fov: GeoTrapezoid, filename: str) -> None: - """Exports the computed position and field of view (FOV) into a geojson file - - The GeoJSON file is not used by the node but can be accessed by GIS software to visualize the data it contains. - - :param position: Computed camera position or projected principal point for gimbal projection - :param: fov: Field of view of camera - :param filename: Name of file to write into - :return: - """ - assert_type(position, GeoPoint) - assert_type(fov, GeoTrapezoid) - assert_type(filename, str) - try: - position._geoseries.append(fov._geoseries).to_file(filename) - except Exception as e: - self.get_logger().error(f'Could not write file {filename} because of exception:' - f'\n{e}\n{traceback.print_exc()}') - # endregion - - # region PublicAPI - @abstractmethod - def publish(self, position: Position) -> None: - """Publishes the estimated position - - This method should be implemented by the extending class to adapt the base node for any given use case. - - :param position: Visually estimated position and attitude - """ - pass - - def terminate_pools(self) -> None: - """Terminates the WMS and pose estimator pools - - .. note:: - Call this method after :meth:`.destroy_timers` and before destroying your node and shutting down for a - clean exit. - """ - if self._pose_estimator_pool is not None: - self.get_logger().info('Terminating pose estimator pool.') - self._pose_estimator_pool.terminate() - - if self._wms_pool is not None: - self.get_logger().info('Terminating WMS pool.') - self._wms_pool.terminate() - - def destroy_timers(self) -> None: - """Destroys the map update timer - - .. note:: - Call this method before destroying your node and before :meth:`.terminate_pools` and shutting down for a - clean exit. - """ - if self._map_update_timer is not None: - self.get_logger().info('Destroying map update timer.') - self._map_update_timer.destroy() - - def unsubscribe_topics(self) -> None: - """Unsubscribes from all ROS topics - - .. note:: - Call this method when before destroying your node for a clean exit. - """ - self._bridge.unsubscribe_all() - # endregion diff --git a/gisnav/nodes/bbox_node.py b/gisnav/nodes/bbox_node.py new file mode 100644 index 00000000..e980ce28 --- /dev/null +++ b/gisnav/nodes/bbox_node.py @@ -0,0 +1,331 @@ +"""A node that publishes bounding box of field of view projected to ground from vehicle approximate location""" +from typing import Optional + +import numpy as np +from rclpy.qos import QoSPresetProfiles +from geographic_msgs.msg import BoundingBox, GeoPoint, GeoPointStamped, GeoPoseStamped +from mavros_msgs.msg import Altitude +from geometry_msgs.msg import Quaternion +from sensor_msgs.msg import Image + +from . import messaging +from .base.camera_subscriber_node import CameraSubscriberNode +from ..data import BBox, Pose, FixedCamera, Attitude, DataValueError, MapData, ImagePair, ImageData, Img, \ + ContextualMapData +from ..geo import GeoPt as GeoPt, GeoSquare, get_dynamic_map_radius + + +class BBoxNode(CameraSubscriberNode): + """A node that publishes bounding box that matches camera field of view projected to ground + + This is the suggested bounding box for the next map update, or "approximate location" of what the camera is seeing. + + .. note:: + :class:`.KeypointPoseEstimator` takes elevation data into account if DEMs are provided, but the projection + logic here currently assumes planar terrain regardless of whether a DEM is provided via :class:`.MapNode`. + """ + ROS_D_GIMBAL_PROJECTION = True + """Default flag to enable map updates based on expected center of field of view (FOV) projected onto ground + + When this flag is enabled, map rasters are retrieved for the expected camera FOV instead of the expected position + of the vehicle, which increases the chances that the FOV is fully contained in the map raster. This again increases + the chances of getting a good pose estimate. + """ + + ROS_D_MAX_MAP_RADIUS = 400 + """Default maximum map radius for used maps""" + + ROS_D_DEBUG_EXPORT_PROJECTION = '' # 'projection.json' + """Default filename for exporting GeoJSON containing projected field of view (FOV) and FOV center + + Set to '' to disable + + .. note:: + Exporting the projection as GeoJSON and viewing it in a GIS software may be useful for e.g. debugging + """ + + ROS_PARAM_DEFAULTS = [ + ('gimbal_projection', ROS_D_GIMBAL_PROJECTION, False), + ('max_map_radius', ROS_D_MAX_MAP_RADIUS, False), + ('export_projection', ROS_D_DEBUG_EXPORT_PROJECTION, False), + ] + """List containing ROS parameter name, default value and read_only flag tuples""" + + def __init__(self, name: str): + """Class initializer + + :param name: Node name + """ + super().__init__(name) + + # Subscribers + self._vehicle_geopose = None + self._vehicle_geopose_sub = self.create_subscription(GeoPoseStamped, + messaging.ROS_TOPIC_VEHICLE_GEOPOSE, + self._vehicle_geopose_callback, + QoSPresetProfiles.SENSOR_DATA.value) + + self._vehicle_altitude = None + self._vehicle_altitude_sub = self.create_subscription(Altitude, + messaging.ROS_TOPIC_VEHICLE_ALTITUDE, + self._vehicle_altitude_callback, + QoSPresetProfiles.SENSOR_DATA.value) + + self._terrain_geopoint = None + self._terrain_geopoint_sub = self.create_subscription(GeoPointStamped, + messaging.ROS_TOPIC_TERRAIN_GEOPOINT, + self._terrain_geopoint_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._home_geopoint = None + self._home_geopoint_sub = self.create_subscription(GeoPointStamped, + messaging.ROS_TOPIC_HOME_GEOPOINT, + self._home_geopoint_callback, + QoSPresetProfiles.SENSOR_DATA.value) + + self._terrain_altitude = None + self._terrain_altitude_sub = self.create_subscription(Altitude, + messaging.ROS_TOPIC_TERRAIN_ALTITUDE, + self._terrain_altitude_callback, + QoSPresetProfiles.SENSOR_DATA.value) + + self._gimbal_quaternion = None + self._gimbal_quaternion_sub = self.create_subscription(Quaternion, + messaging.ROS_TOPIC_GIMBAL_QUATERNION, + self._gimbal_quaternion_callback, + QoSPresetProfiles.SENSOR_DATA.value) + + # Publishers + self._bounding_box_pub = self.create_publisher(BoundingBox, + messaging.ROS_TOPIC_BOUNDING_BOX, + QoSPresetProfiles.SENSOR_DATA.value) + + @property + def _is_gimbal_projection_enabled(self) -> bool: + """True if map rasters should be retrieved for projected field of view instead of vehicle position + + If this is set to false, map rasters are retrieved for the vehicle's global position instead. This is typically + fine as long as the camera is not aimed too far in to the horizon and has a relatively wide field of view. For + best results, this should be enabled to ensure the field of view is fully contained within the area of the + retrieved map raster. + + .. note:: + If you know your camera will be nadir-facing, disabling ``gimbal_projection`` may improve performance + """ + gimbal_projection_flag = self.get_parameter('gimbal_projection').get_parameter_value().bool_value + if type(gimbal_projection_flag) is bool: + return gimbal_projection_flag + else: + # Default behavior (safer) + self.get_logger().warn(f'Could not read gimbal projection flag: {gimbal_projection_flag}. Assume False.') + return False + + def _vehicle_geopose_callback(self, msg: GeoPoseStamped) -> None: + """Receives vehicle :class:`geographic_msgs.msg.GeoPoseStamped` message""" + self._vehicle_geopose = msg + + # Vehicle GeoPose changes affect bbox estimate -> call _publish here + self._publish() + + def _vehicle_altitude_callback(self, msg: Altitude) -> None: + """Receives vehicle :class:`mavros_msgs.msg.Altitude` message""" + self._vehicle_altitude = msg + + # No need to call _publish here: + # Vehicle altitude changes do affect bbox estimate but _publish is already called in GeoPose callback + # Altitude message is only subscribed to because it provides altitude AGL and AMSL unlike GeoPose + #self._publish() + + def _terrain_geopoint_callback(self, msg: GeoPointStamped) -> None: + """Receives terrain :class:`geographic_msgs.msg.GeoPointStamped` message""" + self._terrain_geopoint = msg + + def _home_geopoint_callback(self, msg) -> None: + """Receives home :class:`geographic_msgs.msg.GeoPointStamped` message""" + self._home_geopoint = msg + + def _terrain_altitude_callback(self, msg: Altitude) -> None: + """Receives terrain :class:`mavros_msgs.msg.Altitude` message""" + self._terrain_altitude = msg + + def _gimbal_quaternion_callback(self, msg: Quaternion) -> None: + """Receives gimbal :class:`geometry_msgs.msg.Quaternion` message""" + self._gimbal_quaternion = msg + + def image_callback(self, msg: Image) -> None: + """Receives :class:`sensor_msgs.msg.Image` message""" + # Do nothing - implementation enforced by parent class + pass + + # region Mock Image Pair + def _mock_image_pair(self, xy: GeoPt) -> Optional[ImagePair]: + """Creates mock :class:`.ImagePair` for guessing projected FOV needed for map requests, or None if not available + + The mock image pair will be paired with a pose guess to compute the expected field of view. The expected field + of view is used to request a new map that overlaps with what the camera is looking at. + + .. seealso: + :meth:`._mock_map_data` and :meth:`._mock_image_data` + + :param xy: Vehicle position + :return: Mock image pair that can be paired with a pose guess to generate a FOV guess, or None if not available + """ + image_data = self._mock_image_data() + map_data = self._mock_map_data(xy) + if image_data is None or map_data is None: + self.get_logger().warn('Missing required inputs for generating mock image PAIR.') + return None + contextual_map_data = ContextualMapData(rotation=0, crop=image_data.image.dim, map_data=map_data, + mock_data=True) + image_pair = ImagePair(image_data, contextual_map_data) + return image_pair + + # TODO: make property + def _mock_image_data(self) -> Optional[ImageData]: + """Creates mock :class:`.ImageData` for guessing projected FOV for map requests, or None if not available + + .. seealso: + :meth:`._mock_map_data` and :meth:`._mock_image_pair` + """ + if self.img_dim is None or self.camera_data is None: + self.get_logger().warn('Missing required camera data or img dim for generating mock image data.') + return None + + image_data = ImageData(image=Img(np.zeros(self.img_dim)), + frame_id='mock_image_data', + timestamp=self.usec, + camera_data=self.camera_data) + return image_data + + def _mock_map_data(self, xy: GeoPt) -> Optional[MapData]: + """Creates mock :class:`.MapData` for guessing projected FOV needed for map requests, or None if not available + + The mock image pair will be paired with a pose guess to compute the expected field of view. The expected field + of view is used to request a new map that overlaps with what the camera is looking at. + + .. seealso: + :meth:`._mock_image_pair` and :meth:`._mock_image_data` + + :param xy: Vehicle position + :return: Mock map data with mock images but with real expected bbox, or None if not available + """ + if self.camera_data is None or self.map_size_with_padding is None: + self.get_logger().warn('Missing required inputs for generating mock MAP DATA.') + return None + + if self._vehicle_altitude is None: + self.get_logger().warn('Cannot determine vehicle altitude AGL, skipping creating mock map data') + return None + altitude_agl = self._vehicle_altitude.terrain + if altitude_agl < 0: + self.get_logger().warn(f'Altitude AGL {altitude_agl} was negative, skipping mock map data.') + return + + # Scaling factor of image pixels := terrain_altitude + scaling = (self.map_size_with_padding[0] / 2) / self.camera_data.fx + radius = scaling * altitude_agl + + bbox = GeoSquare(xy, radius) + map_data = MapData(bbox=BBox(*bbox.bounds), image=Img(np.zeros(self.map_size_with_padding))) + return map_data + # endregion + + def _guess_fov_center(self, xy: GeoPt) -> Optional[GeoPt]: + """Guesses WGS84 coordinates of camera field of view (FOV) projected on ground from given origin + + :param xy: Camera position + :return: Center of the projected FOV, or None if not available + """ + if self._gimbal_quaternion is None: + self.get_logger().warn('Gimbal quaternion not available, cannot project gimbal FOV.') + return None + else: + gimbal_attitude = Attitude(q=messaging.as_np_quaternion(self._gimbal_quaternion)) + gimbal_attitude = gimbal_attitude.to_esd() # Need coordinates in image frame, not NED + + assert gimbal_attitude is not None + + if self.camera_data is None: + self.get_logger().warn('Camera data not available, cannot create a mock pose to generate a FOV guess.') + return None + + if self._terrain_altitude is None: + self.get_logger().warn('Terrain altitude not available, cannot create a mock pose to generate a FOV guess.') + return None + + if self._terrain_geopoint is None: + self.get_logger().warn('Terrain geopoint not available, cannot create a mock pose to generate a FOV guess.') + return None + + if self._home_geopoint is None: + self.get_logger().warn('Home geopoint not available, cannot create a mock pose to generate a FOV guess.') + return None + + translation = -gimbal_attitude.r @ np.array([self.camera_data.cx, self.camera_data.cy, -self.camera_data.fx]) + try: + pose = Pose(gimbal_attitude.r, translation.reshape((3, 1))) + except DataValueError as e: + self.get_logger().warn(f'Pose input values: {gimbal_attitude.r}, {translation} were invalid: {e}.') + return None + + try: + assert self._terrain_altitude is not None + assert self._terrain_geopoint is not None + assert self._home_geopoint is not None + mock_fixed_camera = FixedCamera(pose=pose, + image_pair=self._mock_image_pair(xy), + terrain_altitude_amsl=self._terrain_altitude.amsl, + terrain_altitude_ellipsoid=self._terrain_geopoint.position.altitude, + home_position=self._home_geopoint.position, + timestamp=self.usec) + except DataValueError as _: + self.get_logger().warn(f'Could not create a valid mock projection of FOV.') + return None + + if __debug__: + export_projection = self.get_parameter('export_projection').get_parameter_value().string_value + if export_projection != '': + self._export_position(mock_fixed_camera.fov.c, mock_fixed_camera.fov.fov, export_projection) + + return mock_fixed_camera.fov.fov.to_crs('epsg:4326').center + + def _publish(self) -> None: + """Publishes :class:`geographic_msgs.msg.BoundingBox` message + + If :py:attr:`._is_gimbal_projection_enabled` is True, the center of the projected camera field of view is + used instead of the vehicle position to ensure the field of view is best contained in the new map raster. + """ + if self._vehicle_geopose is None: + self.get_logger().warn('Cannot determine vehicle geopose, skipping publishing bbox') + return + + if self._vehicle_altitude is None: + self.get_logger().warn('Cannot determine vehicle altitude above ground, skipping publishing bbox') + return None + + geopt = messaging.geopoint_to_geopt(self._vehicle_geopose.pose.position) + + if self._is_gimbal_projection_enabled: + assert self._vehicle_geopose is not None + projected_center = self._guess_fov_center(geopt) + if projected_center is None: + self.get_logger().warn('Could not project field of view center. Using vehicle position for map center ' + 'instead.') + else: + projected_center = None + + assert self._vehicle_altitude is not None + map_update_altitude_agl = self._vehicle_altitude.terrain + if map_update_altitude_agl is None: + self.get_logger().warn('Cannot determine altitude AGL, skipping map update.') + return None + if map_update_altitude_agl <= 0: + self.get_logger().warn(f'Map update altitude {map_update_altitude_agl} should be > 0, skipping map update.') + return None + max_map_radius = self.get_parameter('max_map_radius').get_parameter_value().integer_value + map_radius = get_dynamic_map_radius(self.camera_data, max_map_radius, map_update_altitude_agl) + map_candidate = GeoSquare(projected_center if projected_center is not None else geopt, map_radius) + + bbox = BBox(*map_candidate.bounds) + bbox = BoundingBox(min_pt=GeoPoint(latitude=bbox.bottom, longitude=bbox.left, altitude=np.nan), + max_pt=GeoPoint(latitude=bbox.top, longitude=bbox.right, altitude=np.nan)) + self._bounding_box_pub.publish(bbox) diff --git a/gisnav/nodes/map_node.py b/gisnav/nodes/map_node.py new file mode 100644 index 00000000..45b749af --- /dev/null +++ b/gisnav/nodes/map_node.py @@ -0,0 +1,586 @@ +"""Contains :class:`.Node` that provides :class:`OrthoImage3D` s""" +import time +from typing import Optional, Tuple, get_args + +import numpy as np +import cv2 +import requests +from owslib.wms import WebMapService +from owslib.util import ServiceException +from shapely.geometry import box +from pygeodesy.geoids import GeoidPGM +from rclpy.timer import Timer +from rclpy.qos import QoSPresetProfiles +from geographic_msgs.msg import GeoPoint, GeoPointStamped, GeoPose, GeoPoseStamped, BoundingBox +from mavros_msgs.msg import Altitude +from sensor_msgs.msg import Image +from std_msgs.msg import Float32 +from cv_bridge import CvBridge +from gisnav_msgs.msg import OrthoImage3D + +from . import messaging +from .base.camera_subscriber_node import CameraSubscriberNode +from ..assertions import assert_len, assert_type +from ..data import BBox, MapData, Img +from ..geo import GeoSquare, GeoPt, get_dynamic_map_radius + + +class MapNode(CameraSubscriberNode): + """Publishes :class:`.OrthoImage3D` of approximate location to a topic and provides them through a service + + Downloads and stores orthoimage and optional DEM from WMS for location of projected camera field of view. + + Subscribes to :class:`.CameraInfo` and :class:`.FOV` messages to determine bounding box for next map to be cached. + Requests new map whenever FOV overlap with current cached map gets too small. Publishes a :class:`.OrthoImage3D` + with a high-resolution image and an optional digital elevation model (DEM) that can be used for pose estimation. + + .. warning:: + ``OWSLib`` *as of version 0.25.0* uses the Python ``requests`` library under the hood but does not seem to + document the various exceptions it raises that are passed through by ``OWSLib`` as part of its public API. + The :meth:`.get_map` method is therefore expected to raise `errors and exceptions + `_ that are specific to the + ``requests`` library. + + These errors and exceptions are not handled by the :class:`.MapNode` to avoid a direct dependency to + ``requests``. They are therefore handled as unexpected errors. + """ + + ROS_D_URL = 'http://localhost:80/wms' + """Default WMS URL""" + + ROS_D_VERSION = '1.3.0' + """Default WMS version""" + + ROS_D_TIMEOUT = 10 + """Default WMS GetMap request timeout in seconds""" + + ROS_D_PUBLISH_RATE = 1 + """Default publish rate for :class:`.OrthoImage3D` messages in Hz""" + + ROS_D_LAYERS = ['imagery'] + """Default WMS GetMap request layers parameter for image raster + + .. note:: + The combined layers should cover the flight area of the vehicle at high resolution. Typically this list would + have just one layer for high resolution aerial or satellite imagery. + """ + + ROS_D_DEM_LAYERS = ['osm-buildings-dem'] + """Default WMS GetMap request layers parameter for DEM raster + + .. note:: + This is an optional elevation layer that makes the pose estimation more accurate especially when flying at low + altitude. It should be a grayscale raster with pixel values corresponding meters relative to origin. Origin + can be whatever system is used (e.g. USGS DEM uses NAVD 88). + """ + + ROS_D_STYLES = [''] + """Default WMS GetMap request styles parameter for image raster + + .. note:: + Must be same length as :py:attr:`.ROS_D_LAYERS`. Use empty strings for server default styles. + """ + + ROS_D_DEM_STYLES = [''] + """Default WMS GetMap request styles parameter for DEM raster + + .. note:: + Must be same length as :py:attr:`.ROS_D_DEM_LAYERS`. Use empty strings for server default styles. + """ + + ROS_D_SRS = 'EPSG:4326' + """Default WMS GetMap request SRS parameter""" + + ROS_D_IMAGE_FORMAT = 'image/jpeg' + """Default WMS GetMap request image format""" + + ROS_D_IMAGE_TRANSPARENCY = False + """Default WMS GetMap request image transparency + + .. note:: + Not supported by jpeg format + """ + + ROS_D_MAP_OVERLAP_UPDATE_THRESHOLD = 0.85 + """Overlap ration between FOV and current map, under which a new map will be requested.""" + + ROS_D_MAP_UPDATE_UPDATE_DELAY = 1 + """Default delay in seconds for throttling WMS GetMap requests + + .. note:: + TODO: ROS_D_MAP_UPDATE_UPDATE_DELAY not currently used but could be useful (old param from basenode) + + When the camera is mounted on a gimbal and is not static, this delay should be set quite low to ensure that whenever + camera field of view is moved to some other location, the map update request will follow very soon after. The field + of view of the camera projected on ground generally moves *much faster* than the vehicle itself. + + .. note:: + This parameter provides a hard upper limit for WMS GetMap request frequency. Even if this parameter is set low, + WMS GetMap requests will likely be much less frequent because they will throttled by the conditions set in + :meth:`._should_request_new_map`. + """ + + # Altitude in meters used for DEM request to get local origin elevation + _DEM_REQUEST_ALTITUDE = 100 + + _WMS_CONNECTION_ATTEMPT_DELAY = 10 + """Delay in seconds until a new WMS connection is attempted in case of connection error""" + + ROS_PARAM_DEFAULTS = [ + ('url', ROS_D_URL, True), + ('version', ROS_D_VERSION, True), + ('timeout', ROS_D_TIMEOUT, True), + ('publish_rate', ROS_D_PUBLISH_RATE, True), + ('layers', ROS_D_LAYERS, False), + ('styles', ROS_D_STYLES, False), + ('dem_layers', ROS_D_DEM_LAYERS, False), + ('dem_styles', ROS_D_DEM_STYLES, False), + ('srs', ROS_D_SRS, False), + ('transparency', ROS_D_IMAGE_TRANSPARENCY, False), + ('format', ROS_D_IMAGE_FORMAT, False), + ('map_overlap_update_threshold', ROS_D_MAP_OVERLAP_UPDATE_THRESHOLD, False), + ] + """List containing ROS parameter name, default value and read_only flag tuples""" + + def __init__(self, name: str): + """Class initializer + + :param name: Node name + """ + super().__init__(name) + + # Publishers + self._terrain_altitude_pub = self.create_publisher(Altitude, + messaging.ROS_TOPIC_TERRAIN_ALTITUDE, + QoSPresetProfiles.SENSOR_DATA.value) + self._egm96_height_pub = self.create_publisher(Float32, + messaging.ROS_TOPIC_EGM96_HEIGHT, + QoSPresetProfiles.SENSOR_DATA.value) + self._terrain_geopoint_pub = self.create_publisher(GeoPointStamped, + messaging.ROS_TOPIC_TERRAIN_GEOPOINT, + QoSPresetProfiles.SENSOR_DATA.value) + self._ortho_image_3d_msg = None # This outgoing message is on a timer, needs to be stored + self._ortho_image_3d_pub = self.create_publisher(OrthoImage3D, + messaging.ROS_TOPIC_ORTHOIMAGE, + QoSPresetProfiles.SENSOR_DATA.value) + + # Subscribers + self._bounding_box = None + self._bounding_box_sub = self.create_subscription(BoundingBox, + messaging.ROS_TOPIC_BOUNDING_BOX, + self._bounding_box_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._vehicle_geopose = None + self._vehicle_geopose_callback = self.create_subscription(GeoPoseStamped, + messaging.ROS_TOPIC_VEHICLE_GEOPOSE, + self._vehicle_geopose_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._home_geopoint = None + self._home_geopoint_sub = self.create_subscription(GeoPointStamped, + messaging.ROS_TOPIC_HOME_GEOPOINT, + self._home_geopoint_callback, + QoSPresetProfiles.SENSOR_DATA.value) + + publish_rate = self.get_parameter('publish_rate').get_parameter_value().integer_value + self._publish_timer = self._create_publish_timer(publish_rate) + self._ortho_image_3d_msg = None + + # For map update timer / DEM requests + self._origin_dem_altitude = None + self._home_dem = None # dem map data + self._map_data = None + + # TODO: make configurable / use shared folder home path instead + self._egm96 = GeoidPGM('/usr/share/GeographicLib/geoids/egm96-5.pgm', kind=-3) + + self._cv_bridge = CvBridge() + + url = self.get_parameter('url').get_parameter_value().string_value + version = self.get_parameter('version').get_parameter_value().string_value + timeout = self.get_parameter('timeout').get_parameter_value().integer_value + self._wms_client = None + while self._wms_client is None: + try: + self.get_logger().error(f'Connecting to WMS endpoint...') + self._wms_client = WebMapService(url, version=version, timeout=timeout) + except requests.exceptions.ConnectionError as e: + self.get_logger().error(f'Could not connect to WMS endpoint, trying again in ' + f'{self._WMS_CONNECTION_ATTEMPT_DELAY}s...') + time.sleep(self._WMS_CONNECTION_ATTEMPT_DELAY) + else: + self.get_logger().info(f'WMS client setup complete.') + + # region Properties + @property + def _publish_timer(self) -> Timer: + """:class:`gisnav_msgs.msg.OrthoImage3D` publish and map update timer""" + return self.__publish_timer + + @_publish_timer.setter + def _publish_timer(self, value: Timer) -> None: + assert_type(value, Timer) + self.__publish_timer = value + + @property + def _ortho_image_3d_msg(self) -> Optional[OrthoImage3D]: + """:class:`gisnav_msgs.msg.OrthoImage3D` message to publish""" + return self.__ortho_image_3d_msg + + @_ortho_image_3d_msg.setter + def _ortho_image_3d_msg(self, value: Optional[OrthoImage3D]) -> None: + assert_type(value, get_args(Optional[OrthoImage3D])) + self.__ortho_image_3d_msg = value + + @property + def _origin_dem_altitude(self) -> Optional[float]: + """Elevation layer (DEM) altitude at local frame origin""" + return self.__origin_dem_altitude + + @_origin_dem_altitude.setter + def _origin_dem_altitude(self, value: Optional[float]) -> None: + assert_type(value, get_args(Optional[float])) + self.__origin_dem_altitude = value + + # region rclpy subscriber callbacks + def _vehicle_geopose_callback(self, msg: GeoPoseStamped) -> None: + """Stores :class:`geographic_msgs.msg.GeoPoseStamped` message""" + self._vehicle_geopose = msg + + # Needed by autopilot node to publish vehicle/home GeoPose with ellipsoid altitude + height = self._egm96.height(msg.pose.position.latitude, msg.pose.position.longitude) + height_msg = Float32(data=height) + self._egm96_height_pub.publish(height_msg) + + def _home_geopoint_callback(self, msg: GeoPointStamped) -> None: + """Stores :class:`geographic_msgs.msg.GeoPointStamped` message""" + self._home_geopoint = msg + + def _bounding_box_callback(self, msg: BoundingBox) -> None: + """Stores :class:`geograhpic_msgs.msg.BoundingBox` message""" + self._bounding_box = msg + + bbox = BBox(msg.min_pt.longitude, msg.min_pt.latitude, msg.max_pt.longitude, msg.max_pt.latitude) + if self._should_request_new_map(bbox): + if self.map_size_with_padding is not None: + img, dem = self._get_map(bbox, self.map_size_with_padding) + self._map_data = MapData(bbox=bbox, image=Img(img), elevation=Img(dem)) + self._ortho_image_3d_msg = self._create_msg(bbox, img, dem) + else: + self.get_logger().warn(f'Cannot request new map, could not determine size ' + f'({self.map_size_with_padding}) parameter for GetMap request.') + + def image_callback(self, msg: Image) -> None: + """Receives :class:`sensor_msgs.msg.Image` message""" + # Do nothing - enforced by CameraSubscriberNode parent + pass + # endregion rclpy subscriber callbacks + + def _should_request_new_map(self, bbox: BBox) -> bool: + """Returns True if a new map should be requested + + This check is made to avoid retrieving a new map that is almost the same as the previous map. Relaxing map + update constraints should not improve accuracy of position estimates unless the map is so old that the field of + view either no longer completely fits inside (vehicle has moved away or camera is looking in other direction) + or is too small compared to the size of the map (vehicle altitude has significantly decreased). + + :param bbox: Bounding box of latest containing camera field of view + :return: True if new map should be requested + """ + if self._bounding_box is not None: + if self._ortho_image_3d_msg is not None: + bbox_previous = messaging.bounding_box_to_bbox(self._ortho_image_3d_msg.bbox) + threshold = self.get_parameter('map_overlap_update_threshold').get_parameter_value().double_value + bbox1, bbox2 = box(*bbox), box(*bbox_previous) + ratio1 = bbox1.intersection(bbox2).area / bbox1.area + ratio2 = bbox2.intersection(bbox1).area / bbox2.area + ratio = min(ratio1, ratio2) + if ratio < threshold: + return True + else: + return True + else: + return False + + def _create_publish_timer(self, publish_rate: int) -> Timer: + """Returns a timer to publish :class:`gisnav_msgs.msg.OrthoImage3D` + + :param publish_rate: Publishing rate for the timer (in Hz) + :return: The :class:`.Timer` instance + """ + if publish_rate <= 0: + error_msg = f'Map update rate must be positive ({publish_rate} Hz provided).' + self.get_logger().error(error_msg) + raise ValueError(error_msg) + timer = self.create_timer(1 / publish_rate, self._update_and_publish) + return timer + + def _get_map(self, bbox: BBox, size: Tuple[int, int]) -> Optional[Tuple[np.ndarray, np.ndarray]]: + """Sends GetMap request to WMS for image and DEM layers and returns them as a tuple, or None if not available + + Returns zero raster as DEM if no DEM layer is available. + + TODO: Currently no support for separate arguments for imagery and height layers. Assumes height layer is + available at same CRS as imagery layer. + + :param bbox: Bounding box of the map (left, bottom, right, top) + :param size: Map raster resolution (height, width) + :return: Tuple of imagery and DEM rasters, or None if not available + """ + assert_type(bbox, BBox) + assert_type(size, tuple) + + layers, styles = self.get_parameter('layers').get_parameter_value().string_array_value,\ + self.get_parameter('styles').get_parameter_value().string_array_value + assert_len(styles, len(layers)) + assert all(isinstance(x, str) for x in layers) + assert all(isinstance(x, str) for x in styles) + + dem_layers, dem_styles = self.get_parameter('dem_layers').get_parameter_value().string_array_value,\ + self.get_parameter('dem_styles').get_parameter_value().string_array_value + assert_len(dem_styles, len(dem_layers)) + assert all(isinstance(x, str) for x in dem_layers) + assert all(isinstance(x, str) for x in dem_styles) + + srs = self.get_parameter('srs').get_parameter_value().string_value + format_ = self.get_parameter('format').get_parameter_value().string_value + transparency = self.get_parameter('transparency').get_parameter_value().bool_value + + self.get_logger().info(f'Requesting orthoimage and DEM for\n' + f'bbox: {bbox},\n' + f'layers: {layers},\n' + f'styles: {styles},\n' + f'DEM layers: {dem_layers},\n' + f'DEM styles: {dem_styles},\n' + f'SRS: {srs},\n' + f'size: {size},\n' + f'transparency: {transparency},\n' + f'format: {format_}.') + + # Do not handle possible requests library related exceptions here (see class docstring) + try: + self.get_logger().info(f'Requesting orthoimage...') + img = self._wms_client.getmap(layers=layers, styles=styles, srs=srs, bbox=bbox, size=size, + format=format_, transparent=transparency) + except ServiceException as se: + self.get_logger().error(f'GetMap request for orthoimage ran into an unexpected exception: {se}') + return None + finally: + self.get_logger().info(f'Orthoimage request complete.') + img = self._read_img(img) + + dem = None + if len(dem_layers) > 0 and dem_layers[0]: + try: + self.get_logger().info(f'Requesting DEM...') + dem = self._wms_client.getmap(layers=dem_layers, styles=dem_styles, srs=srs, bbox=bbox, size=size, + format=format_, transparent=transparency) + except ServiceException as se: + self.get_logger().error(f'GetMap request for DEM ran into an unexpected exception: {se}') + return None + finally: + self.get_logger().info(f'DEM request complete.') + dem = self._read_img(dem, True) + else: + # Assume flat (:=zero) terrain if no DEM layer provided + self.get_logger().debug(f'No DEM layer provided, assuming flat (=zero) elevation model.') + dem = np.zeros_like(img) + + return img, dem + + @staticmethod + def _read_img(img: bytes, grayscale: bool = False) -> np.ndarray: + """Reads image bytes and returns numpy array + + :return: Image as np.ndarray + """ + img = np.frombuffer(img.read(), np.uint8) + img = cv2.imdecode(img, cv2.IMREAD_UNCHANGED) if not grayscale else cv2.imdecode(img, cv2.IMREAD_GRAYSCALE) + assert_type(img, np.ndarray) + #assert_ndim(img, 3) + return img + + def _create_msg(self, bbox: BBox, img: np.ndarray, dem: np.ndarray) -> OrthoImage3D: + """Stores :class:`.OrthoImage3D message for later publication + + :param bbox: The bounding box for the two maps + :param img: The high resolution image + :param dem: The DEM raster + """ + return OrthoImage3D( + bbox=messaging.bbox_to_bounding_box(bbox), + img=self._cv_bridge.cv2_to_imgmsg(img, encoding="passthrough"), + dem=self._cv_bridge.cv2_to_imgmsg(dem, encoding="passthrough") + #dem=self._cv_bridge.cv2_to_imgmsg(dem, encoding="mono8") # TODO: need to use mono16? 255 meters max? + ) + + def _terrain_altitude_at_position(self, position: Optional[GeoPt], local_origin: bool = False) -> Optional[float]: + """Raw terrain altitude from DEM if available, or None if not available + + :param position: Position to query + :param local_origin: True to use :py:attr:`._home_dem` (retrieved specifically for local frame origin) + :return: Raw altitude in DEM coordinate frame and units + """ + map_data = self._map_data if not local_origin else self._home_dem + if map_data is not None and position is not None: + elevation = map_data.elevation.arr + bbox = map_data.bbox + polygon = box(*bbox) + point = position._geoseries[0] + + if polygon.contains(point): + h, w = elevation.shape[0:2] + assert h, w == self._img_dim + left, bottom, right, top = bbox + x = w * (position.lon - left) / (right - left) + y = h * (position.lat - bottom) / (top - bottom) + try: + dem_elevation = elevation[int(np.floor(y)), int(np.floor(x))] + except IndexError as _: + # TODO: might be able to handle this + self.get_logger().warn('Position seems to be outside current elevation raster, cannot compute ' + 'terrain altitude.') + return None + + return float(dem_elevation) + else: + # Should not happen + self.get_logger().warn('Did not have elevation raster for current location or local frame origin ' + 'altitude was unknwon, cannot compute terrain altitude.') + return None + + self.get_logger().warn(f'Map data or position not provided, cannot determine DEM elevation.') + return None + + def _terrain_altitude_amsl_at_position(self, position: Optional[GeoPt], local_origin: bool = False): + """Terrain altitude in meters AMSL accroding to DEM if available, or None if not available + + :param position: Position to query + :param local_origin: Set to True to use :py:attr:`._home_dem` instead of :py:attr:`._map_data` + :return: Terrain altitude AMSL in meters at position + """ + dem_elevation = self._terrain_altitude_at_position(position, local_origin) + if dem_elevation is not None and self._origin_dem_altitude is not None and self._home_geopoint is not None: + elevation_relative = dem_elevation - self._origin_dem_altitude + elevation_amsl = elevation_relative + self._home_geopoint.position.altitude \ + - self._egm96.height(self._home_geopoint.position.latitude, + self._home_geopoint.position.longitude) + return float(elevation_amsl) + + return None + + def _should_request_dem_for_local_frame_origin(self) -> bool: + """Returns True if a new map should be requested to determine elevation value for local frame origin + + DEM value for local frame origin is needed if elevation layer is used in order to determine absolute altitude + of altitude estimates (GISNav estimates altitude against DEM, or assumes altitude at 0 if no DEM is provided). + + :return: True if new map should be requested + """ + # TODO: re-request if home position/local frame origin has changed? currently they are assumed equal + if self._origin_dem_altitude is not None: + self.get_logger().debug(f'Not requesting DEM because origin_dem_altitude is already set.') + return False + + if self._home_geopoint is None: + self.get_logger().debug(f'Not requesting DEM because local_frame_origin is not available.') + return False + + if self.camera_data is None: + self.get_logger().debug(f'Not requesting DEM because camera_data is not available.') + return False + + return True + + def _publish_terrain_altitude(self) -> None: + """Publishes terrain altitude at current position""" + terrain_altitude_amsl = None + if self._vehicle_geopose is not None and self._home_geopoint is not None: + xy = GeoPt(x=self._vehicle_geopose.pose.position.longitude, y=self._vehicle_geopose.pose.position.latitude) + terrain_altitude_amsl = self._terrain_altitude_amsl_at_position(xy) + if terrain_altitude_amsl is None: + # Probably have not received bbox yet so no map_data, try the data that was retrieved for local position + # origin instead (assume we are at starting position) + self.get_logger().warn(f'Could not get terrain altitude amsl for position from map data for publishing ' + f'geopoint, trying DEM which is intended for local origin...') + terrain_altitude_amsl = self._terrain_altitude_amsl_at_position(xy, True) + + # TODO: redundant in autopilot.py snapshot, remove the bridge eventually + assert self._home_geopoint is not None + home_altitude_amsl = self._home_geopoint.position.altitude \ + - self._egm96.height(self._home_geopoint.position.latitude, + self._home_geopoint.position.longitude) + terrain_altitude_agl = 0. + if terrain_altitude_amsl is not None: + terrain_altitude_ellipsoid = terrain_altitude_amsl \ + + self._egm96.height(self._vehicle_geopose.pose.position.latitude, + self._vehicle_geopose.pose.position.longitude) + else: + terrain_altitude_ellipsoid = None + + terrain_altitude_home = terrain_altitude_amsl - home_altitude_amsl \ + if terrain_altitude_amsl is not None and home_altitude_amsl is not None else None + + if terrain_altitude_amsl is not None and terrain_altitude_home is not None \ + and terrain_altitude_agl is not None: + terrain_altitude_msg = Altitude(header=messaging.create_header('base_link'), + amsl=terrain_altitude_amsl, + local=terrain_altitude_home, + relative=terrain_altitude_home, + terrain=terrain_altitude_agl, + bottom_clearance=terrain_altitude_agl) + self._terrain_altitude_pub.publish(terrain_altitude_msg) + + # Also publish geopoint message, includes lat and lon in atomic message + if terrain_altitude_ellipsoid is not None: + geopoint_msg = GeoPointStamped( + header=messaging.create_header('base_link'), + position=GeoPoint( + latitude=xy.lat, + longitude=xy.lon, + altitude=terrain_altitude_ellipsoid + ) + ) + self._terrain_geopoint_pub.publish(geopoint_msg) + else: + self.get_logger().warn(f'Terrain altitude ellipsoid was None so skipping publishing.') + else: + self.get_logger().warn(f'Some altitude values were None so skipping publishing altitude ' + f'({terrain_altitude_amsl}, {terrain_altitude_home}, {terrain_altitude_agl}).') + else: + self.get_logger().warn(f'Could not determine vehicle or home position, skipping publishing terrain altitude') + + def _update_and_publish(self) -> None: + """Updates map and publishes :class:`gisnav_msgs.msg.OrthoImage3d` and terrain :class:`mavros_msgs.msg.Altitude` + + Additionally requests DEM for home (assumed local frame origin) if needed (see + :meth:`._should_request_dem_for_local_frame_origin). + + Calls :meth:`._request_new_map` if the center and altitude coordinates for the new map raster are available + and the :meth:`._should_request_new_map` check passes. + """ + if self._should_request_dem_for_local_frame_origin(): + assert self._home_geopoint is not None + assert self.camera_data is not None + max_map_radius = self.get_parameter('max_map_radius').get_parameter_value().integer_value + map_radius = get_dynamic_map_radius(self.camera_data, max_map_radius, self._DEM_REQUEST_ALTITUDE) + xy = GeoPt(x=self._home_geopoint.position.longitude, y=self._home_geopoint.position.latitude) + map_candidate = GeoSquare(xy, map_radius) + + bbox = BBox(*map_candidate.bounds) + if self.map_size_with_padding is not None: + self.get_logger().info(f'Requesting DEM for home/local frame origin (assumed same!).') + img, dem = self._get_map(bbox, self.map_size_with_padding) + self._home_dem = MapData(bbox=bbox, image=Img(img), elevation=Img(dem)) + + # TODO: assumes that this local_frame_origin is the starting location, same that was used for the request + # --> not strictly true even if it works for the simulation + if self._origin_dem_altitude is None: + if self._home_geopoint is not None: + self._origin_dem_altitude = self._terrain_altitude_at_position(xy, local_origin=True) + else: + self.get_logger().warn('Required map size unknown, skipping requesting DEM for home.') + + if self._ortho_image_3d_msg is not None: + self._ortho_image_3d_pub.publish(self._ortho_image_3d_msg) + + self._publish_terrain_altitude() diff --git a/gisnav/nodes/messaging.py b/gisnav/nodes/messaging.py new file mode 100644 index 00000000..c9600790 --- /dev/null +++ b/gisnav/nodes/messaging.py @@ -0,0 +1,145 @@ +"""Helper functions for ROS messaging""" +import time +import numpy as np +from typing import Union + +from std_msgs.msg import Header +from geometry_msgs.msg import Quaternion +from geographic_msgs.msg import GeoPoint, GeoPointStamped, BoundingBox + +from ..assertions import assert_type, assert_shape +from ..geo import GeoPt +from ..data import BBox + + +# region ROS topic names +ROS_TOPIC_BOUNDING_BOX = 'gisnav/bounding_box' +"""Name of ROS topics into which :class:`geographic_msgs.msg.BoundingBox` will be published""" + +ROS_TOPIC_VEHICLE_GEOPOSE = 'gisnav/vehicle_geopose' +"""Name of ROS topic into which :class:`geographic_msgs.msg.GeoPoseStamped` will be published""" + +ROS_TOPIC_VEHICLE_GEOPOSE_ESTIMATE = 'gisnav/vehicle_geopose/estimate' +"""Name of ROS topic into which :class:`geographic_msgs.msg.GeoPoseStamped` estimate will be published""" + +ROS_TOPIC_VEHICLE_ALTITUDE = 'gisnav/vehicle_altitude' +"""Name of ROS topics into which :class:`mavros_msgs.msg.Altitude` will be published""" + +ROS_TOPIC_VEHICLE_ALTITUDE_ESTIMATE = 'gisnav/vehicle_altitude/estimate' +"""Name of ROS topics into which :class:`mavros_msgs.msg.Altitude` estimate will be published""" + +ROS_TOPIC_GIMBAL_QUATERNION = 'gisnav/gimbal_quaternion' +"""Name of ROS topics into which :class:`geometry_msgs.msg.Quaternion` will be published""" + +ROS_TOPIC_HOME_GEOPOINT = 'gisnav/home_geopoint' +"""Name of ROS topics into which :class:`geographic_msgs.msg.GeoPointStamped` will be published""" + +ROS_TOPIC_ORTHOIMAGE = 'gisnav/orthoimage_3d' +"""ROS publish topic for :class:`.OrthoImage3D` message""" + +ROS_TOPIC_TERRAIN_ALTITUDE = 'gisnav/terrain_altitude' +"""ROS publish topic for :class:`mavros_msgs.msg.Altitude` message""" + +ROS_TOPIC_TERRAIN_GEOPOINT = 'gisnav/terrain_geopoint' +"""ROS publish topic for :class:`geographic_msgs.msg.GeoPointStamped` message""" + +ROS_TOPIC_EGM96_HEIGHT = 'gisnav/egm96_height' +"""ROS publish topic for EGM96 ellipsoid height :class:`std_msgs.msg.Float32` message""" + +ROS_TOPIC_GPS_INPUT = '/mavros/gps_input/gps_input' +"""Name of ROS topic for outgoing :class:`mavros_msgs.msg.GPSINPUT` messages over MAVROS""" + +ROS_TOPIC_SENSOR_GPS = '/fmu/sensor_gps/in' +"""Name of ROS topic for outgoing :class:`px4_msgs.msg.SensorGps` messages over PX4 microRTPS bridge""" +# endregion ROS topic names + + +def create_header(frame_id: str = '') -> Header: + """Creates a class:`std_msgs.msg.Header` for an outgoing ROS message + + :param frame_id: Header frame_id value + :return: ROS message header + """ + time_ns = time.time_ns() + sec = int(time_ns / 1e9) + nanosec = int(time_ns - (1e9 * sec)) + header = Header() + header.stamp.sec = sec + header.stamp.nanosec = nanosec + header.frame_id = frame_id + return header + + +def usec_from_header(header: Header) -> int: + """Returns timestamp in microseconds from :class:`.std_msgs.msg.Header` stamp""" + return int(1e6 * header.stamp.sec + 1e-3 * header.stamp.nanosec) + + +def as_ros_quaternion(q: np.ndarray) -> Quaternion: + """Converts (x, y, z, w) format numpy array quaternion to ROS :class:`geometric_msg.msg.Quaternion` + + .. seealso: + :meth:`.ros_to_np_quaternion` + + :param q: NumPy array quaternion of shape in (x, y, z, w) format + :return: ROS quaternion message + """ + assert_type(q, np.ndarray) + q = q.squeeze() + assert_shape(q, (4,)) + return Quaternion(x=q[0].item(), y=q[1].item(), z=q[2].item(), w=q[3].item()) + + +def as_np_quaternion(q: Quaternion) -> np.ndarray: + """Converts (x, y, z, w) format numpy array quaternion to ROS :class:`geometric_msg.msg.Quaternion` + + .. seealso: + :meth:`.np_to_ros_quaternion` + + :param q: ROS quaternion message + :return: NumPy array quaternion in (x, y, z, w) format + """ + return np.array([q.x, q.y, q.z, q.w]) + + +def wxyz_to_xyzw_q(q: np.ndarray) -> np.ndarray: + """Converts quaternion from (w, x, y, z) to (x, y, z, w) format + + .. note:: + (w, x, y, z) is used by e.g. :class:`px4_msgs.msg.VehicleAttitude` while (x, y, z, w) is used by e.g. + :class:`mavros_msgs.msg.Quaternion` and also SciPy. + + :param q: Quaternion in (w, x, y, z) format + :return: Quaternion in (x, y, z, w) format + """ + q_out = q.squeeze() + assert_shape(q_out, (4,)) + q_out = np.append(q_out[1:], q_out[0]) + q_out = q_out.reshape(q.shape) # Re-add potential padding + return q_out + + +def geopoint_to_geopt(msg: Union[GeoPoint, GeoPointStamped]) -> GeoPt: + """Convert :class:`geographic_msgs.msg.GeoPoint` or :class:`geographic_msgs.msg.GeoPointStamped` to :class:`.GeoPt` + + :param msg: ROS GeoPoint(Stamped) message + :return: GeoPt instance + """ + if isinstance(msg, GeoPoint): + return GeoPt(x=msg.longitude, y=msg.latitude) + else: + assert isinstance(msg, GeoPointStamped) + return GeoPt(x=msg.position.longitude, y=msg.position.latitude) + + +def bbox_to_bounding_box(bbox: BBox) -> BoundingBox: + """Converts :class:`.BBox` to ROS :class:`geographic_msgs.msg.BoundingBox`""" + return BoundingBox( + min_pt=GeoPoint(latitude=bbox.bottom, longitude=bbox.left, altitude=np.nan), + max_pt=GeoPoint(latitude=bbox.top, longitude=bbox.right, altitude=np.nan) + ) + + +def bounding_box_to_bbox(msg: BoundingBox) -> BBox: + """Converts :class:`geographic_msgs.msg.BoundingBox` to :class:`.BBox`""" + return BBox(msg.min_pt.longitude, msg.min_pt.latitude, msg.max_pt.longitude, msg.max_pt.latitude) diff --git a/gisnav/nodes/mock_gps_node.py b/gisnav/nodes/mock_gps_node.py index 80d6e1ad..4131dff5 100644 --- a/gisnav/nodes/mock_gps_node.py +++ b/gisnav/nodes/mock_gps_node.py @@ -1,93 +1,153 @@ -"""Extends :class:`.BaseNode` to publish mock GPS (GNSS) messages that can substitute real GPS""" +"""Extends :class:`.BridgeNode` to publish mock GPS (GNSS) messages that can substitute real GPS""" import time import numpy as np -import rclpy import socket import json -from typing import Optional, Union +from rclpy.qos import QoSPresetProfiles + +from typing import Optional from datetime import datetime from px4_msgs.msg import SensorGps -from mavros_msgs.msg import GPSINPUT +from mavros_msgs.msg import GPSINPUT, Altitude +from geographic_msgs.msg import GeoPoseStamped from gps_time import GPSTime -from gisnav.nodes.base_node import BaseNode -from gisnav.data import FixedCamera +from . import messaging +from gisnav.nodes.base.base_node import BaseNode +from gisnav.data import Attitude from gisnav.assertions import assert_type class MockGPSNode(BaseNode): """A node that publishes a mock GPS message over the microRTPS bridge""" + ROS_D_UDP_IP = "127.0.0.1" + """MAVProxy GPSInput plugin default host""" - GPS_INPUT_TOPIC_NAME = '/mavros/gps_input/gps_input' - """Name of ROS topic for outgoing :class:`mavros_msgs.msg.GPSINPUT` messages over MAVROS""" - - SENSOR_GPS_TOPIC_NAME = '/fmu/sensor_gps/in' - """Name of ROS topic for outgoing :class:`px4_msgs.msg.SensorGps` messages over PX4 microRTPS bridge""" + ROS_D_UDP_PORT = 25100 + """MAVProxy GPSInput plugin default port""" - UDP_IP = "127.0.0.1" - """MAVProxy GPSInput plugin host""" + ROS_D_PX4_MICRORTPS = True + """True for PX4 microRTPS bridge, False for ArduPilot MAVROS""" - UDP_PORT = 25100 - """MAVProxy GPSInput plugin port""" + ROS_PARAM_DEFAULTS = [ + ('udp_ip', ROS_D_UDP_IP, True), + ('udp_port', ROS_D_UDP_PORT, True), + ('px4_micrortps', ROS_D_PX4_MICRORTPS, True), + ] + """List containing ROS parameter name, default value and read_only flag tuples""" - def __init__(self, name: str, package_share_dir: str, px4_micrortps: bool = True): + def __init__(self, name: str): """Class initializer :param name: Node name - :param package_share_dir: Package share directory - :param px4_micrortps: Set True to use PX4 microRTPS bridge, MAVROS otherwise """ - super().__init__(name, package_share_dir) - self._px4_micrortps = px4_micrortps + super().__init__(name) + + # MAVROS only + self._udp_ip = self.get_parameter('udp_ip').get_parameter_value().string_value + self._udp_port = self.get_parameter('udp_port').get_parameter_value().integer_value + + self._px4_micrortps = self.get_parameter('px4_micrortps').get_parameter_value().bool_value if self._px4_micrortps: - self._gps_publisher = self.create_publisher(SensorGps, - self.SENSOR_GPS_TOPIC_NAME, - rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value) + self._mock_gps_pub = self.create_publisher(SensorGps, + messaging.ROS_TOPIC_SENSOR_GPS, + QoSPresetProfiles.SENSOR_DATA.value) self._socket = None else: - self._gps_publisher = self.create_publisher(GPSINPUT, - self.GPS_INPUT_TOPIC_NAME, - rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value) + # TODO: try to get MAVROS to work for GPSINPUT message and get rid of UDP socket + #self._mock_gps_pub = self.create_publisher(GPSINPUT, + # messaging.ROS_TOPIC_GPS_INPUT, + # QoSPresetProfiles.SENSOR_DATA.value) + self._mock_gps_pub = None self._socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - def publish(self, fixed_camera: FixedCamera) -> None: + self._vehicle_geopose_estimate_sub = self.create_subscription(GeoPoseStamped, + messaging.ROS_TOPIC_VEHICLE_GEOPOSE_ESTIMATE, + self._vehicle_geopose_estimate_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._vehicle_altitude_estimate_sub = self.create_subscription(Altitude, + messaging.ROS_TOPIC_VEHICLE_ALTITUDE_ESTIMATE, + self._vehicle_altitude_estimate_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._geopose_estimate = None + self._altitude_estimate = None + + def _vehicle_geopose_estimate_callback(self, msg: GeoPoseStamped) -> None: + """Handles latest geopose estimate + + :param msg: Latest :class:`geographic_msgs.msg.GeoPose` message + """ + self._geopose_estimate = msg + if self._altitude_estimate is not None: + lat, lon = msg.pose.position.latitude, msg.pose.position.longitude + alt_amsl = self._altitude_estimate.amsl + alt_ellipsoid = msg.pose.position.altitude + q = messaging.as_np_quaternion(msg.pose.orientation) + yaw = Attitude(q=q).yaw + timestamp = messaging.usec_from_header(msg.header) + self._publish(lat, lon, alt_amsl, alt_ellipsoid, yaw, timestamp) + else: + self.get_logger().warn('Altitude estimate not yet received, skipping publishing mock GPS message.') + + def _vehicle_altitude_estimate_callback(self, msg: Altitude) -> None: + """Handles latest altitude message + + :param msg: Latest :class:`mavros_msgs.msg.Altitude` message + """ + self._altitude_estimate = msg + + def _publish(self, lat, lon, altitude_amsl, altitude_ellipsoid, heading, timestamp) -> None: """Publishes drone position as a :class:`px4_msgs.msg.SensorGps` message - :param fixed_camera: Estimated fixed camera + :param lat: Vehicle latitude + :param lon: Vehicle longitude + :param altitude_amsl: Vehicle altitude AMSL + :param altitude_ellipsoid: Vehicle altitude above WGS 84 ellipsoid + :param heading: Vehicle heading in radians + :param timestamp: Timestamp for outgoing mock GPS message (e.g. system time) + """ - assert_type(fixed_camera, FixedCamera) - msg: Optional[Union[dict, SensorGps]] = self._generate_sensor_gps(fixed_camera) if self._px4_micrortps \ - else self._generate_gps_input(fixed_camera) + if self._px4_micrortps: + msg: SensorGps = self._generate_sensor_gps(lat, lon, altitude_amsl, altitude_ellipsoid, heading, timestamp) + else: + msg: dict = self._generate_gps_input(lat, lon, altitude_amsl, heading, timestamp) if msg is not None: if self._px4_micrortps: assert_type(msg, SensorGps) - self._gps_publisher.publish(msg) + assert self._socket is None + self._mock_gps_pub.publish(msg) else: assert_type(msg, dict) - self._socket.sendto(f'{json.dumps(msg)}'.encode('utf-8'), (self.UDP_IP, self.UDP_PORT)) + assert self._mock_gps_pub is None + self._socket.sendto(f'{json.dumps(msg)}'.encode('utf-8'), (self._udp_ip, self._udp_port)) else: self.get_logger().info('Could not create GPS message, skipping publishing.') - def _generate_gps_input(self, fixed_camera: FixedCamera) -> Optional[dict]: + def _generate_gps_input(self, lat, lon, altitude_amsl, heading, timestamp) -> Optional[dict]: """Generates a :class:`.GPSINPUT` message to send over MAVROS + .. note:: + Currently the message is sent directly over a UDP socket so the GPS_INPUT is returned as a Python dict, + not as a :class:`mavros_msgs.msg.GPSINPUT` + .. seealso: `GPS_INPUT_IGNORE_FLAGS `_ - """ - position = fixed_camera.position - - if position.altitude.amsl is None: - self.get_logger().warn(f'AMSL altitude not estimated ({position.altitude}).') - return + :param lat: Vehicle latitude + :param lon: Vehicle longitude + :param altitude_amsl: Vehicle altitude in meters AMSL + :param heading: Vehicle heading in radians + :param timestamp: System time in microseconds + :return: MAVLink GPS_INPUT message as Python dict + """ msg = {} # Adjust UTC epoch timestamp for estimation delay - msg['usec'] = int(time.time_ns() / 1e3) - (self._bridge.synchronized_time - fixed_camera.timestamp) + msg['usec'] = timestamp # int(time.time_ns() / 1e3) - (self._bridge.synchronized_time - timestamp) msg['gps_id'] = 0 msg['ignore_flags'] = 56 # vel_horiz + vel_vert + speed_accuracy @@ -95,9 +155,9 @@ def _generate_gps_input(self, fixed_camera: FixedCamera) -> Optional[dict]: msg['time_week'] = gps_time.week_number msg['time_week_ms'] = int(gps_time.time_of_week * 1e3) # TODO this implementation accurate only up to 1 second msg['fix_type'] = 3 # 3D position - msg['lat'] = int(position.lat * 1e7) - msg['lon'] = int(position.lon * 1e7) - msg['alt'] = position.altitude.amsl # ArduPilot Gazebo SITL expects AMSL + msg['lat'] = int(lat * 1e7) + msg['lon'] = int(lon * 1e7) + msg['alt'] = altitude_amsl # ArduPilot Gazebo SITL expects AMSL msg['horiz_accuracy'] = 10.0 # position.eph msg['vert_accuracy'] = 3.0 # position.epv msg['speed_accuracy'] = np.nan # should be in ignore_flags @@ -109,33 +169,34 @@ def _generate_gps_input(self, fixed_camera: FixedCamera) -> Optional[dict]: msg['satellites_visible'] = np.iinfo(np.uint8).max # TODO check yaw sign (NED or ENU?) - yaw = int(np.degrees(position.attitude.yaw % (2 * np.pi)) * 100) + yaw = int(np.degrees(heading % (2 * np.pi)) * 100) yaw = 36000 if yaw == 0 else yaw # MAVLink definition 0 := not available msg['yaw'] = yaw return msg - def _generate_sensor_gps(self, fixed_camera: FixedCamera) -> Optional[SensorGps]: - """Generates a :class:`.SensorGps` message to send over PX4 microRTPS brige""" - position = fixed_camera.position - - if position.altitude.amsl is None: - self.get_logger().warn(f'AMSL altitude not estimated ({position.altitude}).') - return + def _generate_sensor_gps(self, lat, lon, altitude_amsl, altitude_ellipsoid, heading, timestamp) -> Optional[SensorGps]: + """Generates a :class:`.SensorGps` message to send over PX4 microRTPS brige + :param lat: Vehicle latitude + :param lon: Vehicle longitude + :param altitude_amsl: Vehicle altitude in meters AMSL + :param altitude_ellipsoid: Vehicle altitude in meters above WGS 84 ellipsoid + :param heading: Vehicle heading in radians + :param timestamp: System time in microseconds + """ msg = SensorGps() - msg.timestamp = self._bridge.synchronized_time # position.timestamp - # msg.timestamp_sample = msg.timestamp + msg.timestamp = timestamp msg.timestamp_sample = 0 # msg.device_id = self._generate_device_id() msg.device_id = 0 msg.fix_type = 3 msg.s_variance_m_s = np.nan msg.c_variance_rad = np.nan - msg.lat = int(position.lat * 1e7) - msg.lon = int(position.lon * 1e7) - msg.alt = int(position.altitude.amsl * 1e3) - msg.alt_ellipsoid = int(position.altitude.ellipsoid * 1e3) + msg.lat = int(lat * 1e7) + msg.lon = int(lon * 1e7) + msg.alt = int(altitude_amsl * 1e3) + msg.alt_ellipsoid = int(altitude_ellipsoid * 1e3) msg.eph = 10.0 # position.eph msg.epv = 3.0 # position.epv msg.hdop = 0.0 @@ -154,7 +215,7 @@ def _generate_sensor_gps(self, fixed_camera: FixedCamera) -> Optional[SensorGps] msg.time_utc_usec = int(time.time() * 1e6) msg.satellites_used = np.iinfo(np.uint8).max msg.time_utc_usec = int(time.time() * 1e6) - msg.heading = position.attitude.yaw + msg.heading = heading msg.heading_offset = np.nan # msg.heading_accuracy = np.nan # msg.rtcm_injection_rate = np.nan @@ -174,9 +235,12 @@ def _generate_device_id(self) -> int: # = 11469064 return 11469064 - def unsubscribe_topics(self) -> None: - """Unsubscribes ROS topics and closes GPS_INPUT MAVLink UDP socket""" - super().unsubscribe_topics() + def destroy_node(self) -> None: + """Closes GPS_INPUT MAVLink UDP socket when node is destroyed""" if not self._px4_micrortps: - self.get_logger().info('Closing UDP socket.') + assert self._socket is not None + self.get_logger().info('Closing UDP socket...') self._socket.close() + + # socket closed - call parent method + super().destroy_node() diff --git a/gisnav/nodes/pose_estimation_node.py b/gisnav/nodes/pose_estimation_node.py new file mode 100644 index 00000000..e48c70e1 --- /dev/null +++ b/gisnav/nodes/pose_estimation_node.py @@ -0,0 +1,514 @@ +"""Module that contains the pose estimation node""" +import os +import sys +import yaml +import numpy as np +import traceback +import importlib +from typing import Optional, Union, List, Tuple, get_args + +import cv2 +from ament_index_python.packages import get_package_share_directory +from scipy.spatial.transform import Rotation +from rclpy.qos import QoSPresetProfiles +from cv_bridge import CvBridge +from geometry_msgs.msg import Pose +from sensor_msgs.msg import CameraInfo, Image +from mavros_msgs.msg import Altitude +from geometry_msgs.msg import Quaternion +from geographic_msgs.msg import GeoPoint, GeoPointStamped, GeoPose, GeoPoseStamped +from gisnav_msgs.msg import OrthoImage3D + +from . import messaging +from .base.camera_subscriber_node import CameraSubscriberNode +from ..pose_estimators.pose_estimator import PoseEstimator +from ..assertions import assert_type, assert_ndim, assert_shape +from ..data import Pose, FixedCamera, DataValueError, ImageData, Img, Attitude, ContextualMapData, MapData, ImagePair, \ + Position +from ..geo import GeoPt, GeoTrapezoid + + +class PoseEstimationNode(CameraSubscriberNode): + """Estimates and publishes pose between two images + + Compares images from :class:`sensor_msgs.msg.Image` message to maps from :class:`gisnav_msgs.msg.OrthoImage3D` + message to estimate :class:`geographic_msgs.msg.GeoPoseStamped`. + """ + # Encoding of input video (input to CvBridge) + # e.g. gscam2 only supports bgr8 so this is used to override encoding in image header + _IMAGE_ENCODING = 'bgr8' + + ROS_D_POSE_ESTIMATOR_PARAMS = 'launch/params/pose_estimators/loftr_params.yaml' + """Default parameters for initializing :class:`.PoseEstimator`""" + + ROS_D_DEBUG_EXPORT_POSITION = '' # 'position.json' + """Default filename for exporting GeoJSON containing estimated field of view and position + + Set to '' to disable + """ + + ROS_D_MISC_MAX_PITCH = 30 + """Default maximum camera pitch from nadir in degrees for attempting to estimate pose against reference map + + .. seealso:: + :py:attr:`.ROS_D_MAP_UPDATE_MAX_PITCH` + :py:attr:`.ROS_D_MAP_UPDATE_GIMBAL_PROJECTION` + """ + + ROS_D_MISC_MIN_MATCH_ALTITUDE = 80 + """Default minimum ground altitude in meters under which matches against map will not be attempted""" + + ROS_D_MISC_ATTITUDE_DEVIATION_THRESHOLD = 10 + """Magnitude of allowed attitude deviation of estimate from expectation in degrees""" + + ROS_PARAM_DEFAULTS = [ + ('pose_estimator_params', ROS_D_POSE_ESTIMATOR_PARAMS, True), + ('max_pitch', ROS_D_MISC_MAX_PITCH, False), + ('min_match_altitude', ROS_D_MISC_MIN_MATCH_ALTITUDE, False), + ('attitude_deviation_threshold', ROS_D_MISC_ATTITUDE_DEVIATION_THRESHOLD, False), + ('export_position', ROS_D_DEBUG_EXPORT_POSITION, False), + ] + """List containing ROS parameter name, default value and read_only flag tuples""" + + def __init__(self, name: str) -> None: + """Node initializer + + :param name: Node name + """ + super().__init__(name) + self._package_share_dir = get_package_share_directory('gisnav') + + # region setup pose estimator + params_file = self.get_parameter('pose_estimator_params').get_parameter_value().string_value + params = self._load_config(params_file) + module_name, class_name = params.get('class_name', '').rsplit('.', 1) + pose_estimator: PoseEstimator = self._import_class(class_name, module_name) + self._estimator = pose_estimator(*params.get('args', [])) + # endregion setup pose estimator + + self._map_data = None + + # Converts image_raw to cv2 compatible image + self._cv_bridge = CvBridge() + + # region subscribers + self._orthoimage_3d = None + self._orthoimage_3d_sub = self.create_subscription(OrthoImage3D, + messaging.ROS_TOPIC_ORTHOIMAGE, + self._orthoimage_3d_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._terrain_altitude = None + self._terrain_altitude_sub = self.create_subscription(Altitude, + messaging.ROS_TOPIC_TERRAIN_ALTITUDE, + self._terrain_altitude_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._terrain_geopoint = None + self._terrain_geopoint_sub = self.create_subscription(GeoPointStamped, + messaging.ROS_TOPIC_TERRAIN_GEOPOINT, + self._terrain_geopoint_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._vehicle_altitude = None + self._vehicle_altitude_sub = self.create_subscription(Altitude, + messaging.ROS_TOPIC_VEHICLE_ALTITUDE, + self._vehicle_altitude_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._gimbal_quaternion = None + self._gimbal_quaternion_sub = self.create_subscription(Quaternion, + messaging.ROS_TOPIC_GIMBAL_QUATERNION, + self._gimbal_quaternion_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._vehicle_geopose = None + self._vehicle_geopose_sub = self.create_subscription(GeoPoseStamped, + messaging.ROS_TOPIC_VEHICLE_GEOPOSE, + self._vehicle_geopose_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._home_geopoint = None + self._home_geopoint_sub = self.create_subscription(GeoPointStamped, + messaging.ROS_TOPIC_HOME_GEOPOINT, + self._home_geopoint_callback, + QoSPresetProfiles.SENSOR_DATA.value) + # endregion subscribers + + # region publishers + self._geopose_pub = self.create_publisher(GeoPoseStamped, + messaging.ROS_TOPIC_VEHICLE_GEOPOSE_ESTIMATE, + QoSPresetProfiles.SENSOR_DATA.value) + self._altitude_pub = self.create_publisher(Altitude, + messaging.ROS_TOPIC_VEHICLE_ALTITUDE_ESTIMATE, + QoSPresetProfiles.SENSOR_DATA.value) + # endregion publishers + + def _import_class(self, class_name: str, module_name: str) -> type: + """Dynamically imports class from given module if not yet imported + + :param class_name: Name of the class to import + :param module_name: Name of module that contains the class + :return: Imported class + """ + if module_name not in sys.modules: + self.get_logger().info(f'Importing module {module_name}.') + importlib.import_module(module_name) + imported_class = getattr(sys.modules[module_name], class_name, None) + assert imported_class is not None, f'{class_name} was not found in module {module_name}.' + return imported_class + + def _load_config(self, yaml_file: str) -> dict: + """Loads params from the provided YAML file + + :param yaml_file: Path to the yaml file + :return: The loaded yaml file as dictionary + """ + assert_type(yaml_file, str) + with open(os.path.join(self._package_share_dir, yaml_file), 'r') as f: + # noinspection PyBroadException + try: + config = yaml.safe_load(f) + self.get_logger().info(f'Loaded params:\n{config}.') + return config + except Exception as e: + self.get_logger().error(f'Could not load params file {yaml_file} because of unexpected exception.') + raise + + @property + def _altitude_scaling(self) -> Optional[float]: + """Returns camera focal length divided by camera altitude in meters""" + if self.camera_data is not None and self._vehicle_altitude is not None: + return self.camera_data.fx / self._vehicle_altitude.terrain # TODO: assumes fx == fy + else: + self.get_logger().warn('Could not estimate elevation scale because camera focal length and/or vehicle ' + 'altitude is unknown.') + return None + + @property + def _r_guess(self) -> Optional[np.ndarray]: + """Gimbal rotation matrix guess""" + if self._gimbal_quaternion is None: + self.get_logger().warn('Gimbal set attitude not available, will not provide pose guess.') + return None + else: + gimbal_attitude = Attitude(q=messaging.as_np_quaternion(self._gimbal_quaternion)) + gimbal_attitude = gimbal_attitude.to_esd() # Need coordinates in image frame, not NED + return gimbal_attitude.r + + @property + def _contextual_map_data(self) -> Optional[ContextualMapData]: + """Returns contextual (rotated) map data for pose estimation + + :return: Rotated map with associated metadata, or None if not available + """ + # Get cropped and rotated map + if self._gimbal_quaternion is not None: + gimbal_attitude = Attitude(q=messaging.as_np_quaternion(self._gimbal_quaternion), extrinsic=True) + roll = gimbal_attitude.roll + camera_yaw = gimbal_attitude.yaw + if abs(roll) > np.pi / 2: + # Assume camera pitch (NED frame) is lower than -90 degrees + # A naive interpretation of the Euler angles would assume the camera (and the vehicle) is upside down + # cause the map being rotated incorrectly by 180 degrees compared to what is needed for pose estimation + # This might happen e.g. if gimbal has been pointing straight down nadir and vehicle body has suddenly + # gained additional negative pitch before gimbal has had time to stabilize + self.get_logger().debug(f'Gimbal absolute Euler roll angle over 90 degrees: assuming gimbal is not ' + f'upside down and that gimbal absolute pitch is over 90 degrees instead.') + camera_yaw = (camera_yaw + np.pi / 2) % (2 * np.pi) + assert_type(camera_yaw, float) + assert -2 * np.pi <= camera_yaw <= 2 * np.pi, f'Unexpected gimbal yaw value: {camera_yaw} ([-2*pi, 2*pi] expected).' + else: + self.get_logger().warn(f'Camera yaw unknown, cannot estimate pose.') + return None + + return ContextualMapData(rotation=camera_yaw, map_data=self._map_data, crop=self.img_dim, + altitude_scaling=self._altitude_scaling) + + def _should_estimate(self) -> bool: + """Determines whether :meth:`._estimate` should be called + + Match should be attempted if (1) a reference map has been retrieved, (2) camera roll or pitch is not too high + (e.g. facing horizon instead of nadir), and (3) drone is not flying too low. + + :return: True if pose estimation be attempted + """ + # Check condition (1) - that MapData exists + if self._map_data is None: + self.get_logger().warn(f'No reference map available. Skipping pose estimation.') + return False + + # Check condition (2) - whether camera roll/pitch is too large + max_pitch = self.get_parameter('max_pitch').get_parameter_value().integer_value + 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 (3) - whether vehicle altitude is too low + min_alt = self.get_parameter('min_match_altitude').get_parameter_value().integer_value + assert min_alt > 0 + if self._vehicle_altitude is None or self._vehicle_altitude.terrain is np.nan: + self.get_logger().warn('Cannot determine altitude AGL, skipping map update.') + return False + if self._vehicle_altitude.terrain < min_alt: + self.get_logger().warn(f'Assumed altitude {self._vehicle_altitude.terrain} was lower than minimum ' + f'threshold for matching ({min_alt}) or could not be determined. Skipping pose ' + f'estimation.') + return False + + return True + + def image_callback(self, msg: Image) -> None: + """Handles latest :class:`sensor_msgs.msg.Image` message + + :param msg: The :class:`sensor_msgs.msg.Image` message + """ + cv_image = self._cv_bridge.imgmsg_to_cv2(msg, self._IMAGE_ENCODING) + + # Check that image dimensions match declared dimensions + if self.img_dim is not None: + cv_img_shape = cv_image.shape[0:2] + assert cv_img_shape == self.img_dim, f'Converted cv_image shape {cv_img_shape} did not match ' \ + f'declared image shape {self.img_dim}.' + + if self.camera_data is None: + self.get_logger().warn('Camera data not yet available, skipping frame.') + return None + + image_data = ImageData(image=Img(cv_image), frame_id=msg.header.frame_id, timestamp=self.usec, + camera_data=self.camera_data) + + if self._should_estimate(): + assert self._map_data is not None + assert self.camera_data is not None + assert hasattr(self._map_data, 'image'), 'Map data unexpectedly did not contain the image data.' + + image_pair = ImagePair(image_data, self._contextual_map_data) + pose = self._estimator.estimate(image_data.image.arr, image_pair.ref.image.arr, self.camera_data.k) + if pose is not None: + pose = Pose(*pose) + self._post_process_pose(pose, image_pair) + else: + self.get_logger().warn(f'Could not estimate a pose, skipping this frame.') + return None + + def _orthoimage_3d_callback(self, msg: OrthoImage3D) -> None: + """Handles latest :class:`gisnav_msgs.msg.OrthoImage3D` message + + :param msg: Latest :class:`gisnav_msgs.msg.OrthoImage3D` message + """ + bbox = messaging.bounding_box_to_bbox(msg.bbox) + img = self._cv_bridge.imgmsg_to_cv2(msg.img, desired_encoding='passthrough') + dem = self._cv_bridge.imgmsg_to_cv2(msg.dem, desired_encoding='passthrough') + assert_type(img, np.ndarray) + assert_ndim(dem, 2) + assert_shape(dem, img.shape[0:2]) + + assert self.map_size_with_padding is not None + + # Should already have received camera info so map_size_with_padding should not be None + assert img.shape[0:2] == self.map_size_with_padding, f'Decoded map {img.shape[0:2]} is not of specified ' \ + f'size {self.map_size_with_padding}.' + + elevation = Img(dem) if dem is not None else None + self._map_data = MapData(bbox=bbox, image=Img(img), elevation=elevation) + + def _terrain_altitude_callback(self, msg: Altitude) -> None: + """Handles latest terrain :class:`mavros_msgs.msg.Altitude` message + + :param msg: Latest terrain :class:`mavros_msgs.msg.Altitude` message + """ + self._terrain_altitude = msg + + def _home_geopoint_callback(self, msg: GeoPointStamped) -> None: + """Receives home :class:`geographic_msgs.msg.GeoPointStamped` message + + :param msg: Latest home :class:`geographic_msgs.msg.GeoPointStamped` message + """ + self._home_geopoint = msg + + def _terrain_geopoint_callback(self, msg: GeoPointStamped) -> None: + """Receives terrain :class:`geographic_msgs.msg.GeoPointStamped` message + + :param msg: Latest terrain :class:`geographic_msgs.msg.GeoPointStamped` message + """ + self._terrain_geopoint = msg + + def _vehicle_altitude_callback(self, msg: Altitude) -> None: + """Receives vehicle :class:`mavros_msgs.msg.Altitude` message + + :param msg: Latest vehicle :class:`mavros_msgs.msg.Altitude` message + """ + self._vehicle_altitude = msg + + def _gimbal_quaternion_callback(self, msg: Quaternion) -> None: + """Receives gimbal :class:`geometry_msgs.msg.Quaternion` message + + .. warning:: + This could be gimbal set attitude, not actual attitude + + :param msg: Latest gimbal :class:`geometry_msgs.msg.Quaternion` message + """ + self._gimbal_quaternion = msg + + def _vehicle_geopose_callback(self, msg: GeoPoseStamped) -> None: + """Receives vehicle :class:`geographic_msgs.msg.GeoPoseStamped` message + + :param msg: Latest vehicle :class:`geographic_msgs.msg.GeoPoseStamped` message + """ + self._vehicle_geopose = msg + + def _post_process_pose(self, pose: Pose, image_pair: ImagePair) -> None: + """Handles estimated pose + + :param pose: Pose result from pose estimation node worker + :param image_pair: Image pair input from which pose was estimated + """ + try: + # Compute DEM value at estimated position + # This is in camera intrinsic (pixel) units with origin at whatever the DEM uses + # For example, the USGS DEM uses NAVD 88 + x, y = -pose.t.squeeze()[0:2] + x, y = int(x), int(y) + elevation = image_pair.ref.elevation.arr[y, x] + pose = Pose(pose.r, pose.t - elevation) + except DataValueError as _: + self.get_logger().warn(f'Estimated pose was not valid, skipping this frame.') + return None + except IndexError as __: + # TODO: might be able to handle this + self.get_logger().warn(f'Estimated pose was not valid, skipping this frame.') + return None + + try: + assert self._terrain_geopoint is not None + assert self._terrain_altitude is not None + fixed_camera = FixedCamera(pose=pose, image_pair=image_pair, + terrain_altitude_amsl=self._terrain_altitude.amsl, + terrain_altitude_ellipsoid=self._terrain_geopoint.position.altitude, + home_position=self._home_geopoint.position, + timestamp=image_pair.qry.timestamp) + except DataValueError as _: + self.get_logger().warn(f'Could not estimate a valid camera position, skipping this frame.') + return None + + if not self._is_valid_estimate(fixed_camera, self._r_guess): + self.get_logger().warn('Estimate did not pass post-estimation validity check, skipping this frame.') + return None + + assert fixed_camera is not None + # noinspection PyUnreachableCode + if __debug__: + # Visualize projected FOV estimate + fov_pix = fixed_camera.fov.fov_pix + ref_img = fixed_camera.image_pair.ref.image.arr + map_with_fov = cv2.polylines(ref_img.copy(), + [np.int32(fov_pix)], True, + 255, 3, cv2.LINE_AA) + + img = np.vstack((map_with_fov, fixed_camera.image_pair.qry.image.arr)) + cv2.imshow("Projected FOV", img) + cv2.waitKey(1) + + # Export GeoJSON + export_geojson = self.get_parameter('export_position').get_parameter_value().string_value + if export_geojson != '': + self._export_position(fixed_camera.position.xy, fixed_camera.fov.fov, export_geojson) + + self.publish(fixed_camera.position) + + def _is_valid_estimate(self, fixed_camera: FixedCamera, r_guess_: np.ndarray) -> bool: + """Returns True if the estimate is valid + + Compares computed estimate to guess based on earlier gimbal attitude. This will reject estimates made when + the gimbal was not stable (which is strictly not necessary) if gimbal attitude is based on set attitude and not + actual attitude, which is assumed to filter out more inaccurate estimates. + """ + if r_guess_ is None: + self.get_logger().warn('Reference gimbal attitude (r_guess) was not available, cannot do post-estimation ' + 'validity check.') + return False + + r_guess = Rotation.from_matrix(r_guess_) + # Adjust for map rotation + camera_yaw = fixed_camera.image_pair.ref.rotation + camera_yaw = Rotation.from_euler('xyz', [0, 0, camera_yaw], degrees=False) + r_guess *= camera_yaw + + r_estimate = Rotation.from_matrix(fixed_camera.pose.r) + + magnitude = Rotation.magnitude(r_estimate * r_guess.inv()) + + threshold = self.get_parameter('attitude_deviation_threshold').get_parameter_value().integer_value + threshold = np.radians(threshold) + + if magnitude > threshold: + self.get_logger().warn(f'Estimated rotation difference to expected was too high (magnitude ' + f'{np.degrees(magnitude)}).') + return False + + return True + + 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 roll or pitch is too high up from nadir to make matching against a map + worthwhile. + + :param max_pitch: The limit for the pitch in degrees from nadir over which it will be considered too high + :return: True if pitch is too high + """ + assert_type(max_pitch, get_args(Union[int, float])) + pitch = None + if self._gimbal_quaternion is not None: + gimbal_attitude = Attitude(q=messaging.as_np_quaternion(self._gimbal_quaternion), extrinsic=True) + # 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(gimbal_attitude.pitch) + 90 + else: + self.get_logger().warn('Gimbal attitude was not available, assuming camera pitch too high.') + return True + + assert pitch is not None + if pitch > max_pitch: + self.get_logger().warn(f'Camera pitch {pitch} is above limit {max_pitch}.') + return True + + return False + + def _export_position(self, position: GeoPt, fov: GeoTrapezoid, filename: str) -> None: + """Exports the computed position and field of view into a GeoJSON file + + .. note:: + The GeoJSON file is not used by the node but can be accessed by GIS software to visualize the data it + contains + + :param position: Computed camera position or projected principal point for gimbal projection + :param: fov: Field of view of camera projected to ground + :param filename: Name of file to write into + """ + assert_type(position, GeoPt) + assert_type(fov, GeoTrapezoid) + assert_type(filename, str) + try: + position._geoseries.append(fov._geoseries).to_file(filename) + except Exception as e: + self.get_logger().error(f'Could not write file {filename} because of exception:' + f'\n{e}\n{traceback.print_exc()}') + + def publish(self, position: Position) -> None: + """Publishes estimated position over ROS topic + + :param position: Estimated position + """ + altitude_msg = Altitude(header=messaging.create_header('base_link'), + amsl=position.altitude.amsl, + local=position.altitude.home, + relative=position.altitude.home, + terrain=position.altitude.agl, + bottom_clearance=position.altitude.agl) + self._altitude_pub.publish(altitude_msg) + + geopose_msg = GeoPoseStamped( + header=messaging.create_header('base_link'), + pose=GeoPose( + position=GeoPoint(latitude=position.lat, longitude=position.lon, altitude=position.altitude.ellipsoid), + orientation=messaging.as_ros_quaternion(position.attitude.q) + ) + ) + self._geopose_pub.publish(geopose_msg) diff --git a/gisnav/nodes/px4_node.py b/gisnav/nodes/px4_node.py new file mode 100644 index 00000000..e41874d2 --- /dev/null +++ b/gisnav/nodes/px4_node.py @@ -0,0 +1,205 @@ +"""Module that contains the PX4Node ROS 2 node.""" +from typing import Optional + +import numpy as np +from scipy.spatial.transform import Rotation +from rclpy.qos import QoSPresetProfiles +from geometry_msgs.msg import Quaternion +from geographic_msgs.msg import GeoPoint, GeoPointStamped, GeoPose, GeoPoseStamped +from mavros_msgs.msg import Altitude +from px4_msgs.msg import VehicleAttitude, VehicleLocalPosition, VehicleGlobalPosition, GimbalDeviceSetAttitude + +from . import messaging +from .base.autopilot_node import AutopilotNode +from ..assertions import assert_shape + + +class PX4Node(AutopilotNode): + """ROS 2 node that acts as an adapter for PX4's microRTPS bridge + + .. warning:: + Current implementation uses :class:`px4_msgs.msg.GimbalDeviceSetAttitude` instead of + :class:`px4_msgs.msg.GimbalDeviceAttitudeStatus` for :py:attr:`.gimbal_quaternion` because the SITL simulation + does not publish the actual attitude. The set attitude does not match actual attitude in situations where + gimbal has not yet stabilized. + """ + + ROS_PARAM_DEFAULTS = [] + """List containing ROS parameter name, default value and read_only flag tuples""" + + def __init__(self, name: str) -> None: + """Initializes the ROS 2 node + + :param name: Name of the node + """ + super().__init__(name) + + self._vehicle_global_position = None + self._vehicle_global_position_sub = self.create_subscription(VehicleGlobalPosition, + '/fmu/vehicle_global_position/out', + self._vehicle_global_position_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._vehicle_local_position = None + self._vehicle_local_position_sub = self.create_subscription(VehicleLocalPosition, + '/fmu/vehicle_local_position/out', + self._vehicle_local_position_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._vehicle_attitude = None + self._vehicle_attitude_sub = self.create_subscription(VehicleAttitude, + '/fmu/vehicle_attitude/out', + self._vehicle_attitude_callback, + QoSPresetProfiles.SENSOR_DATA.value) + self._gimbal_device_set_attitude = None + self._gimbal_device_set_attitude_sub = self.create_subscription(GimbalDeviceSetAttitude, + '/fmu/gimbal_device_set_attitude/out', + self._gimbal_device_set_attitude_callback, + QoSPresetProfiles.SENSOR_DATA.value) + + # region ROS subscriber callbacks + def _vehicle_global_position_callback(self, msg: VehicleGlobalPosition) -> None: + """Handles latest :class:`px4_msgs.msg.VehicleGlobalPosition` message + + Calls :meth:`.publish_vehicle_geopose` and :meth:`.publish_vehicle_altitude` because the contents of those + messages are affected by an updated :class:`px4_msgs.msg.VehicleGlobalPosition` message. + + :param msg: :class:`px4_msgs.msg.VehicleGlobalPosition` message from the PX4-ROS 2 bridge + """ + self._vehicle_global_position = msg + self.publish_vehicle_geopose() + self.publish_vehicle_altitude() + + def _vehicle_local_position_callback(self, msg: VehicleLocalPosition) -> None: + """Handles latest :class:`px4_msgs.msg.VehicleLocalPosition` message + + Calls :meth:`.publish_home_geopoint` because the content of that message is affected by an updated + :class:`px4_msgs.msg.VehicleLocalPosition` message. + + :param msg: :class:`px4_msgs.msg.VehicleLocalPosition` message from the PX4-ROS 2 bridge + """ + self._vehicle_local_position = msg + self.publish_home_geopoint() + + def _vehicle_attitude_callback(self, msg: VehicleAttitude) -> None: + """Handles latest :class:`px4_msgs.msg.VehicleAttitude` message + + Calls :meth:`.publish_vehicle_geopose` because the content of that message is affected by an updated + :class:`px4_msgs.msg.VehicleAttitude` message. + + :param msg: :class:`px4_msgs.msg.VehicleAttitude` message from the PX4-ROS 2 bridge + """ + self._vehicle_attitude = msg + self.publish_vehicle_geopose() + + def _gimbal_device_set_attitude_callback(self, msg: GimbalDeviceSetAttitude) -> None: + """Handles latest :class:`px4_msgs.msg.GimbalDeviceSetAttitude` message + + Calls :meth:`.publish_gimbal_quaternion` because the content of that message is affected by an updated + :class:`px4_msgs.msg.GimbalDeviceSetAttitude` message. + + :param msg: :class:`px4_msgs.msg.GimbalDeviceSetAttitude` message from the PX4-ROS 2 bridge + """ + self._gimbal_device_set_attitude = msg + self.publish_gimbal_quaternion() + # endregion ROS subscriber callbacks + + # region computed attributes + @property + def vehicle_geopose(self) -> Optional[GeoPoseStamped]: + """Vehicle pose as :class:`geographic_msgs.msg.GeoPoseStamped` message or None if not available""" + if self._vehicle_global_position is not None and self._vehicle_attitude is not None: + latitude, longitude = self._vehicle_global_position.lat, self._vehicle_global_position.lon + orientation = messaging.as_ros_quaternion(messaging.wxyz_to_xyzw_q(self._vehicle_attitude.q)) + + if self.egm96_height is not None: + # compute ellipsoid altitude + altitude = self._vehicle_global_position.alt + self.egm96_height.data + else: + altitude = np.nan + + return GeoPoseStamped(header=messaging.create_header('base_link'), + pose=GeoPose( + position=GeoPoint(latitude=latitude, longitude=longitude, altitude=altitude), + orientation=orientation) + ) + else: + # TODO: could publish a GeoPoint message even if VehicleAttitude is not available + self.get_logger().warn('VehicleGlobalPosition and/or VehicleAttitude message not yet received, cannot ' + 'determine vehicle geopose.') + return None + + @property + def _vehicle_altitude_local(self) -> Optional[float]: + """Returns z coordinate from :class:`px4_msgs.msg.VehicleLocalPosition` message or None if not available""" + if self._vehicle_local_position is not None: + if self._vehicle_local_position.z_valid: + return self._vehicle_local_position.z + else: + self.get_logger().warn('VehicleLocalPosition message z is not valid, cannot determine vehicle local ' + 'altitude') + return None + else: + self.get_logger().warn('VehicleLocalPosition message not yet received, cannot determine vehicle local ' + 'altitude.') + return None + + @property + def vehicle_altitude(self) -> Optional[Altitude]: + """Vehicle altitude as :class:`mavros_msgs.msg.Altitude` message or None if not available""" + if self._vehicle_global_position is not None and self.terrain_altitude is not None: + amsl = self._vehicle_global_position.alt + terrain = self._vehicle_global_position.alt - self.terrain_altitude.amsl + local = self._vehicle_altitude_local if self._vehicle_altitude_local is not None else np.nan + altitude = Altitude( + header=messaging.create_header('base_link'), + amsl=amsl, + local=local, # TODO: home altitude ok? see https://mavlink.io/en/messages/common.html#ALTITUDE + relative=-local, + terrain=terrain, + bottom_clearance=np.nan + ) + return altitude + else: + self.get_logger().warn(f'VehicleGlobalPosition {self._vehicle_global_position} and/or terrain Altitude' + f' {self.terrain_altitude} message not yet received, cannot determine vehicle ' + f'altitude.') + return None + + @property + def gimbal_quaternion(self) -> Optional[Quaternion]: + """Gimbal orientation as :class:`geometry_msgs.msg.Quaternion` message or None if not available""" + if self._vehicle_attitude is None or self._gimbal_device_set_attitude is None: + return None + + # Gimbal roll & pitch/tilt is assumed stabilized so only need yaw/pan + yaw_mask = np.array([1, 0, 0, 1]) # TODO: remove assumption + assert_shape(self._vehicle_attitude.q.squeeze(), (4,)) + vehicle_yaw = self._vehicle_attitude.q * yaw_mask + # geometry_msgs Quaternion expects (x, y, z, w) while px4_msgs VehicleAttitude has (w, x, y, z) + vehicle_yaw = Rotation.from_quat(np.append(vehicle_yaw[1:], vehicle_yaw[0])) + + assert_shape(self._gimbal_device_set_attitude.q.squeeze(), (4,)) + gimbal_quaternion_frd = self._gimbal_device_set_attitude.q + gimbal_quaternion_frd = Rotation.from_quat(np.append(gimbal_quaternion_frd[1:], gimbal_quaternion_frd[0])) + gimbal_quaternion_ned = vehicle_yaw * gimbal_quaternion_frd # TODO: ENU instead of NED? ROS convention? + gimbal_quaternion_ned = messaging.as_ros_quaternion(gimbal_quaternion_ned.as_quat()) + + return gimbal_quaternion_ned + + @property + def home_geopoint(self) -> Optional[GeoPointStamped]: + """Home position as :class:`geographic_msgs.msg.GeoPointStamped` message or None if not available""" + if self._vehicle_local_position is not None: + latitude, longitude = self._vehicle_local_position.ref_lat, self._vehicle_local_position.ref_lon + + if self.egm96_height is not None: + # compute ellipsoid altitude + altitude = self._vehicle_local_position.ref_alt + self.egm96_height.data + else: + altitude = np.nan + + return GeoPointStamped(header=messaging.create_header('base_link'), + position=GeoPoint(latitude=latitude, longitude=longitude, altitude=altitude)) + else: + self.get_logger().warn('VehicleLocalPosition message not yet received, cannot determine home geopoint.') + return None + # endregion computed attributes diff --git a/gisnav/pose_estimators/__init__.py b/gisnav/pose_estimators/__init__.py index e69de29b..351602a0 100644 --- a/gisnav/pose_estimators/__init__.py +++ b/gisnav/pose_estimators/__init__.py @@ -0,0 +1,14 @@ +"""GISNav pose estimator adapters + +All pose estimators are defined in dedicated modules to keep individual file size down. They are imported here to +package namespace for convenience. For example: + +.. code-block:: + + #from gisnav.pose_estimators.loftr_pose_estimator import LoFTRPoseEstimator + from gisnav.pose_estimators import LoFTRPoseEstimator +""" +from .pose_estimator import PoseEstimator +from .keypoint_pose_estimator import KeypointPoseEstimator +from .superglue_pose_estimator import SuperGluePoseEstimator +from .loftr_pose_estimator import LoFTRPoseEstimator diff --git a/gisnav/pose_estimators/keypoint_pose_estimator.py b/gisnav/pose_estimators/keypoint_pose_estimator.py index 861305a2..5e2302dd 100644 --- a/gisnav/pose_estimators/keypoint_pose_estimator.py +++ b/gisnav/pose_estimators/keypoint_pose_estimator.py @@ -4,17 +4,16 @@ from abc import abstractmethod from typing import Tuple, Optional -from dataclasses import dataclass, field from gisnav.pose_estimators.pose_estimator import PoseEstimator -from gisnav.assertions import assert_type, assert_len, assert_pose +from gisnav.assertions import assert_pose class KeypointPoseEstimator(PoseEstimator): """Abstract base class for all keypoint-based pose estimators - This class implements the :meth:`.estimate_pose` method that estimates a pose from matched keypoints. An abstract - :meth:`.find_matching_keypoints` method must be implemented by extending classes. + This class implements an :meth:`.estimate` method that estimates a pose from matched keypoints. An abstract + :meth:`._find_matching_keypoints` method must be implemented by extending classes. """ _HOMOGRAPHY_MINIMUM_MATCHES = 4 """Minimum matches for homography estimation, should be at least 4""" @@ -25,7 +24,7 @@ def __init__(self, min_matches: int): :param min_matches: Minimum (>=4) required matched keypoints for pose estimates """ # Use provided value as long as it's above _HOMOGRAPHY_MINIMUM_MATCHES - self._min_matches = max(self._HOMOGRAPHY_MINIMUM_MATCHES, min_matches or _HOMOGRAPHY_MINIMUM_MATCHES) + self._min_matches = max(self._HOMOGRAPHY_MINIMUM_MATCHES, min_matches or self._HOMOGRAPHY_MINIMUM_MATCHES) @abstractmethod def _find_matching_keypoints(self, query: np.ndarray, reference: np.ndarray) \ @@ -41,9 +40,9 @@ def _find_matching_keypoints(self, query: np.ndarray, reference: np.ndarray) \ """ pass - def estimate_pose(self, query: np.ndarray, reference: np.ndarray, k: np.ndarray, - guess: Optional[Tuple[np.ndarray, np.ndarray]] = None, - elevation_reference: Optional[np.ndarray] = None) \ + def estimate(self, query: np.ndarray, reference: np.ndarray, k: np.ndarray, + guess: Optional[Tuple[np.ndarray, np.ndarray]] = None, + elevation_reference: Optional[np.ndarray] = None) \ -> Optional[Tuple[np.ndarray, np.ndarray]]: """Returns pose between given query and reference images, or None if no pose could not be estimated @@ -54,7 +53,7 @@ def estimate_pose(self, query: np.ndarray, reference: np.ndarray, k: np.ndarray, :param k: Camera intrinsics matrix (3, 3) :param guess: Optional initial guess for camera pose :param elevation_reference: Optional elevation raster (same size resolution as reference image, grayscale) - :return: Pose tuple consisting of a rotation (3, 3) and translation (3, 1) np.ndarrays + :return: Pose tuple consisting of a rotation (3, 3) and translation (3, 1) NumPy arrays """ if elevation_reference is not None: assert elevation_reference.shape[0:2] == reference.shape[0:2] diff --git a/gisnav/pose_estimators/loftr_pose_estimator.py b/gisnav/pose_estimators/loftr_pose_estimator.py index 32777149..639c80e3 100644 --- a/gisnav/pose_estimators/loftr_pose_estimator.py +++ b/gisnav/pose_estimators/loftr_pose_estimator.py @@ -30,9 +30,6 @@ class TorchDevice(Enum): def __init__(self, min_matches: int) -> None: """Class initializer - This method is intended to be called inside :meth:`.initializer` together with a global variable declaration - so that attributes initialized here are also available for :meth:`.worker`. - :param min_matches: Minimum required keypoint matches (should be >= 4) """ super(LoFTRPoseEstimator, self).__init__(min_matches) diff --git a/gisnav/pose_estimators/pose_estimator.py b/gisnav/pose_estimators/pose_estimator.py index ef24e617..f6e39b51 100644 --- a/gisnav/pose_estimators/pose_estimator.py +++ b/gisnav/pose_estimators/pose_estimator.py @@ -6,61 +6,14 @@ from abc import ABC, abstractmethod from typing import Optional, Tuple -from gisnav.assertions import assert_type, assert_pose - class PoseEstimator(ABC): - """Abstract base class for all pose estimators - - This class implements static initializer and worker methods that are intended to be used with - :class:`multiprocessing.pool.Pool` or :class:`multiprocessing.pool.ThreadPool`. The abstract :meth:`.estimate_pose` - must be implemented by extending classes. - """ - - POSE_ESTIMATOR_GLOBAL_VAR = '_pose_estimator' - """A global pose estimator instance is stored under this name""" - - @staticmethod - def initializer(class_: type, *args) -> None: - """Creates a global instance of given :class:`.PoseEstimator` child class under - :py:attr:`.POSE_ESTIMATOR_GLOBAL_VAR`. - - :param class_: The :class:`.PoseEstimator` child class to initialize - :return: - """ - if PoseEstimator.POSE_ESTIMATOR_GLOBAL_VAR not in globals(): - globals()[PoseEstimator.POSE_ESTIMATOR_GLOBAL_VAR]: PoseEstimator = class_(*args) - - @staticmethod - def worker(query: np.ndarray, reference: np.ndarray, k: np.ndarray, - guess: Optional[Tuple[np.ndarray, np.ndarray]] = None, - elevation_reference: Optional[np.ndarray] = None) -> Optional[Tuple[np.ndarray, np.ndarray]]: - """Returns pose between provided images, or None if pose cannot be estimated - - :param query: The first (query) image for pose estimation - :param reference: The second (reference) image for pose estimation - :param k: Camera intrinsics matrix of shape (3, 3) - :param guess: Optional initial guess for camera pose - :param elevation_reference: Optional elevation raster (same size resolution as reference image, grayscale) - :return: Pose tuple consisting of a rotation (3, 3) and translation (3, 1) numpy arrays - """ - pose_estimator: PoseEstimator = globals()[PoseEstimator.POSE_ESTIMATOR_GLOBAL_VAR] - - if guess is not None: - assert_pose(guess) - - pose = pose_estimator.estimate_pose(query, reference, k, guess, elevation_reference) - - # Do independent check - child class might not check their output - if pose is not None: - assert_pose(pose) - - return pose + """Abstract base class for all pose estimators""" @abstractmethod - def estimate_pose(self, query: np.ndarray, reference: np.ndarray, k: np.ndarray, - guess: Optional[Tuple[np.ndarray, np.ndarray]] = None, - elevation_reference: Optional[np.ndarray] = None) -> Optional[Tuple[np.ndarray, np.ndarray]]: + def estimate(self, query: np.ndarray, reference: np.ndarray, k: np.ndarray, + guess: Optional[Tuple[np.ndarray, np.ndarray]] = None, + elevation_reference: Optional[np.ndarray] = None) -> Optional[Tuple[np.ndarray, np.ndarray]]: """Returns pose between provided images, or None if pose cannot be estimated :param query: The first (query) image for pose estimation diff --git a/gisnav/pose_estimators/superglue_pose_estimator.py b/gisnav/pose_estimators/superglue_pose_estimator.py index e3494c8f..17b51a5c 100644 --- a/gisnav/pose_estimators/superglue_pose_estimator.py +++ b/gisnav/pose_estimators/superglue_pose_estimator.py @@ -26,11 +26,9 @@ class TorchDevice(Enum): def __init__(self, min_matches: int, params: dict) -> None: """Class initializer - This method is intended to be called inside :meth:`.initializer` together with a global variable declaration - so that attributes initialized here are also available for :meth:`.worker`. - :param min_matches: Minimum required keypoint matches (should be >= 4) - :param params: SuperGluePoseEstimator config to be passed to :class:`models.matching.Matching` + :param params: SuperGluePoseEstimator params to be passed to + :class:`SuperGluePretrainedNetwork.models.matching.Matching` """ super(SuperGluePoseEstimator, self).__init__(min_matches) self._device = SuperGluePoseEstimator.TorchDevice.CUDA.value if torch.cuda.is_available() else \ diff --git a/gisnav/wms.py b/gisnav/wms.py deleted file mode 100644 index c16e912b..00000000 --- a/gisnav/wms.py +++ /dev/null @@ -1,115 +0,0 @@ -"""Contains a WMS client for asynchronously requesting map rasters from an external endpoint""" -from __future__ import annotations - -import numpy as np -import cv2 - -from typing import Tuple, List, Optional -from owslib.wms import WebMapService -from owslib.util import ServiceException - -from gisnav.assertions import assert_type, assert_ndim -from gisnav.data import MapData, Img - - -class WMSClient: - """A WMS client for asynchronously requesting map rasters - - This class defines static initializer and worker methods that are intended to be used with the - :class:`multiprocessing.pool.Pool` and :class:`multiprocessing.pool.ThreadPool` interfaces. - - .. note:: - You probably should not need to instantiate :class:`.WMSClient` directly from outside this class. Use the - :meth:`.initializer` to create your client instead, and the :meth:`.worker` to use it. - - .. warning:: - ``OWSLib`` *as of version 0.25.0* uses the Python ``requests`` library under the hood but does not seem to - document the various exceptions it raises that are passed through by ``OWSLib`` as part of its public API. - The :meth:`.worker` method is therefore expected to raise `errors and exceptions - `_ that are specific to the - ``requests`` library. - - These errors and exceptions are not handled by the :class:`.WMSClient` to avoid a direct dependency to - ``requests``. The recommended approach is therefore to handle them as unexpected errors in the error callback - of the invoking :meth:`multiprocessing.pool.Pool.apply_async`, - :meth:`multiprocessing.pool.ThreadPool.apply_async`, or similar method. - """ - - WMS_CLIENT_GLOBAL_VAR = '_wms_client' - """A global :class:`.WMSClient` instance is stored under this name""" - - _IMAGE_TRANSPARENCY = False - """Image background transparency (not supported by jpeg format)""" - - def __init__(self, url: str, version: str, timeout: int): - """Initializes instance attributes - - :param url: WMS endpoint url - :param version: WMS version (e.g. '1.1.1') - :param timeout: WMS request timeout in seconds - """ - # Do not handle possible connection related exceptions here (see class docstring) - self._wms = WebMapService(url, version=version, timeout=timeout) - - @staticmethod - def initializer(url: str, version_: str, timeout_: int) -> None: - """Creates a global instance of :class:`.WMSClient` under :py:attr:`.WMS_CLIENT_GLOBAL_VAR`. - - :param url: WMS server endpoint url - :param version_: WMS server version - :param timeout_: WMS request timeout seconds - """ - if WMSClient.WMS_CLIENT_GLOBAL_VAR not in globals(): - globals()[WMSClient.WMS_CLIENT_GLOBAL_VAR] = WMSClient(url, version_, timeout_) - - @staticmethod - def worker(layers: List[str], styles: List[str], bbox: Tuple[float], map_size: Tuple[int, int], srs_str: str, - image_format: str, elevation_layers: Optional[List[str]] = None) \ - -> Tuple[np.ndarray, Optional[np.ndarray]]: - """Requests one or more map imagery and optional height raster layers from the WMS server - - TODO: Currently no support for separate arguments for imagery and height layers. Assumes height layer is - available at same styles and CRS as imagery layer. - - :param layers: List of requested map layers - :param styles: Optional styles of same length as layers, use empty strings for default styles - :param bbox: Bounding box of the map as tuple (left, bottom, right, top) - :param map_size: Map size tuple (height, width) - :param srs_str: WMS server SRS - :param image_format: WMS server requested image format - :param elevation_layers: Optional layers for height raster - :return: Tuple of imagery and height (Optional) rasters - """ - assert WMSClient.WMS_CLIENT_GLOBAL_VAR in globals() - wms_client: WMSClient = globals()[WMSClient.WMS_CLIENT_GLOBAL_VAR] - - # Do not handle possible requests library related exceptions here (see class docstring) - try: - map_ = wms_client._wms.getmap(layers=layers, styles=styles, srs=srs_str, bbox=bbox, size=map_size, - format=image_format, transparent=WMSClient._IMAGE_TRANSPARENCY) - except ServiceException as _: - # TODO: handle OWSLib exceptions - currently passed on to error callback - raise - - map_ = np.frombuffer(map_.read(), np.uint8) - map_ = cv2.imdecode(map_, cv2.IMREAD_UNCHANGED) - assert_type(map_, np.ndarray) - assert_ndim(map_, 3) - - elevation = None - if elevation_layers is not None: - try: - elevation = wms_client._wms.getmap(layers=elevation_layers, styles=styles, srs=srs_str, bbox=bbox, - size=map_size, format=image_format, - transparent=False) - except ServiceException as _: - # TODO: handle exception for height layer - pass - - if elevation is not None: - elevation = np.frombuffer(elevation.read(), np.uint8) # TODO: handle values above 255? - elevation = cv2.imdecode(elevation, cv2.IMREAD_GRAYSCALE) - assert_type(elevation, np.ndarray) - assert_ndim(elevation, 2) # Grayscale, no color channels - - return map_, elevation diff --git a/launch/ardupilot.launch.py b/launch/ardupilot.launch.py new file mode 100644 index 00000000..e8bb0beb --- /dev/null +++ b/launch/ardupilot.launch.py @@ -0,0 +1,38 @@ +"""Launches GISNav with ArduPilot SITL simulation configuration.""" +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import ThisLaunchFileDir +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + +from gisnav.data import PackageData + + +def generate_launch_description(): + """Generates launch description with ArduPilot MAVROS adapter""" + dirname = os.path.dirname(__file__) + filename = os.path.join(dirname, '../package.xml') + package_data = PackageData.parse_package_data(os.path.abspath(filename)) + package_share_dir = get_package_share_directory(package_data.package_name) + + ld = LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/base.launch.py']) + ), + ]) + ld.add_action(Node( + package='gisnav', + name='ardupilot_node', + executable='ardupilot_node', + parameters=[os.path.join(package_share_dir, 'launch/params/ardupilot_node.yaml')] + )) + ld.add_action(Node( + package='gisnav', + name='mock_gps_node', + executable='mock_gps_node', + parameters=[os.path.join(package_share_dir, 'launch/params/mock_gps_node_ardupilot.yaml')] + )) + return ld diff --git a/launch/base.launch.py b/launch/base.launch.py new file mode 100644 index 00000000..111a34a8 --- /dev/null +++ b/launch/base.launch.py @@ -0,0 +1,41 @@ +"""Launches GISNav's autopilot agnostic nodes + +The :class:`.PoseEstimationNode`, :class:`.BBoxNode`, and :class:`.MapNode` nodes are autopilot agnostic and are +launched from a shared description defined in this file. +""" +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +from gisnav.data import PackageData + + +def generate_launch_description(): + """Generates shared autopilot agnostic launch description""" + dirname = os.path.dirname(__file__) + filename = os.path.join(dirname, '../package.xml') + package_data = PackageData.parse_package_data(os.path.abspath(filename)) + package_share_dir = get_package_share_directory(package_data.package_name) + + ld = LaunchDescription() + ld.add_action(Node( + package='gisnav', + name='map_node', + executable='map_node', + parameters=[os.path.join(package_share_dir, 'launch/params/map_node.yaml')] + )) + ld.add_action(Node( + package='gisnav', + name='bbox_node', + executable='bbox_node', + parameters=[os.path.join(package_share_dir, 'launch/params/bbox_node.yaml')] + )) + ld.add_action(Node( + package='gisnav', + name='pose_estimation_node', + executable='pose_estimation_node', + parameters=[os.path.join(package_share_dir, 'launch/params/pose_estimation_node.yaml')] + )) + return ld diff --git a/launch/examples/base_camera_topic_remap.launch.py b/launch/examples/base_camera_topic_remap.launch.py new file mode 100644 index 00000000..44aec91a --- /dev/null +++ b/launch/examples/base_camera_topic_remap.launch.py @@ -0,0 +1,46 @@ +"""Launches GISNav's autopilot agnostic nodes + +The :class:`.PoseEstimationNode`, :class:`.BBoxNode`, and :class:`.MapNode` nodes are autopilot agnostic and are +launched from a shared description defined in this file. +""" +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +from gisnav.data import PackageData +from gisnav.nodes.base.camera_subscriber_node import ROS_CAMERA_INFO_TOPIC, ROS_IMAGE_TOPIC + + +def generate_launch_description(): + """Generates shared autopilot agnostic launch description""" + dirname = os.path.dirname(__file__) + filename = os.path.join(dirname, '../package.xml') + package_data = PackageData.parse_package_data(os.path.abspath(filename)) + package_share_dir = get_package_share_directory(package_data.package_name) + + ld = LaunchDescription() + ld.add_action(Node( + package='gisnav', + name='map_node', + executable='map_node', + parameters=[os.path.join(package_share_dir, 'launch/params/map_node.yaml')] + )) + ld.add_action(Node( + package='gisnav', + name='bbox_node', + executable='bbox_node', + parameters=[os.path.join(package_share_dir, 'launch/params/bbox_node.yaml')] + )) + ld.add_action(Node( + package='gisnav', + name='pose_estimation_node', + executable='pose_estimation_node', + parameters=[os.path.join(package_share_dir, 'launch/params/pose_estimation_node.yaml')], + remappings=[ + (ROS_IMAGE_TOPIC, 'image'), + (ROS_CAMERA_INFO_TOPIC, 'camera_info'), + ] + )) + return ld diff --git a/launch/mock_gps_node.launch.py b/launch/mock_gps_node.launch.py deleted file mode 100644 index 6fc3bd2b..00000000 --- a/launch/mock_gps_node.launch.py +++ /dev/null @@ -1,30 +0,0 @@ -"""Launches :class:`.MockGPSNode` with the typhoon_h480__ksql_airport parameters""" -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -from gisnav.data import PackageData - - -def generate_launch_description(): - """Generates launch description with typhoon_h480__ksql_airport config file""" - dirname = os.path.dirname(__file__) - filename = os.path.join(dirname, '../package.xml') - package_data = PackageData.parse_package_data(os.path.abspath(filename)) - - ld = LaunchDescription() - config = os.path.join( - get_package_share_directory(package_data.package_name), - 'config', - 'typhoon_h480__ksql_airport.yaml' - ) - node = Node( - package='gisnav', - name='mock_gps_node', - executable='mock_gps_node', - parameters=[config] - ) - ld.add_action(node) - return ld diff --git a/launch/params/ardupilot_node.yaml b/launch/params/ardupilot_node.yaml new file mode 100644 index 00000000..082dc7b9 --- /dev/null +++ b/launch/params/ardupilot_node.yaml @@ -0,0 +1,2 @@ +ardupilot_node: + ros__parameters: [] \ No newline at end of file diff --git a/launch/params/bbox_node.yaml b/launch/params/bbox_node.yaml new file mode 100644 index 00000000..7933fd61 --- /dev/null +++ b/launch/params/bbox_node.yaml @@ -0,0 +1,5 @@ +bbox_node: + ros__parameters: + gimbal_projection: True + max_map_radius: 1000 + export_projection: '' \ No newline at end of file diff --git a/launch/params/map_node.yaml b/launch/params/map_node.yaml new file mode 100644 index 00000000..cc129a6c --- /dev/null +++ b/launch/params/map_node.yaml @@ -0,0 +1,9 @@ +map_node: + ros__parameters: + url: 'http://localhost:80/wms' + layers: ['imagery'] + dem_layers: ['osm-buildings'] + # srs: 'EPSG:4326' # default value, disabled for testing purposes + image_format: 'image/jpeg' + map_update_overlay_threshold: 0.85 + max_map_radius: 1000 diff --git a/launch/params/mock_gps_node_ardupilot.yaml b/launch/params/mock_gps_node_ardupilot.yaml new file mode 100644 index 00000000..66173088 --- /dev/null +++ b/launch/params/mock_gps_node_ardupilot.yaml @@ -0,0 +1,3 @@ +mock_gps_node: + ros__parameters: + px4_micrortps: False # Set to False for ArduPilot MAVROS diff --git a/launch/params/mock_gps_node_px4.yaml b/launch/params/mock_gps_node_px4.yaml new file mode 100644 index 00000000..2b0c2cdd --- /dev/null +++ b/launch/params/mock_gps_node_px4.yaml @@ -0,0 +1,3 @@ +mock_gps_node: + ros__parameters: + px4_micrortps: True # Set to False for ArduPilot MAVROS diff --git a/launch/params/pose_estimation_node.yaml b/launch/params/pose_estimation_node.yaml new file mode 100644 index 00000000..d7b9e20a --- /dev/null +++ b/launch/params/pose_estimation_node.yaml @@ -0,0 +1,6 @@ +pose_estimation_node: + ros__parameters: + export_position: '' + max_pitch: 30 + min_match_altitude: 50 + attitude_deviation_threshold: 10 diff --git a/config/loftr_params.yaml b/launch/params/pose_estimators/loftr_params.yaml similarity index 100% rename from config/loftr_params.yaml rename to launch/params/pose_estimators/loftr_params.yaml diff --git a/config/superglue_params.yaml b/launch/params/pose_estimators/superglue_params.yaml similarity index 83% rename from config/superglue_params.yaml rename to launch/params/pose_estimators/superglue_params.yaml index ffbe6fbe..46934151 100644 --- a/config/superglue_params.yaml +++ b/launch/params/pose_estimators/superglue_params.yaml @@ -1,5 +1,5 @@ class_name: 'gisnav.pose_estimators.superglue_pose_estimator.SuperGluePoseEstimator' -args: # Recommended outdoor config from SuperGluePoseEstimator repo's README.md +args: # Recommended outdoor params from SuperGluePoseEstimator repo's README.md - 15 # _min_matches - superpoint: nms_radius: 3 diff --git a/launch/params/px4_node.yaml b/launch/params/px4_node.yaml new file mode 100644 index 00000000..a75fec40 --- /dev/null +++ b/launch/params/px4_node.yaml @@ -0,0 +1,2 @@ +px4_node: + ros__parameters: [] \ No newline at end of file diff --git a/launch/px4.launch.py b/launch/px4.launch.py new file mode 100644 index 00000000..1b007315 --- /dev/null +++ b/launch/px4.launch.py @@ -0,0 +1,38 @@ +"""Launches GISNav with PX4 SITL simulation configuration""" +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import ThisLaunchFileDir +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + +from gisnav.data import PackageData + + +def generate_launch_description(): + """Generates launch description with PX4 Fast DDS bridge adapter""" + dirname = os.path.dirname(__file__) + filename = os.path.join(dirname, '../package.xml') + package_data = PackageData.parse_package_data(os.path.abspath(filename)) + package_share_dir = get_package_share_directory(package_data.package_name) + + ld = LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/base.launch.py']) + ), + ]) + ld.add_action(Node( + package='gisnav', + name='px4_node', + executable='px4_node', + parameters=[os.path.join(package_share_dir, 'launch/params/px4_node.yaml')] + )) + ld.add_action(Node( + package='gisnav', + name='mock_gps_node', + executable='mock_gps_node', + parameters=[os.path.join(package_share_dir, 'launch/params/mock_gps_node_px4.yaml')] + )) + return ld diff --git a/launch/test_mock_gps_node.launch.py b/launch/test_mock_gps_node.launch.py deleted file mode 100644 index 66141f01..00000000 --- a/launch/test_mock_gps_node.launch.py +++ /dev/null @@ -1,30 +0,0 @@ -"""Launches :class:`.MockGPSNode` with the test_typhoon_h480__ksql_airport parameters""" -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch_ros.actions import Node - -from gisnav.data import PackageData - - -def generate_launch_description(): - """Generates launch description with test_typhoon_h480__ksql_airport config file""" - dirname = os.path.dirname(__file__) - filename = os.path.join(dirname, '../package.xml') - package_data = PackageData.parse_package_data(os.path.abspath(filename)) - - ld = LaunchDescription() - config = os.path.join( - get_package_share_directory(package_data.package_name), - 'config', - 'test_typhoon_h480__ksql_airport.yaml' - ) - node = Node( - package='gisnav', - name='mock_gps_node', - executable='mock_gps_node', - parameters=[config] - ) - ld.add_action(node) - return ld diff --git a/package.xml b/package.xml index da161bcb..3908f0f9 100644 --- a/package.xml +++ b/package.xml @@ -2,26 +2,35 @@ gisnav - 0.62.0 + 0.63.0 Estimates airborne drone global position by matching video to map retrieved from onboard GIS server. Harri Makelin Harri Makelin MIT + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + ament_copyright ament_flake8 ament_pep257 python3-pytest launch_testing - rclpy - rcl_interfaces - cv_bridge - sensor_msgs - mavros_msgs - std_msgs - px4_msgs - ros2launch + rclpy + rosidl_typesupport_c + rcl_interfaces + cv_bridge + std_msgs + sensor_msgs + geometry_msgs + mavros_msgs + geographic_info + geographic_msgs + px4_msgs + gisnav_msgs + ros2launch ament_python diff --git a/requirements-dev.txt b/requirements-dev.txt index dda1d53b..2cac3811 100644 --- a/requirements-dev.txt +++ b/requirements-dev.txt @@ -5,6 +5,7 @@ autodocsumm==0.2.7 docutils>=0.17 myst_parser==0.16.1 pygments==2.11.2 # make Sphinx highlighting work +sphinx-design # Unit & ROS 2 integration tests pytest diff --git a/requirements.txt b/requirements.txt index b09cf6ae..432392bc 100644 --- a/requirements.txt +++ b/requirements.txt @@ -5,6 +5,7 @@ torch scipy>=1.4.0 OWSLib==0.25.0 pyyaml +requests # Geospatial objects handling geopandas @@ -20,4 +21,4 @@ yacs gps-time # MAVROS WGS84 ellipsoid to AMSL altitude conversion -pygeodesy \ No newline at end of file +pygeodesy diff --git a/setup.py b/setup.py index ca19af04..f8a4a522 100644 --- a/setup.py +++ b/setup.py @@ -8,9 +8,11 @@ # Setup packages depending on what submodules have been downloaded packages_ = [ pdata.package_name, - pdata.package_name + '.autopilots', pdata.package_name + '.pose_estimators', - pdata.package_name + '.nodes' + pdata.package_name + '.nodes', + pdata.package_name + '.nodes.base', + 'test', + 'test.launch' ] package_dir_ = {} package_data_ = {} @@ -48,8 +50,11 @@ ('share/' + pdata.package_name, ['package.xml']), # Need to download weights separately, here in weights folder (os.path.join('share', pdata.package_name, 'weights'), glob('weights/*.ckpt')), - (os.path.join('share', pdata.package_name, 'config'), glob('config/*.yaml')), + (os.path.join('share', pdata.package_name, 'launch/params'), glob('launch/params/*.yaml')), + (os.path.join('share', pdata.package_name, 'launch/params/pose_estimators'), + glob('launch/params/pose_estimators/*.yaml')), (os.path.join('share', pdata.package_name, 'launch'), glob('launch/*.launch*')), + (os.path.join('share', pdata.package_name, 'launch/examples'), glob('launch/examples/*.launch*')), ], zip_safe=True, author=pdata.author, @@ -61,7 +66,13 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'mock_gps_node = gisnav.__main__:main' + 'px4_node = gisnav.nodes:run_px4_node', + 'ardupilot_node = gisnav.nodes:run_ardupilot_node', + 'mavros_node = gisnav.nodes:run_mavros_node', + 'mock_gps_node = gisnav.nodes:run_mock_gps_node', + 'map_node = gisnav.nodes:run_map_node', + 'bbox_node = gisnav.nodes:run_bbox_node', + 'pose_estimation_node = gisnav.nodes:run_pose_estimation_node', ], }, ) diff --git a/gisnav/autopilots/__init__.py b/test/launch/__init__.py similarity index 100% rename from gisnav/autopilots/__init__.py rename to test/launch/__init__.py diff --git a/test/launch/test_ardupilot_launch.py b/test/launch/test_ardupilot_launch.py new file mode 100644 index 00000000..b6d58329 --- /dev/null +++ b/test/launch/test_ardupilot_launch.py @@ -0,0 +1,161 @@ +"""Tests ArduPilot launch""" +import unittest +import os +import time +from typing import List, Tuple + +import rclpy +import pytest +from rclpy.node import Node +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest +from geometry_msgs.msg import PoseStamped, Quaternion +from mavros_msgs.msg import HomePosition, MountControl, Altitude +from sensor_msgs.msg import NavSatFix, CameraInfo, Image +from std_msgs.msg import Float32 +from geographic_msgs.msg import BoundingBox, GeoPointStamped, GeoPoseStamped +from gisnav_msgs.msg import OrthoImage3D + + +@pytest.mark.launch_test +def generate_test_description(): + """Generates a PX4 launch description""" + dirname = os.path.dirname(__file__) + filename = os.path.join(dirname, '../../launch/ardupilot.launch.py') + ld = IncludeLaunchDescription(PythonLaunchDescriptionSource(filename)) + return LaunchDescription([ + ld, + ReadyToTest(), + ]) + + +class TestArduPilotLaunch(unittest.TestCase): + """Test that all nodes initialize with correct ROS topics""" + + GISNAV_TOPIC_NAMES_AND_TYPES = [ + ('/gisnav/orthoimage_3d', OrthoImage3D), + ('/gisnav/bounding_box', BoundingBox), + ('/gisnav/vehicle_geopose', GeoPoseStamped), + ('/gisnav/vehicle_geopose/estimate', GeoPoseStamped), + ('/gisnav/vehicle_altitude', Altitude), + ('/gisnav/vehicle_altitude/estimate', Altitude), + ('/gisnav/gimbal_quaternion', Quaternion), + ('/gisnav/home_geopoint', GeoPointStamped), + ('/gisnav/terrain_altitude', Altitude), + ('/gisnav/terrain_geopoint', GeoPointStamped), + ('/gisnav/egm96_height', Float32) + ] + """List of GISNav internal topic names and types""" + + CAMERA_TOPIC_NAMES_AND_TYPES = [ + ('/camera/camera_info', CameraInfo), + ('/camera/image_raw', Image) + ] + """List of camera topic names and types""" + + AUTOPILOT_TOPIC_NAMES_AND_TYPES = [ + ('/mavros/global_position/global', NavSatFix), + ('/mavros/local_position/pose', PoseStamped), + ('/mavros/home_position/home', HomePosition), + ('/mavros/mount_control/command', MountControl), + # ('/mavros/gps_input/gps_input', GPSINPUT) # not currently used - uses UDP socket instead of MAVROS topic + ] + """List of autopilot topic names and types""" + + TOPIC_NAMES_AND_TYPES = GISNAV_TOPIC_NAMES_AND_TYPES + CAMERA_TOPIC_NAMES_AND_TYPES \ + + AUTOPILOT_TOPIC_NAMES_AND_TYPES + """List of all expected topic names and types""" + + NODE_NAMES_AND_NAMESPACES = { + ('mock_gps_node', '/'), + ('bbox_node', '/'), + ('map_node', '/'), + ('pose_estimation_node', '/'), + ('ardupilot_node', '/'), + } + """List of tuples of node names and namespaces""" + + def __init__(self, *args, **kwargs): + """Initializer override for declaring attributes used in the test case""" + super(TestArduPilotLaunch, self).__init__(*args, **kwargs) + self.test_node = None + + def _get_names_and_namespaces_within_timeout(self, timeout: int = 5) -> List[Tuple[str, str]]: + """Returns node names and namespaces found within timeout period + + :param timeout: Timeout in seconds + :return: List of tuples containing node name and type + :raise: :class:`.AssertionError` if names and namespaces are not returned within :param timeout_sec: + """ + names_and_namespaces = None + end_time = time.time() + timeout + while time.time() < end_time: # and names_and_namespaces is None: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + names_and_namespaces = self.test_node.get_node_names_and_namespaces() + + assert names_and_namespaces is not None, f'Could not get node names and namespaces within {timeout}s timeout.' + return names_and_namespaces + + def _get_topic_names_and_types_within_timeout(self, timeout: int = 5) -> List[Tuple[str, str]]: + """Returns topic names and types found within timeout period + + :param timeout: Timeout in seconds + :return: List of tuples containing topic name and type + :raise: :class:`.AssertionError` if names and types are not returned within :param timeout_sec: + """ + names_and_types = None + end_time = time.time() + timeout + while time.time() < end_time: # and names_and_types is None: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + names_and_types = self.test_node.get_topic_names_and_types() + + assert names_and_types is not None, f'Could not get topic names and types within {timeout}s timeout.' + return names_and_types + + @classmethod + def setUpClass(cls): + """Initialize ROS context""" + rclpy.init() + + @classmethod + def tearDownClass(cls): + """Shutdown ROS context""" + rclpy.shutdown() + + def setUp(self) -> None: + """Creates a ROS node for testing""" + self.test_node = Node('test_node') + + def tearDown(self) -> None: + """Destroys the ROS node""" + self.test_node.destroy_node() + + def test_node_names_and_namespaces(self): + """Tests that nodes are running with correct name and namespace""" + names, _ = tuple(zip(*self.NODE_NAMES_AND_NAMESPACES)) + + found_names_and_namespaces = self._get_names_and_namespaces_within_timeout(10) + found_names, found_namespaces = tuple(zip(*found_names_and_namespaces)) + + assert set(names).issubset(found_names), f'Not all nodes ({names}) were discovered ({found_names}).' + for name, namespace in self.NODE_NAMES_AND_NAMESPACES: + self.assertEqual(namespace, dict(found_names_and_namespaces).get(name, None)) + + def test_topic_names_and_types(self): + """Tests that the nodes subscribe to and publish the expected ROS topics""" + names, _ = tuple(zip(*self.TOPIC_NAMES_AND_TYPES)) + + found_names_and_types = self._get_topic_names_and_types_within_timeout() + found_names, found_types = tuple(zip(*found_names_and_types)) + + assert set(names).issubset(found_names), f'Not all topics ({names}) were discovered ({found_names}).' + for name, type_ in self.TOPIC_NAMES_AND_TYPES: + types = dict(found_names_and_types).get(name) + assert types is not None + self.assertEqual(type_.__class__.__name__.replace('Metaclass_', ''), types[0].split('/')[-1]) + + +if __name__ == '__main__': + unittest.main() diff --git a/test/launch/test_px4_launch.py b/test/launch/test_px4_launch.py new file mode 100644 index 00000000..a17accf0 --- /dev/null +++ b/test/launch/test_px4_launch.py @@ -0,0 +1,163 @@ +"""Tests PX4 launch""" +import unittest +import os +import time +from typing import List, Tuple + +import rclpy +import pytest +from rclpy.node import Node +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.actions import ReadyToTest +from std_msgs.msg import Float32 +from sensor_msgs.msg import CameraInfo, Image +from geometry_msgs.msg import Quaternion +from geographic_msgs.msg import BoundingBox, GeoPointStamped, GeoPoseStamped +from mavros_msgs.msg import Altitude +from gisnav_msgs.msg import OrthoImage3D +from px4_msgs.msg import VehicleGlobalPosition, VehicleLocalPosition, VehicleAttitude, GimbalDeviceSetAttitude, \ + SensorGps + + +@pytest.mark.launch_test +def generate_test_description(): + """Generates a PX4 launch description""" + dirname = os.path.dirname(__file__) + filename = os.path.join(dirname, '../../launch/px4.launch.py') + ld = IncludeLaunchDescription(PythonLaunchDescriptionSource(filename)) + return LaunchDescription([ + ld, + ReadyToTest(), + ]) + + +class TestPX4Launch(unittest.TestCase): + """Test that all nodes initialize with correct ROS topics""" + + GISNAV_TOPIC_NAMES_AND_TYPES = [ + ('/gisnav/orthoimage_3d', OrthoImage3D), + ('/gisnav/bounding_box', BoundingBox), + ('/gisnav/vehicle_geopose', GeoPoseStamped), + ('/gisnav/vehicle_geopose/estimate', GeoPoseStamped), + ('/gisnav/vehicle_altitude', Altitude), + ('/gisnav/vehicle_altitude/estimate', Altitude), + ('/gisnav/gimbal_quaternion', Quaternion), + ('/gisnav/home_geopoint', GeoPointStamped), + ('/gisnav/terrain_altitude', Altitude), + ('/gisnav/terrain_geopoint', GeoPointStamped), + ('/gisnav/egm96_height', Float32) + ] + """List of GISNav internal topic names and types""" + + CAMERA_TOPIC_NAMES_AND_TYPES = [ + ('/camera/camera_info', CameraInfo), + ('/camera/image_raw', Image) + ] + """List of camera topic names and types""" + + AUTOPILOT_TOPIC_NAMES_AND_TYPES = [ + ('/fmu/vehicle_global_position/out', VehicleGlobalPosition), + ('/fmu/vehicle_local_position/out', VehicleLocalPosition), + ('/fmu/vehicle_attitude/out', VehicleAttitude), + ('/fmu/gimbal_device_set_attitude/out', GimbalDeviceSetAttitude), + ('/fmu/sensor_gps/in', SensorGps) + ] + """List of autopilot topic names and types""" + + TOPIC_NAMES_AND_TYPES = GISNAV_TOPIC_NAMES_AND_TYPES + CAMERA_TOPIC_NAMES_AND_TYPES \ + + AUTOPILOT_TOPIC_NAMES_AND_TYPES + """List of all expected topic names and types""" + + NODE_NAMES_AND_NAMESPACES = { + ('mock_gps_node', '/'), + ('bbox_node', '/'), + ('map_node', '/'), + ('pose_estimation_node', '/'), + ('px4_node', '/'), + } + """List of tuples of node names and namespaces""" + + def __init__(self, *args, **kwargs): + """Initializer override for declaring attributes used in the test case""" + super(TestPX4Launch, self).__init__(*args, **kwargs) + self.test_node = None + + def _get_names_and_namespaces_within_timeout(self, timeout: int = 5) -> List[Tuple[str, str]]: + """Returns node names and namespaces found within timeout period + + :param timeout: Timeout in seconds + :return: List of tuples containing node name and type + :raise: :class:`.AssertionError` if names and namespaces are not returned within :param timeout_sec: + """ + names_and_namespaces = None + end_time = time.time() + timeout + while time.time() < end_time: # and names_and_namespaces is None: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + names_and_namespaces = self.test_node.get_node_names_and_namespaces() + + assert names_and_namespaces is not None, f'Could not get node names and namespaces within {timeout}s timeout.' + return names_and_namespaces + + def _get_topic_names_and_types_within_timeout(self, timeout: int = 5) -> List[Tuple[str, str]]: + """Returns topic names and types found within timeout period + + :param timeout: Timeout in seconds + :return: List of tuples containing topic name and type + :raise: :class:`.AssertionError` if names and types are not returned within :param timeout_sec: + """ + names_and_types = None + end_time = time.time() + timeout + while time.time() < end_time: # and names_and_types is None: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + names_and_types = self.test_node.get_topic_names_and_types() + + assert names_and_types is not None, f'Could not get topic names and types within {timeout}s timeout.' + return names_and_types + + @classmethod + def setUpClass(cls): + """Initialize ROS context""" + rclpy.init() + + @classmethod + def tearDownClass(cls): + """Shutdown ROS context""" + rclpy.shutdown() + + def setUp(self) -> None: + """Creates a ROS node for testing""" + self.test_node = Node('test_node') + + def tearDown(self) -> None: + """Destroys the ROS node""" + self.test_node.destroy_node() + + def test_node_names_and_namespaces(self): + """Tests that nodes are running with correct name and namespace""" + names, _ = tuple(zip(*self.NODE_NAMES_AND_NAMESPACES)) + + found_names_and_namespaces = self._get_names_and_namespaces_within_timeout(10) + found_names, found_namespaces = tuple(zip(*found_names_and_namespaces)) + + assert set(names).issubset(found_names), f'Not all nodes ({names}) were discovered ({found_names}).' + for name, namespace in self.NODE_NAMES_AND_NAMESPACES: + self.assertEqual(namespace, dict(found_names_and_namespaces).get(name, None)) + + def test_topic_names_and_types(self): + """Tests that the nodes subscribe to and publish the expected ROS topics""" + names, _ = tuple(zip(*self.TOPIC_NAMES_AND_TYPES)) + + found_names_and_types = self._get_topic_names_and_types_within_timeout() + found_names, found_types = tuple(zip(*found_names_and_types)) + + assert set(names).issubset(found_names), f'Not all topics ({names}) were discovered ({found_names}).' + for name, type_ in self.TOPIC_NAMES_AND_TYPES: + types = dict(found_names_and_types).get(name) + assert types is not None + self.assertEqual(type_.__class__.__name__.replace('Metaclass_', ''), types[0].split('/')[-1]) + + +if __name__ == '__main__': + unittest.main() diff --git a/test/sitl/sitl_test_mock_gps_node.py b/test/sitl/sitl_test_mock_gps_node.py index 72ec39b8..eefdc445 100644 --- a/test/sitl/sitl_test_mock_gps_node.py +++ b/test/sitl/sitl_test_mock_gps_node.py @@ -9,7 +9,7 @@ from mavsdk import System from mavsdk.log_files import LogFilesResult, LogFilesError -DOCKER_CONTAINERS = ['gisnav-docker_mapserver_1', 'gisnav-docker_px4-sitl_1'] +DOCKER_CONTAINERS = ['gisnav-docker_mapserver_1', 'gisnav-docker_sitl_1'] SYS_ADDR = 'udp://0.0.0.0:14550' MISSION_FILE = os.path.join(os.path.dirname(__file__), '../assets/ksql_airport.plan') MAVLINK_CONNECTION_TIMEOUT_SEC = 15 diff --git a/test/test_mock_gps_node.py b/test/test_mock_gps_node.py deleted file mode 100644 index 50f94e39..00000000 --- a/test/test_mock_gps_node.py +++ /dev/null @@ -1,167 +0,0 @@ -"""Tests :class:`.MockGPSNode""" -import multiprocessing.pool -import unittest -import os -import time - -import rclpy -import pytest - -from unittest.mock import patch -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_testing.actions import ReadyToTest - -from gisnav.data import PackageData -from gisnav.nodes.mock_gps_node import MockGPSNode - - -@pytest.mark.launch_test -def generate_test_description(): - """Generates a :class:`.MockGPSNode` launch description""" - dirname = os.path.dirname(__file__) - filename = os.path.join(dirname, '../launch/test_mock_gps_node.launch.py') - ld = IncludeLaunchDescription(PythonLaunchDescriptionSource(filename)) - - return LaunchDescription([ - ld, - ReadyToTest(), - ]) - - -class TestInit(unittest.TestCase): - """Tests that :class:`.MockGPSNode` initializes correctly""" - - NODE_NAME = 'mock_gps_node' - """This node name should match with node name in mock_gps_node.launch.py""" - - NODE_NAMESPACE = '/' - """Expected node namespace""" - - SUBSCRIBER_NAMES_AND_TYPES = [ - ('/fmu/gimbal_device_set_attitude/out', ['px4_msgs/msg/GimbalDeviceSetAttitude']), - ('/fmu/vehicle_attitude/out', ['px4_msgs/msg/VehicleAttitude']), - ('/fmu/vehicle_global_position/out', ['px4_msgs/msg/VehicleGlobalPosition']), - ('/fmu/vehicle_local_position/out', ['px4_msgs/msg/VehicleLocalPosition']), - ('/camera/camera_info', ['sensor_msgs/msg/CameraInfo']), - ('/camera/image_raw', ['sensor_msgs/msg/Image']) - ] - """Expected node subscribe topic names and message types""" - - MIN_MATCH_ALTITUDE = 80 - """Expected misc.min_match_altitude setting in ``test_typhoon_h480__ksql_airport.yaml`` file""" - - @staticmethod - def mock_setup_wms_pool(self) -> multiprocessing.pool.Pool: - """Returns a mock pool without attempting to connect to WMS endpoint""" - return multiprocessing.pool.Pool() - - @classmethod - def setUpClass(cls): - """Initialize ROS context""" - rclpy.init() - - @classmethod - def tearDownClass(cls): - """Shutdown ROS context""" - rclpy.shutdown() - - @patch.multiple(MockGPSNode, - _setup_wms_pool=mock_setup_wms_pool - ) - def setUp(self) -> None: - """Creates the ROS node""" - dirname = os.path.dirname(__file__) - filename = os.path.join(dirname, '../package.xml') - package_data = PackageData.parse_package_data(os.path.abspath(filename)) - self.node = MockGPSNode(self.NODE_NAME, get_package_share_directory(package_data.package_name)) - - def tearDown(self) -> None: - """Destroys the ROS node""" - self.node.destroy_timers() - self.node.terminate_pools() - self.node.destroy_node() - - def test_node_names_and_namespaces(self): - """Tests that :class:`.MockGPSNode` is running with the correct name and namespace""" - names_and_namespaces = None - timeout_sec = 2 - end_time = time.time() + timeout_sec - while time.time() < end_time and names_and_namespaces is None: - rclpy.spin_once(self.node, timeout_sec=0.1) - names_and_namespaces = self.node.get_node_names_and_namespaces() - - assert names_and_namespaces is not None, f'Could not determine node name and namespace within timeout of ' \ - f'{timeout_sec} seconds.' - - found = False - for name, namespace in names_and_namespaces: - if name == self.NODE_NAME: - assert namespace == self.NODE_NAMESPACE, f'Node namespace "{namespace}" did not match expectation ' \ - f'"{self.NODE_NAMESPACE}".' - found = True - - assert found, f'Could not find expected name "{self.NODE_NAME}" in names and namespaces ' \ - f'{names_and_namespaces}.' - - def test_subscriber_names_and_types(self): - """Tests that parent class :class:`.BaseNode` subscribes to the correct ROS topics""" - subscriber_names_and_types = None - timeout_sec = 2 - end_time = time.time() + timeout_sec - while time.time() < end_time and subscriber_names_and_types is None: - rclpy.spin_once(self.node, timeout_sec=0.1) - subscriber_names_and_types = self.node.get_subscriber_names_and_types_by_node(self.NODE_NAME, - self.NODE_NAMESPACE) - - assert subscriber_names_and_types is not None, f'Could not determine subscriber names and types within ' \ - f'timeout of {timeout_sec} seconds.' - - for name, type_ in subscriber_names_and_types: - assert (name, type_) in self.SUBSCRIBER_NAMES_AND_TYPES, f'Unexpected subscription for topic name ' \ - f'"{name}" with type "{type_}".' - - for name, type_ in self.SUBSCRIBER_NAMES_AND_TYPES: - assert (name, type_) in subscriber_names_and_types, f'Expected subscription for topic name "{name}" with ' \ - f'type "{type_}" was not found in node subscriptions.' - - def test_ros_parameter_defaults(self): - """Tests that ROS parameters are declared from defaults as expected - - Assumes node is initialized with ``test_typhoon_h480__ksql_airport.yaml`` ROS parameter file - """ - rclpy.spin_once(self.node, timeout_sec=3) - - wms_version = self.node.get_parameter('wms.version').get_parameter_value().string_value - assert wms_version == self.node.ROS_D_WMS_VERSION, f'WMS version {wms_version} did not match expected default '\ - f'value {self.node.ROS_D_WMS_VERSION}' - - max_pitch = self.node.get_parameter('misc.max_pitch').get_parameter_value().integer_value - assert max_pitch == self.node.ROS_D_MISC_MAX_PITCH, f'Max pitch {max_pitch} did not match expected default '\ - f'value {self.node.ROS_D_MISC_MAX_PITCH}' - - min_match_altitude = self.node.get_parameter('misc.min_match_altitude').get_parameter_value().integer_value - assert min_match_altitude == self.MIN_MATCH_ALTITUDE, f'Min pose estimation altitude {min_match_altitude} did '\ - f'not match expected value {self.MIN_MATCH_ALTITUDE}' - - map_max_pitch = self.node.get_parameter('map_update.max_pitch').get_parameter_value().integer_value - assert map_max_pitch == self.node.ROS_D_MAP_UPDATE_MAX_PITCH, f'Max map update pitch {map_max_pitch} did not ' \ - f'match expected default value ' \ - f'{self.node.ROS_D_MAP_UPDATE_MAX_PITCH}' - - pose_estimator_params_file = self.node.get_parameter('pose_estimator.params_file')\ - .get_parameter_value().string_value - assert pose_estimator_params_file == self.node.ROS_D_POSE_ESTIMATOR_PARAMS_FILE, \ - f'Pose estimator parameter file name "{pose_estimator_params_file}" did not match expected default value ' \ - f'"{self.node.ROS_D_POSE_ESTIMATOR_PARAMS_FILE}".' - - export_position = self.node.get_parameter('debug.export_position').get_parameter_value().string_value - assert export_position == self.node.ROS_D_DEBUG_EXPORT_POSITION, \ - f'Position export (DEBUG) filename "{export_position}" did not match expected default value ' \ - f'"{self.node.ROS_D_DEBUG_EXPORT_POSITION}".' - - -if __name__ == '__main__': - unittest.main() \ No newline at end of file