Merge pull request #20 from hmakelin/hmakelin-autopilot-bridge

Add module to support different autopilots

Author: hmakelin

README.md

@@ -27,23 +27,20 @@ You will need to have [NVIDIA Container Toolkit][2] for Docker installed.
 ```bash
 git clone https://github.com/hmakelin/gisnav-docker.git
 cd gisnav-docker
-docker-compose build px4-sitl
+docker-compose build sitl
 ```
 
-> **Note** The build for the `px4-sitl` image takes a long time, especially if you are building it for the first time.
+> **Note** The build for the `sitl` image takes a long time.
 
-Once the `px4-sitl` image has been built, run the `mapserver` and `px4-sitl` services:
+Once the `sitl` image has been built, run the `mapserver` and `px4-sitl` services:
 
 ```bash
-docker-compose up -d mapserver px4-sitl
+docker-compose up -d mapserver sitl
 ```
 
 > **Note**: 
-> * The `mapserver` container needs to download roughly 1 GB of high-resolution aerial imagery, so it may take some 
->   time until it starts serving the WMS endpoint.
-> * The `px4-sitl` container should pop up [Gazebo][4] and [QGroundControl][5] automatically once ran. The Gazebo 
->   window may take several minutes to appear, while QGroundControl should appear in a few seconds after running the 
->   container.
+> The `mapserver` container needs to download roughly 1 GB of high-resolution aerial imagery, so it may take some 
+> time until it starts serving the WMS endpoint.
 
 [4]: https://gazebosim.org/home
 

config/test_typhoon_h480__ksql_airport.yaml

@@ -11,6 +11,9 @@ mock_gps_node:
       image_format: 'image/jpeg'
       request_timeout: 10
     misc:
+      autopilot: 'gisnav.autopilots.px4_micrortps.PX4microRTPS'
+      image_topic: '/camera/image_raw'
+      camera_info_topic: '/camera/camera_info'
       mock_gps_selection: 1
       attitude_deviation_threshold: 10
       #max_pitch: 30

config/typhoon_h480__ksql_airport.yaml

@@ -10,6 +10,9 @@ mock_gps_node:
       image_format: 'image/jpeg'
       request_timeout: 10
     misc:
+      autopilot: 'gisnav.autopilots.px4_micrortps.PX4microRTPS'
+      image_topic: '/camera/image_raw'
+      camera_info_topic: '/camera/camera_info'
       attitude_deviation_threshold: 10 # degrees
       max_pitch: 30  # 30  # Used by _should_match
       min_match_altitude: 50

config/typhoon_h480__ksql_airport_ardupilot.yaml

@@ -0,0 +1,31 @@
+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'
+      image_topic: '/camera/image_raw'
+      camera_info_topic: '/camera/camera_info'
+      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: ''

docs/index.rst

@@ -5,33 +5,34 @@ Welcome to GISNav's developer documentation!
 
 This site contains the following pages:
 
-    * `Get Started <pages/get_started.html>`_
+* `Get Started <pages/get_started.html>`_
 
-        A quick demonstration of GNSS-free visual navigation with GISNav's :class:`.MockGPSNode`
+    A quick demonstration of GNSS-free visual navigation with GISNav's :class:`.MockGPSNode`
 
-    * `Setup <pages/setup.html>`_
+* `Setup <pages/setup.html>`_
 
-        Instructions on how to to setup your own PX4 development and simulation environment
+    Instructions on how to to setup your own PX4 development and simulation environment
 
-    * `Developer Guide <pages/developer_guide.html>`_
+* `Developer Guide <pages/developer_guide.html>`_
 
-        Instructions on how to integrate GISNav with your own project and how to extend its functionality
+    Instructions on how to integrate GISNav with your own project and how to extend its functionality
 
-    * `API Documentation <pages/api_documentation.html>`_
+* `API Documentation <pages/api_documentation.html>`_
 
-        GISNav public API reference for developers
+    GISNav public API reference for developers
 
-    * `Contribute <pages/contribute.html>`_
+* `Contribute <pages/contribute.html>`_
 
-        Contributions to GISNav are welcome! Please see this page for guidance.
+    Contributions to GISNav are welcome! Please see this page for guidance.
 
 .. seealso::
 
-    GISNav is built on ROS 2 and is designed to work with PX4 Autopilot. You may also be interested in their
+    GISNav is built on ROS 2 and supports PX4 and ArduPilot. You may also be interested in their
     documentation:
 
-        * `ROS 2 Documentation <https://docs.ros.org/>`_
-        * `PX4 Autopilot User Guide <https://docs.px4.io/master/en/>`_
+    * `ROS 2 Documentation <https://docs.ros.org/>`_
+    * `PX4 Autopilot User Guide <https://docs.px4.io/master/en/>`_
+    * `ArduPilot Documentation <https://ardupilot.org/ardupilot/>`_
 
 .. toctree::
    :maxdepth: 2

