Port #97 to ros2

.github/workflows/ubuntu-build.yml

@@ -34,7 +34,15 @@ jobs:
           sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
           echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
           sudo apt update && sudo apt install --no-install-recommends -y \
-          rapidjson-dev gz-harmonic
+          rapidjson-dev \
+          libopencv-dev \
+          libunwind-dev \
+          libgstreamer1.0-dev \
+          libgstreamer-plugins-base1.0-dev \
+          gstreamer1.0-plugins-bad \
+          gstreamer1.0-libav \
+          gstreamer1.0-gl \
+          gz-harmonic
 
       # Put ccache into github cache for faster build
       - name: Prepare ccache timestamp

CMakeLists.txt

@@ -52,8 +52,11 @@ else()
 endif()
 
 # --------------------------------------------------------------------------- #
-# Find RapidJSON.
 find_package(RapidJSON REQUIRED)
+find_package(OpenCV REQUIRED)
+
+pkg_check_modules(GST REQUIRED gstreamer-1.0 gstreamer-app-1.0)
+
 
 # --------------------------------------------------------------------------- #
 # Build plugin.
@@ -95,6 +98,21 @@ target_link_libraries(CameraZoomPlugin PRIVATE
   gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
 )
 
+add_library(GstCameraPlugin
+  SHARED
+  src/GstCameraPlugin.cc
+)
+target_include_directories(GstCameraPlugin PRIVATE
+  include
+  ${OpenCV_INCLUDE_DIRS}
+  ${GST_INCLUDE_DIRS}
+)
+target_link_libraries(GstCameraPlugin PRIVATE
+  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
+  ${OpenCV_LIBS}
+  ${GST_LINK_LIBRARIES}
+)
+
 # --------------------------------------------------------------------------- #
 # Install.
 
@@ -103,6 +121,7 @@ install(
   ArduPilotPlugin
   ParachutePlugin
   CameraZoomPlugin
+  GstCameraPlugin
   DESTINATION lib/${PROJECT_NAME}
 )
 

README.md

@@ -51,6 +51,7 @@ Manual - Gazebo Garden Dependencies:
 ```bash
 sudo apt update
 sudo apt install libgz-sim7-dev rapidjson-dev
+sudo apt install libopencv-dev libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl
 ```
 
 #### Harmonic (apt)
@@ -60,6 +61,7 @@ Manual - Gazebo Harmonic Dependencies:
 ```bash
 sudo apt update
 sudo apt install libgz-sim8-dev rapidjson-dev
+sudo apt install libopencv-dev libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev gstreamer1.0-plugins-bad gstreamer1.0-libav gstreamer1.0-gl
 ```
 
 #### Rosdep
@@ -82,6 +84,7 @@ rosdep install --from-paths src --ignore-src -y
 ```bash
 brew update
 brew install rapidjson
+brew install opencv gstreamer
 ```
 
 Ensure the `GZ_VERSION` environment variable is set to either
