Bring back micro-ros-agent and SensorGps for mock GPS node

hmakelin · Apr 7, 2023 · f8d9f76 · f8d9f76
1 parent efc9036
commit f8d9f76
Show file tree

Hide file tree

Showing 25 changed files with 677 additions and 130 deletions.
diff --git a/docker/Makefile b/docker/Makefile
@@ -61,7 +61,7 @@ build-offboard-sitl-dev-%: build-offboard-sitl-middleware-%
 . PHONY: $(addprefix up-offboard-sitl-middleware-, $(AUTOPILOTS))
 up-offboard-sitl-middleware-%:
 	@if [ "$*" = "px4" ]; then \
-		docker compose up -d mavros; \
+		docker compose up -d micro-ros-agent mavros; \
 	elif [ "$*" = "ardupilot" ]; then \
 		docker compose -f docker-compose.yaml -f docker-compose.mavros-sitl-ardupilot.yaml up -d mavros; \
 	else \
@@ -71,7 +71,7 @@ up-offboard-sitl-middleware-%:
 . PHONY: $(addprefix build-offboard-sitl-middleware-, $(AUTOPILOTS))
 build-offboard-sitl-middleware-%:
 	@if [ "$*" = "px4" ]; then \
-		docker compose build mavros; \
+		docker compose build micro-ros-agent mavros; \
 	elif [ "$*" = "ardupilot" ]; then \
 		docker compose -f docker-compose.yaml -f docker-compose.mavros-sitl-ardupilot.yaml build mavros; \
 	else \
@@ -83,9 +83,9 @@ build-offboard-sitl-middleware-%:
 . PHONY: $(addprefix up-companion-hil-middleware-, $(AUTOPILOTS))
 up-companion-hil-middleware-%:
 	@if [ "$*" = "px4" ]; then \
-		docker compose -f docker-compose.yaml -f docker-compose.mavros-serial.yaml up -d mavros; \
+		docker compose -f docker-compose.yaml -f docker-compose.serial.yaml up -d micro-ros-agent mavros; \
 	elif [ "$*" = "ardupilot" ]; then \
-		docker compose -f docker-compose.yaml -f docker-compose.mavros-serial.yaml up -d mavros; \
+		docker compose -f docker-compose.yaml -f docker-compose.serial.yaml up -d mavros; \
 	else \
 		echo "Unsupported target '$*' (try 'px4' or 'ardupilot')."; \
 	fi
@@ -94,9 +94,9 @@ up-companion-hil-middleware-%:
 . PHONY: $(addprefix build-companion-hil-middleware-, $(AUTOPILOTS))
 build-companion-hil-middleware-%:
 	@if [ "$*" = "px4" ]; then \
-		docker compose -f docker-compose.yaml -f docker-compose.mavros-serial.yaml build mavros; \
+		docker compose -f docker-compose.yaml -f docker-compose.serial.yaml build micro-ros-agent mavros; \
 	elif [ "$*" = "ardupilot" ]; then \
-		docker compose -f docker-compose.yaml -f docker-compose.mavros-serial.yaml build mavros; \
+		docker compose -f docker-compose.yaml -f docker-compose.serial.yaml build mavros; \
 	else \
 		echo "Unsupported target '$*' (try 'px4' or 'ardupilot')."; \
 	fi
@@ -114,7 +114,7 @@ up-demo-%: up-offboard-sitl-dev-%
 	if [ "$*" = "px4" ]; then \
 		docker compose up -d gisnav; \
 	elif [ "$*" = "ardupilot" ]; then \
-		docker compose -f docker-compose.yaml -f docker-compose.ardupilot.yaml up -d gisnav; \
+		docker compose -f docker-compose.yaml -f docker-compose.gisnav-ardupilot.yaml up -d gisnav; \
 	else \
 		echo "Unsupported target '$*' (try 'px4' or 'ardupilot')."; \
 	fi
