Merge pull request #21 from hmakelin/hmakelin-autopilot-refactor

Use ROS remapping for camera info and image topics

Author: hmakelin

config/test_typhoon_h480__ksql_airport.yaml

@@ -12,8 +12,6 @@ mock_gps_node:
       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

@@ -11,8 +11,6 @@ mock_gps_node:
       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

@@ -11,8 +11,6 @@ mock_gps_node:
       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

docs/conf.py

@@ -29,7 +29,7 @@
 # -- Project information -----------------------------------------------------
 
 project = package_data.package_name
-copyright = f'2021, {package_data.author}'
+copyright = f'2022, {package_data.author}'
 author = package_data.author
 
 # -- General configuration ---------------------------------------------------
@@ -69,7 +69,16 @@
         "image_light": "img/logo.png",
         "image_dark": "img/logo_inverted.png",
     },
-    "collapse_navigation": True
+    "collapse_navigation": True,
+    "icon_links": [
+        {
+            "name": "GitHub",
+            "url": "https://github.com/hmakelin/gisnav",
+            "icon": "fab fa-github",
+            "type": "fontawesome",
+        }
+    ],
+    "icon_links_label": "Quick Links",
 }
 
 # Make version number accessible in .rst files

docs/pages/api_documentation.rst

@@ -8,6 +8,10 @@ will only be interested in the :class:`.BaseNode` and :class:`.PoseEstimator` cl
 
 .. _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.

docs/pages/developer_guide.rst

@@ -6,6 +6,10 @@ Developer Guide
     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.
 
@@ -314,6 +318,17 @@ 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
 
+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

docs/pages/get_started.rst

@@ -1,7 +1,9 @@
 **************************************************
 Get Started
 **************************************************
+..
+    Skip GitHub video link (:start-after: .mp4)
+
 .. include:: ../../README.md
     :parser: myst_parser.sphinx_
-
-
+    :start-after: .mp4

gisnav/autopilots/ardupilot_mavros.py

@@ -41,16 +41,13 @@ class ArduPilotMAVROS(Autopilot):
     }
     """ROS topics to subscribe"""
 
-    def __init__(self, node: rclpy.node.Node, camera_info_topic: str, image_topic: str,
-                 image_callback: Callable[[Image], None]) -> None:
+    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 camera_info_topic: ROS camera info topic to subscribe to
-        :param image_topic: ROS camera image (raw) topic to subscribe to
         :param image_callback: Callback function for camera image
         """
-        super().__init__(node, camera_info_topic, image_topic, image_callback)
+        super().__init__(node, image_callback)
 
         # TODO: copy to shared folder and use relative path
         self._egm96 = GeoidPGM('/usr/share/GeographicLib/geoids/egm96-5.pgm', kind=-3)

gisnav/autopilots/autopilot.py

@@ -24,27 +24,28 @@ class Autopilot(ABC):
     TELEMETRY_EXPIRATION_LIMIT = 1e6
     """Expiration period in usec for vehicle state (if telemetry data is older than this it should not be used)"""
 
-    def __init__(self, node: rclpy.node.Node, camera_info_topic: str, image_topic: str,
-                 on_image_callback: Callable[[ImageData], None]) -> None:
+    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 camera_info_topic: ROS camera info topic to subscribe to
-        :param image_topic: ROS topic for :class:`.sensor_msgs.msg.Image`
         :param on_image_callback: Callback function for camera image
         """
         self._node = node
         self._topics = {}
 
         # Subscribe to camera data topic
         self.camera_data = None
-        self._camera_info_topic = camera_info_topic
-        self._subscribe(self._camera_info_topic, CameraInfo, self._camera_info_callback,
+        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._image_topic = image_topic
-        self._subscribe(self._image_topic, Image, on_image_callback, rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value)
+        self._subscribe(self.IMAGE_RAW_TOPIC, Image, on_image_callback, rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value)
 
     @property
     def _node(self) -> rclpy.node.Node:
@@ -76,26 +77,6 @@ def camera_data(self, value: Optional[CameraData]) -> None:
         assert_type(value, get_args(Optional[CameraData]))
         self.__camera_data = value
 
-    @property
-    def _camera_info_topic(self) -> str:
-        """CameraInfo ROS topic name."""
-        return self.__camera_info_topic
-
-    @_camera_info_topic.setter
-    def _camera_info_topic(self, value: str) -> None:
-        assert_type(value, str)
-        self.__camera_info_topic = value
-
-    @property
-    def _image_topic(self) -> str:
-        """Image ROS topic name."""
-        return self.__image_topic
-
-    @_image_topic.setter
-    def _image_topic(self, value: str) -> None:
-        assert_type(value, str)
-        self.__image_topic = value
-
     def _subscribe(self, topic_name: str, class_: type, callback: Callable, qos: rclpy.qos.QoSProfile) -> None:
         """Subscribes to ROS topic
 
@@ -137,10 +118,10 @@ def _camera_info_callback(self, msg: CameraInfo) -> None:
             return None
         else:
             self.camera_data = CameraData(k=msg.k.reshape((3, 3)), dim=Dim(msg.height, msg.width))
-            camera_info_topic = self._topics.get(self._camera_info_topic, {}).get(self._TOPICS_SUBSCRIBER_KEY, None)
-            if camera_info_topic is not None:
+            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_topic.destroy()
+                camera_info_sub.destroy()
 
     @property
     @abstractmethod

gisnav/autopilots/px4_micrortps.py

@@ -40,16 +40,13 @@ class PX4microRTPS(Autopilot):
     }
     """ROS topics to subscribe"""
 
-    def __init__(self, node: rclpy.node.Node, camera_info_topic: str, image_topic: str,
-                 image_callback: Callable[[Image], None]) -> None:
+    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 camera_info_topic: ROS camera info topic to subscribe to
-        :param image_topic: ROS camera image (raw) topic to subscribe to
         :param image_callback: Callback function for camera image
         """
-        super().__init__(node, camera_info_topic, image_topic, image_callback)
+        super().__init__(node, image_callback)
 
         self._setup_subscribers()