Use dedicated gscam service instead of gazebo plugin for video stream…

… to ROS bridge
hmakelin · hmakelin · Oct 3, 2023 · Oct 2, 2023 · Oct 3, 2023 · Oct 3, 2023
commit 6ab01fcedd3dedfbeca0f907b04c85608ae5c607
diff --git a/docker/Makefile b/docker/Makefile
@@ -153,22 +153,22 @@ $(eval $(call middleware_template,onboard-hil-middleware,-f docker-compose.yaml
 $(eval $(call middleware_template,onboard-sitl-middleware,))
 $(eval $(call middleware_template,offboard-sitl-middleware,))  # same as onboard
 
-# onboard HIL services: GIS server, ROS middleware, autoheal, and GISNav
-$(eval $(call compose_template,onboard-hil,onboard-hil-middleware,docker-compose.arm64.yaml,mapserver torch-serve autoheal gisnav))
+# onboard HIL services: GIS server, ROS middleware, autoheal, gscam and GISNav
+$(eval $(call compose_template,onboard-hil,onboard-hil-middleware,docker-compose.arm64.yaml,mapserver torch-serve autoheal gscam gisnav))
 
 # onboard SITL services: Same as with HIL but middleware is same as offboard (UDP, not serial)
-$(eval $(call compose_template,onboard-sitl,offboard-sitl-middleware,docker-compose.arm64.yaml,mapserver torch-serve gisnav))
+$(eval $(call compose_template,onboard-sitl,offboard-sitl-middleware,docker-compose.arm64.yaml,mapserver torch-serve gscam gisnav))
 
 # offboard SITL services: Gazebo simulation, QGC
 $(eval $(call compose_template,offboard-sitl,dummy-dependency,,$$* qgc))
 
-# SITL testing services: Gazebo simulation, ROS middleware, mapserver, torch-serve, but excluding GISNav and QGC
-$(eval $(call compose_template,offboard-sitl-test,offboard-sitl-middleware,docker-compose.headless.yaml,$$* torch-serve mapserver))
+# SITL testing services: Gazebo simulation, ROS middleware, mapserver, torch-serve, gscam, but excluding GISNav and QGC
+$(eval $(call compose_template,offboard-sitl-test,offboard-sitl-middleware,docker-compose.headless.yaml,$$* torch-serve gscam mapserver))
 
-# SITL development services: Gazebo simulation, ROS middleware, mapserver, torch-serve, QGC, rviz, but excluding GISNav
-$(eval $(call compose_template,offboard-sitl-dev,offboard-sitl-middleware,,$$* qgc torch-serve mapserver rviz))
+# SITL development services: Gazebo simulation, ROS middleware, mapserver, torch-serve, QGC, gscam rviz, but excluding GISNav
+$(eval $(call compose_template,offboard-sitl-dev,offboard-sitl-middleware,,$$* qgc torch-serve gscam mapserver rviz))
 
-# All SITL services offboard: GIS server, ROS middleware, Gazebo simulation, QGC, gisnav
+# All SITL services offboard: GIS server, ROS middleware, Gazebo simulation, QGC, gscam, gisnav
 $(eval $(call compose_template,demo,offboard-sitl-dev,,gisnav))
 
 # List of Docker Compose service names that need GUI access

diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml
@@ -138,6 +138,23 @@ services:
         ROS_VERSION: humble
     command: rviz2 -d gisnav_config.rviz
 
+  gscam:
+    <<: [*base, *ros]
+    build:
+      context: gscam
+      args:
+        ROS_VERSION: humble
+    command:
+      - "ros2"
+      - "run"
+      - "gscam"
+      - "gscam_node"
+      - "--ros-args"
+      - "--params-file"
+      - "gscam_params.yaml"
+      - "-p"
+      - "camera_info_url:=file:///camera_calibration.yaml"
+
   autoheal:
     <<: [*base]
     image: willfarrell/autoheal

diff --git a/docker/gscam/Dockerfile b/docker/gscam/Dockerfile
@@ -0,0 +1,23 @@
+ARG ROS_VERSION
+FROM ros:${ROS_VERSION}
+
+ARG ROS_VERSION
+ENV ROS_VERSION=${ROS_VERSION}
+
+COPY camera_calibration.yaml gscam_params.yaml /
+COPY --chmod=755 entrypoint.sh /
+
+RUN apt-get update && \
+    apt-get -y dist-upgrade && \
+    apt-get -y install gstreamer1.0-tools libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev \
+      libgstreamer-plugins-good1.0-dev gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl && \
+    apt-get -y install ros-${ROS_VERSION}-gscam && \
+    apt-get clean && \
+    rm -rf /var/lib/apt/lists/*
+
+HEALTHCHECK --interval=10s --timeout=3s \
+  CMD /bin/bash -c "source /opt/ros/${ROS_VERSION}/setup.bash && \
+    ros2 node list | grep -q gscam_node" || exit 1
+
+SHELL ["/bin/bash", "-c"]
+ENTRYPOINT ["/entrypoint.sh"]
diff --git a/docker/gscam/camera_calibration.yaml b/docker/gscam/camera_calibration.yaml
@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 360
+camera_name: cgo3  # Typhoon H480 model has CGO3 camera
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [205.46963709898583, 0, 320, 0, 205.46963709898583, 180, 0, 0, 1]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [0, 0, 0, 0, 0]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0]
diff --git a/docker/gscam/entrypoint.sh b/docker/gscam/entrypoint.sh
@@ -0,0 +1,6 @@
+#!/bin/bash
+set -e
+
+source "/opt/ros/$ROS_VERSION/setup.bash"
+
+exec "$@"
diff --git a/docker/gscam/gscam_params.yaml b/docker/gscam/gscam_params.yaml
@@ -0,0 +1,8 @@
+gscam_publisher:
+  ros__parameters:
+    gscam_config: >
+      udpsrc uri=udp://127.0.0.1:5600 caps='application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264' ! rtph264depay ! avdec_h264 ! videoconvert
+    preroll: False
+    use_gst_timestamps: True
+    frame_id: 'mono'
+    image_encoding: 'rgb8'  # Does not support bgr8, handle this downstream
diff --git a/docker/px4/Dockerfile b/docker/px4/Dockerfile
@@ -25,12 +25,10 @@ RUN git clone --branch v1.14.0-beta2 --single-branch \
 # Explicitly add required \
 WORKDIR PX4-Autopilot
 
-COPY merge_xml.py merge_yaml.py dds_topics.yaml typhoon_h480_camera.xml iris_hitl_camera.xml iris_hitl_ksql_airport.world .
+COPY merge_xml.py merge_yaml.py dds_topics.yaml iris_hitl_ksql_airport.world .
 
-# Add ROS camera plugin for SITL and HIL simulation, setup ksql_airport.world for HIL
+# Setup ksql_airport.world for HIL
 RUN pip install --no-cache-dir lxml && \
-    python3 merge_xml.py typhoon_h480_camera.xml Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/typhoon_h480/typhoon_h480.sdf.jinja && \
-    python3 merge_xml.py iris_hitl_camera.xml Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/iris_hitl/iris_hitl.sdf && \
     python3 merge_xml.py iris_hitl_ksql_airport.world Tools/simulation/gazebo-classic/sitl_gazebo-classic/worlds/hitl_iris.world
 
 # Configure XRCE-DDS bridge

diff --git a/docker/px4/iris_hitl_camera.xml b/docker/px4/iris_hitl_camera.xml
diff --git a/docker/px4/typhoon_h480_camera.xml b/docker/px4/typhoon_h480_camera.xml