@@ -124,7 +124,7 @@ build-demo-%: build-offboard-sitl-dev-%
 	if [ "$*" = "px4" ]; then \
 		docker compose build gisnav; \
 	elif [ "$*" = "ardupilot" ]; then \
-		docker compose -f docker-compose.yaml -f docker-compose.ardupilot.yaml build gisnav; \
+		docker compose -f docker-compose.yaml -f docker-compose.gisnav-ardupilot.yaml build gisnav; \
 	else \
 		echo "Unsupported target '$*' (try 'px4' or 'ardupilot')."; \
 	fi

diff --git a/docker/docker-compose.gisnav-ardupilot.yaml b/docker/docker-compose.gisnav-ardupilot.yaml
@@ -0,0 +1,6 @@
+version: "3.4"
+
+services:
+  # Runs Ardupilot MAVROS launch configuration instead of default PX4 micro-ros
+  gisnav:
+    command: ros2 launch gisnav ardupilot.launch.py
diff --git a/docker/docker-compose.mavros-serial.yaml → docker/docker-compose.serial.yaml b/docker/docker-compose.mavros-serial.yaml → docker/docker-compose.serial.yaml
@@ -2,3 +2,7 @@ services:
   # Connects MAVROS over serial instead of default UDP
   mavros:
     command: ros2 run mavros mavros_node --ros-args --param fcu_url:=/dev/ttyUSB0:57600
+
+  # Makes micro-ros-agent (uros) middleware connect over serial instead of default UDP
+  micro-ros-agent:
+    command: serial --dev /dev/ttyUSB0 -b 57600
diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml
@@ -51,7 +51,7 @@ services:
       context: qgc
       dockerfile: Dockerfile
     env_file:
-      - .env/.x11.env
+      - env/x11.env
     volumes:
       - /tmp/.X11-unix:/tmp/.X11-unix:ro
       - /dev/shm:/dev/shm
@@ -66,7 +66,7 @@ services:
       dockerfile: docker/gisnav/Dockerfile
     image: gisnav  # used by GitHub upload_gisnav_image CI workflow
     env_file:
-      - .env/.x11.env
+      - env/x11.env
     environment:
       - FASTRTPS_DEFAULT_PROFILES_FILE=/opt/colcon_ws/src/gisnav/docker/gisnav/disable_shared_memory.xml
     volumes:
@@ -81,14 +81,14 @@ services:
             - driver: nvidia
               count: 1
               capabilities: [ gpu ]
-    command: ros2 launch gisnav base.launch.py
+    command: ros2 launch gisnav px4.launch.py
 
   px4:
     build:
       context: px4
       dockerfile: Dockerfile
     env_file:
-      - .env/.x11.env
+      - env/x11.env
     volumes:
       - /tmp/.X11-unix:/tmp/.X11-unix:ro
       - /dev/shm:/dev/shm
@@ -110,7 +110,7 @@ services:
       context: ardupilot
       dockerfile: Dockerfile
     env_file:
-      - .env/.x11.env
+      - env/x11.env
     volumes:
       - /tmp/.X11-unix:/tmp/.X11-unix:ro
       - /dev/shm:/dev/shm

diff --git a/docker/.env/.x11.env → docker/env/x11.env b/docker/.env/.x11.env → docker/env/x11.env
diff --git a/docker/micro-ros-agent/Dockerfile b/docker/micro-ros-agent/Dockerfile
@@ -0,0 +1,11 @@
+FROM microros/micro-ros-agent:foxy
+
+LABEL maintainer="Harri Makelin <hmakelin@protonmail.com>"
+
+RUN cd /uros_ws && \
+    mkdir src && \
+    cd src && \
+    git clone https://github.com/px4/px4_msgs.git && \
+    cd .. && \
+    . /opt/ros/foxy/setup.sh && \
+    colcon build --packages-select px4_msgs
diff --git a/docker/px4/6011_typhoon_h480 b/docker/px4/6011_typhoon_h480
@@ -1,6 +1,5 @@
 # SIMULATION USE ONLY
 # GISNav PX4 mock GPS SITL simulation demo default parameters