@@ -197,6 +200,38 @@ greater than one:
 MANUAL> param set SIM_SPEEDUP 10
 ```
 
+### 3. Streaming camera video
+
+Images from camera sensors may be streamed with GStreamer using
+the `GstCameraPlugin` sensor plugin. The example gimbal models include the
+plugin element:
+
+```xml
+<plugin name="GstCameraPlugin"
+    filename="GstCameraPlugin">
+  <udp_host>127.0.0.1</udp_host>
+  <udp_port>5600</udp_port>
+  <use_basic_pipeline>true</use_basic_pipeline>
+  <use_cuda>false</use_cuda>
+</plugin>
+```
+
+The `<image_topic>` and `<enable_topic>` parameters are deduced from the
+topic name for the camera sensor, but may be overriden if required.
+
+The `gimbal.sdf` world includes a 3 degrees of freedom gimbal with a
+zoomable camera. To start streaming:
+
+```bash
+gz topic -t /world/gimbal/model/mount/model/gimbal/link/pitch_link/sensor/camera/image/enable_streaming -m gz.msgs.Boolean -p "data: 1"
+```
+
+Display the streamed video:
+
+```bash
+gst-launch-1.0 -v udpsrc port=5600 caps='application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264' ! rtph264depay ! avdec_h264 ! videoconvert ! autovideosink sync=false
+```
+
 ## Models
 
 In addition to the Iris and Zephyr models included here, a selection

include/GstCameraPlugin.hh

@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2024 ArduPilot
+ */
+
+/*
+ * Copyright (C) 2012-2016 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+#ifndef GSTCAMERAPLUGIN_HH_
+#define GSTCAMERAPLUGIN_HH_
+
+#include <memory>
+
+#include <gz/sim/System.hh>
+
+namespace gz {
+namespace sim {
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+namespace systems {
+
+/// \brief Plugin to stream camera sensor data using GStreamer.
+/// \class GstCameraPlugin
+///
+/// A Gazebo plugin that can be attached to a camera and then streams the
+/// video data using gstreamer.
+///
+/// Parameters
+///  <udp_host>            the UDP host IP, defaults to 127.0.0.1
+///  <udp_port>            the UDP port, defaults to 5600
+///  <rtmp_location>       the RTMP location
+///  <use_basic_pipeline>  set to true if not using <rtmp_location>
+///  <use_cuda>            set to true to use CUDA (if available)
+///  <image_topic>         the camera image topic
+///  <enable_topic>        the topic to enable / disable video streaming
+///
+/// Start streaming
+///   assumes: <enable_topic>/camera/enable_streaming<enable_topic>
+///
+///   gz topic -t "/camera/enable_streaming" -m gz.msgs.Boolean -p "data: 1"
+///
+/// Connect to the stream via command line and open an OpenGL window:
+///
+///   gst-launch-1.0 -v udpsrc port=5600 caps='application/x-rtp,
+///       media=(string)video, clock-rate=(int)90000,
+///       encoding-name=(string)H264'
+///       ! rtph264depay ! avdec_h264 ! videoconvert
+///       ! autovideosink sync=false
+///
+class GstCameraPlugin :
+    public System,
+    public ISystemConfigure,
+    public ISystemPreUpdate
+{
+  /// \brief Destructor
+  public: virtual ~GstCameraPlugin();
+
+  /// \brief Constructor
+  public: GstCameraPlugin();
+
+  // Documentation inherited
+  public: void PreUpdate(const gz::sim::UpdateInfo &_info,
+                         gz::sim::EntityComponentManager &_ecm) final;
+
+  // Documentation inherited
+  public: void Configure(const Entity &_entity,
+                         const std::shared_ptr<const sdf::Element> &_sdf,
+                         EntityComponentManager &_ecm,
+                         EventManager &) final;
+
+  /// \internal
+  /// \brief Private implementation
+  private: class Impl;
+  private: std::unique_ptr<Impl> impl;
+};
+
+}  // namespace systems
+}
+}  // namespace sim
+}  // namespace gz
+
+#endif  // GSTCAMERAPLUGIN_HH_

models/gimbal_small_1d/model.sdf

@@ -123,6 +123,15 @@
         <always_on>1</always_on>
         <update_rate>10</update_rate>
         <visualize>1</visualize>
+
+        <plugin name="GstCameraPlugin"
+            filename="GstCameraPlugin">
+          <udp_host>127.0.0.1</udp_host>
+          <udp_port>5600</udp_port>
+          <use_basic_pipeline>true</use_basic_pipeline>
+          <use_cuda>false</use_cuda>
+        </plugin>
+
       </sensor>
     </link>
     <joint name='tilt_joint' type='revolute'>

models/gimbal_small_2d/model.sdf

@@ -159,6 +159,15 @@
         <always_on>1</always_on>
         <update_rate>10</update_rate>
         <visualize>1</visualize>
+
+        <plugin name="GstCameraPlugin"
+            filename="GstCameraPlugin">
+          <udp_host>127.0.0.1</udp_host>
+          <udp_port>5600</udp_port>
+          <use_basic_pipeline>true</use_basic_pipeline>
+          <use_cuda>false</use_cuda>
+        </plugin>
+
       </sensor>
     </link>
     <joint name='tilt_joint' type='revolute'>

models/gimbal_small_3d/model.sdf

@@ -210,6 +210,14 @@
           <max_zoom>125.0</max_zoom>
           <slew_rate>0.42514285714</slew_rate>
         </plugin>
+
+        <plugin name="GstCameraPlugin"
+            filename="GstCameraPlugin">
+          <udp_host>127.0.0.1</udp_host>
+          <udp_port>5600</udp_port>
+          <use_basic_pipeline>true</use_basic_pipeline>
+          <use_cuda>false</use_cuda>
+        </plugin>
 
       </sensor>
     </link>