docs/pages/developer_guide.rst

@@ -62,32 +62,6 @@ The attributes in the :class:`.Position` input to the :meth:`.BaseNode.publish`
 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`.
 
-.. _ROS 2 Topic Configuration:
-
-ROS 2 Topic Configuration
-____________________________________________________
-To compute the position and attitude estimates, the :class:`.BaseNode` class 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 <https://docs.px4.io/master/en/middleware/micrortps.html#supported-uorb-messages>`_ section of
-the PX4 User Guide.
-
-.. seealso::
-    `PX4-ROS 2 bridge <https://docs.px4.io/master/en/ros/ros2_comm.html>`_ for more information on the PX4-ROS 2 bridge
-
 Modify ROS Parameters
 ____________________________________________________
 ROS parameter server is used to manage the configuration of the :class:`.BaseNode` instance at runtime. An example
@@ -299,6 +273,47 @@ arguments in your ROS YAML parameter file:
 
 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 <https://docs.px4.io/master/en/middleware/micrortps.html#supported-uorb-messages>`_ section of
+the PX4 User Guide.
+
+.. seealso::
+    `PX4-ROS 2 bridge <https://docs.px4.io/master/en/ros/ros2_comm.html>`_ for more information on the PX4-ROS 2 bridge
+
 Testing
 ====================================================
 Unit & ROS 2 integration tests

docs/pages/setup.rst

@@ -17,9 +17,12 @@ Prerequisites
   CUDA versions with the ``nvidia-smi`` command line utility. If you don't have it installed, follow the `NVIDIA CUDA
   Installation Guide for Linux <https://docs.nvidia.com/cuda/cuda-installation-guide-linux/index.html>`_.
 
+.. _PX4 Autopilot:
+
 PX4 Autopilot
 ===================================================
-PX4 **v1.13** is the only autopilot that is currently supported by GISNav.
+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
 <https://docs.px4.io/master/en/simulation/ros_interface.html>`_ with `Fast DDS
@@ -78,7 +81,7 @@ better either through the PX4 shell, through QGroundControl, or in the
 .. _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
@@ -99,7 +102,7 @@ manual repetition:
 .. _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.
 
@@ -156,7 +159,7 @@ folders. You must configure the following send and receive flags for the followi
       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:
 
@@ -194,7 +197,7 @@ before rebuilding again:
     want to restart ``micrortps_agent``.
 
 gscam
-===================================================
+___________________________________________________
 
 The ``typhoon_h480`` build target for Gazebo SITL supports UDP `video streaming
 <https://docs.px4.io/master/en/simulation/gazebo.html#video-streaming>`_ . Here we will use ``gscam`` to publish the
@@ -221,14 +224,41 @@ with the provided configuration files:
     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 <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>`_
+
+As of ``gisnav`` v0.61, :class:`.MockGPSNode` can be used in the ArduPilot SITL simulation included in the
+`gisnav-docker <https://github.com/hmakelin/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
@@ -392,28 +422,6 @@ Test your MapServer WMS service by opening the capabilities XML in your browser:
 
     firefox "http://localhost:80/?map=/etc/mapserver/wms.map&service=WMS&request=GetCapabilities"
 
-Docker commit the MapServer container with preloaded maps
---------------------------------------------------------------
-To upload the image preloaded with maps to Docker Hub, first commit the container to an image:
-
-.. code-block:: bash
-
-    export CONTAINER_ID=$(docker ps -q -f name=$CONTAINER_NAME)
-    export IMAGE_NAME=gisnav-mapserver
-    docker commit $CONTAINER_ID $IMAGE_NAME
-
-Then push the image to Docker Hub:
-
-.. note::
-    Replace ``hmakelin``  with your own user account name, and ``latest`` with your own tag
-
-.. code-block:: bash
-
-    export DOCKER_HUB_USER=hmakelin
-    export tag=latest
-    docker image tag $IMAGE_NAME $DOCKER_HUB_USER/$IMAGE_NAME:$tag
-    docker image push $DOCKER_HUB_USER/$IMAGE_NAME
-
 GISNav
 ===================================================
 
@@ -452,11 +460,19 @@ Build the GISNav package:
 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

gisnav/__main__.py

@@ -2,6 +2,7 @@
 import cProfile
 import pstats
 import rclpy
+import sys
 
 from ament_index_python.packages import get_package_share_directory
 
@@ -26,7 +27,8 @@ def main(args=None):
     mock_gps_node = None
     try:
         rclpy.init(args=args)
-        mock_gps_node = MockGPSNode('mock_gps_node', get_package_share_directory(package_name))
+        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}')
@@ -40,6 +42,7 @@ def main(args=None):
     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()