-# PX4 v1.13
 param set-default NAV_ACC_RAD 20.0
 param set-default MPC_YAWRAUTO_MAX 22.5.0
 param set-default COM_POS_FS_DELAY 5
@@ -9,8 +8,7 @@ param set-default EKF2_GPS_P_NOISE 10
 param set-default EKF2_GPS_P_GATE 7
 param set-default EKF2_GPS_V_NOISE 3
 param set-default SENS_GPS_MASK 4
-
-param set-default MAV_USEHILGPS 1
+param set-default EKF2_REQ_EPH 10.0
 
 #param set-default EKF2_GPS_CTRL 3
 #param set-default EKF2_GPS_CHECK 12
diff --git a/docker/px4/Dockerfile b/docker/px4/Dockerfile
@@ -19,12 +19,15 @@ COPY * /
 RUN pip install lxml && \
     python3 merge_xml.py ros_camera.xml PX4-Autopilot/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/typhoon_h480/typhoon_h480.sdf.jinja
 
+# Configure XRCE-DDS bridge
+RUN python3 merge_yaml.py dds_topics.yaml PX4-Autopilot/src/modules/microdds_client/dds_topics.yaml
+
 # Make initial PX4 build (for faster startup in the future)
 RUN cd PX4-Autopilot && \
     DONT_RUN=1 make px4_sitl gazebo-classic_typhoon_h480__ksql_airport
 
 # Apply configuration files
-RUN cat 6011_typhoon_h480 >> PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480
+RUN cat 6011_typhoon_h480 >> PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes/6011_gazebo-classic_typhoon_h480
 
 RUN rm -rf /var/lib/apt/lists/* && \
     apt clean

diff --git a/docker/px4/dds_topics.yaml b/docker/px4/dds_topics.yaml
@@ -0,0 +1,3 @@
+subscriptions:
+  - topic: /fmu/in/sensor_gps
+    type: px4_msgs::msg::SensorGps
diff --git a/docker/px4/merge_yaml.py b/docker/px4/merge_yaml.py
@@ -0,0 +1,44 @@
+import argparse
+
+import yaml
+
+
+def merge_yaml_files(src_file, dest_file):
+    """
+    Merge the content of the source YAML file into the destination YAML file in-place.
+
+    :param src_file: Path to the source YAML file
+    :param dest_file: Path to the destination YAML file
+    """
+    # Read source YAML file
+    with open(src_file, "r") as src:
+        src_yaml = yaml.safe_load(src)
+
+    # Read destination YAML file
+    with open(dest_file, "r") as dest:
+        dest_yaml = yaml.safe_load(dest)
+
+    # Merge the contents of the source file into the destination file
+    dest_yaml.update(src_yaml)
+
+    # Write the merged content back to the destination file
+    with open(dest_file, "w") as dest:
+        yaml.dump(dest_yaml, dest, default_flow_style=False)
+
+
+def main():
+    """
+    Parse command-line arguments and call the merge_yaml_files function with
+    the provided file paths.
+    """
+    parser = argparse.ArgumentParser(description="Merge two YAML files")
+    parser.add_argument("src_file", help="Path to the source YAML file")
+    parser.add_argument("dest_file", help="Path to the destination YAML file")
+
+    args = parser.parse_args()
+
+    merge_yaml_files(args.src_file, args.dest_file)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/docs/pages/developer_guide/integrate/ros_messaging.rst b/docs/pages/developer_guide/integrate/ros_messaging.rst
@@ -55,46 +55,6 @@ All\* default ROS topic names used by GISNav are listed in the :py:mod:`.messagi
     :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.13.X instead of v1.14.0-beta1.
-
-    .. _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