Camera: ported GstCameraPlugin from PX4 gazebo classic

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}
 )
 

include/GstCameraPlugin.hh

@@ -0,0 +1,71 @@
+/*
+ * 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>
+
+/**
+ * @class GstCameraPlugin
+ * A Gazebo plugin that can be attached to a camera and then streams the video data using gstreamer.
+ * It streams to a configurable UDP IP and UDP Port, defaults are respectively 127.0.0.1 and 5600.
+ * IP and Port can be configured in the SDF as well as by GZ_VIDEO_HOST_IP and GZ_VIDEO_HOST_PORT env variables
+ *
+ * Connect to the stream via command line with:
+ 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
+ */
+namespace gz {
+namespace sim {
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+namespace systems {
+
+/// \brief Camera stream plugin.
+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_3d/model.sdf

@@ -210,6 +210,14 @@
           <max_zoom>125.0</max_zoom>
           <slew_rate>0.42514285714</slew_rate>
         </plugin>
+
+        <plugin filename="GstCameraPlugin"
+            name="GstCameraPlugin">
+            <udpHost>127.0.0.1</udpHost>
+            <udpPort>5600</udpPort>
+            <imageTopic>/world/iris_runway/model/iris_with_gimbal/model/gimbal/link/pitch_link/sensor/camera/image</imageTopic>
+            <enableTopic>/enable_video_stream</enableTopic>
+        </plugin>
 
       </sensor>
     </link>