hmakelin · hmakelin · Apr 29, 2023 · Apr 29, 2023 · Apr 29, 2023 · Apr 29, 2023
diff --git a/docker/Makefile b/docker/Makefile
@@ -51,17 +51,17 @@ build-offboard-sitl-test-%: build-offboard-sitl-middleware-%
 # sitl development services: Gazebo simulation, ROS middleware, mapserver, torch-serve, QGC, but excluding GISNav
 . PHONY: $(addprefix offboard-sitl-dev-, $(AUTOPILOTS))
 offboard-sitl-dev-%: offboard-sitl-middleware-%
-	@docker compose up -d $* torch-serve mapserver qgc
+	@docker compose up -d $* torch-serve mapserver qgc rviz
 
 . PHONY: $(addprefix build-offboard-sitl-dev-, $(AUTOPILOTS))
 build-offboard-sitl-dev-%: build-offboard-sitl-middleware-%
-	@docker compose build $* torch-serve mapserver qgc
+	@docker compose build $* torch-serve mapserver qgc rviz
 
 # offboard sitl middleware
 . PHONY: $(addprefix offboard-sitl-middleware-, $(AUTOPILOTS))
 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 @@ 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,7 +83,7 @@ build-offboard-sitl-middleware-%:
 . PHONY: $(addprefix onboard-hil-middleware-, $(AUTOPILOTS))
 onboard-hil-middleware-%:
 	@if [ "$*" = "px4" ]; then \
-		docker compose -f docker-compose.yaml -f docker-compose.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.serial.yaml up -d mavros; \
 	else \
@@ -94,7 +94,7 @@ onboard-hil-middleware-%:
 . PHONY: $(addprefix build-onboard-hil-middleware-, $(AUTOPILOTS))
 build-onboard-hil-middleware-%:
 	@if [ "$*" = "px4" ]; then \
-		docker compose -f docker-compose.yaml -f docker-compose.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.serial.yaml build mavros; \
 	else \

diff --git a/docker/docker-compose.serial.yaml b/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
@@ -22,6 +22,18 @@ services:
     network_mode: host
     command: mapproxy-util serve-develop -b $HOST:$PORT $HOME/$(basename $CONFIG_FILE)
 
+  micro-ros-agent:
+    build:
+      context: micro-ros-agent
+      dockerfile: Dockerfile
+    network_mode: host
+    environment:
+      - ROS_DOMAIN_ID=0
+      - FASTRTPS_DEFAULT_PROFILES_FILE=/disable_shared_memory.xml
+    volumes:
+      - /dev/shm:/dev/shm
+    command: udp4 -p 8888
+
   mavros:
     build:
       context: mavros
@@ -130,3 +142,26 @@ services:
             - driver: nvidia
               count: 1
               capabilities: [ gpu ]
+
+  # Note: build context is repository root
+  rviz:
+    build:
+      context: rviz
+      dockerfile: Dockerfile
+    env_file:
+      - env/x11.env
+    environment:
+      - FASTRTPS_DEFAULT_PROFILES_FILE=/disable_shared_memory.xml
+    volumes:
+      - /dev/dri:/dev/dri
+    network_mode: host
+    stdin_open: true
+    tty: true
+    deploy:
+      resources:
+        reservations:
+          devices:
+            - driver: nvidia
+              count: 1
+              capabilities: [ gpu ]
+    command: rviz2 -d gisnav_config.rviz
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
@@ -10,7 +10,5 @@ param set-default EKF2_GPS_V_NOISE 3
 param set-default SENS_GPS_MASK 4
 param set-default EKF2_REQ_EPH 10.0
 
-param set-default MAV_USEHILGPS 1
-
 #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,6 +19,9 @@ 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

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/docker/rviz/Dockerfile b/docker/rviz/Dockerfile
@@ -0,0 +1,9 @@
+FROM ros:foxy
+
+RUN apt-get update && \
+    apt-get -y dist-upgrade && \
+    apt-get -y install ros-foxy-rviz2
+
+COPY --chmod=755 * /
+
+ENTRYPOINT ["/entrypoint.sh"]
diff --git a/docker/rviz/disable_shared_memory.xml b/docker/rviz/disable_shared_memory.xml
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles" >
+    <transport_descriptors>
+        <transport_descriptor>
+            <transport_id>CustomUdpTransport</transport_id>
+            <type>UDPv4</type>
+        </transport_descriptor>
+    </transport_descriptors>
+    <participant profile_name="participant_profile" is_default_profile="true">
+        <rtps>
+            <userTransports>
+                <transport_id>CustomUdpTransport</transport_id>
+            </userTransports>
+            <useBuiltinTransports>false</useBuiltinTransports>
+        </rtps>
+    </participant>
+</profiles>
diff --git a/docker/rviz/entrypoint.sh b/docker/rviz/entrypoint.sh
@@ -0,0 +1,6 @@
+#!/bin/bash
+set -e
+
+source "/opt/ros/foxy/setup.bash"
+
+exec "$@"