Video streaming (#11)

* fix: build errors

* feat: video streaming

* Add interface

* feat: implement basic functionality

* chore: cleanup

firstTimeInstall.sh

@@ -40,6 +40,7 @@ if ! grep -q "source /opt/ros/humble/setup.bash" ~/.bashrc; then
 fi
 
 pip3 install black
+pip3 install pylint
 
 sudo rosdep init
 rosdep update
@@ -68,6 +69,7 @@ meson compile -C builddir
 
 export GSTREAMER_DIR=$PWD
 ./gst-env.py --only-environment > setupGstreamer.sh
+sed -i '/PWD/d' setupGstreamer.sh
 sudo chmod +x setupGstreamer.sh
 source setupGstreamer.sh
 if ! grep -q "source $GSTREAMER_DIR/setupGstreamer.sh" ~/.bashrc; then
@@ -79,6 +81,8 @@ cd /tmp
 curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y
 . "$HOME/.cargo/env"
 git clone https://gitlab.freedesktop.org/gstreamer/gst-plugins-rs.git
+
+# Webrtc
 cd gst-plugins-rs/net/webrtc
 cargo build --release --target-dir build
 curl -o- https://raw.githubusercontent.com/nvm-sh/nvm/v0.40.0/install.sh | bash
@@ -92,12 +96,18 @@ npm install
 npm run build
 cd ../..
 cp -r webrtc $GSTREAMER_DIR
-
 export GST_PLUGIN_PATH=$GST_PLUGIN_PATH:$GSTREAMER_DIR/webrtc/build/release
 
-export GST_PLUGIN_PATH=$GST_PLUGIN_PATH:/usr/lib/aarch64-linux-gnu/gstreamer-1.0/deepstream
+# Rtp and congestion controll
+cd /tmp/gst-plugins-rs/net/rtp
+cargo build --release --target-dir build
+cd ..
+cp -r rtp $GSTREAMER_DIR
+export GST_PLUGIN_PATH=$GST_PLUGIN_PATH:$GSTREAMER_DIR/rtp/build/release
+
+export GST_PLUGIN_PATH=$GST_PLUGIN_PATH:/usr/lib/aarch64-linux-gnu/gstreamer-1.0/deepstream:/usr/lib/aarch64-linux-gnu/gstreamer-1.0/
 
-echo GST_PLUGIN_PATH=$GST_PLUGIN_PATH >> $GSTREAMER_DIR/setupGstreamer.sh
+echo "export GST_PLUGIN_PATH=$GST_PLUGIN_PATH" >> $GSTREAMER_DIR/setupGstreamer.sh
 
 echo "Finished building GStreamer"
 

src/camera_streaming/camera_streaming/webrct_node.py

@@ -0,0 +1,157 @@
+import rclpy
+import rclpy.logging
+from rclpy.node import Node
+from interfaces.srv import VideoOut
+import gi
+from gi.repository import Gst
+
+gi.require_version("Gst", "1.0")
+
+
+class WebRTCStreamer(Node):
+    """
+    A ROS2 Node that creates a WebRTC stream from multiple video sources using GStreamer.
+
+    This node listens for a 'start_video' service request, builds a GStreamer pipeline
+    with the provided video sources, and streams the video through WebRTC.
+    The node also supports optional web server functionality to host the stream.
+
+    Attributes:
+        web_server (bool): Flag indicating if a web server is enabled for the stream.
+        web_server_path (str): Path for the web server directory.
+        source_list (dict): A dictionary mapping camera names to device paths.
+        pipeline (Gst.Pipeline): The GStreamer pipeline for video streaming.
+    """
+
+    def __init__(self):
+        """
+        Initializes the WebRTCStreamer node, loads parameters, and sets up the service.
+
+        This constructor initializes GStreamer, declares the necessary parameters
+        (such as web_server, camera_name, and camera_path), and creates the ROS2 service
+        to start the video stream.
+        """
+        Gst.init(None)
+        super().__init__("webrtc_node")
+        self.declare_parameter("web_server", False)
+        self.declare_parameter("web_server_path", ".")
+        self.web_server = (
+            self.get_parameter("web_server").get_parameter_value().bool_value
+        )
+        self.web_server_path = (
+            self.get_parameter("web_server_path").get_parameter_value().string_value
+        )
+        self.start = self.create_service(VideoOut, "start_video", self.start_video_cb)
+        self.declare_parameter("camera_name", [""])
+        self.declare_parameter("camera_path", [""])
+
+        # Fetch the parameter values
+        camera_name = (
+            self.get_parameter("camera_name").get_parameter_value().string_array_value
+        )
+        camera_path = (
+            self.get_parameter("camera_path").get_parameter_value().string_array_value
+        )
+
+        # Convert to dictionary format
+        self.source_list = {}
+        for name, path in zip(camera_name, camera_path):
+            self.source_list[name] = path
+        self.pipeline = None
+
+    def start_video_cb(self, request, response):
+        """
+        Callback function for starting the video stream.
+
+        This function constructs a GStreamer pipeline based on the requested sources,
+        starts the pipeline, and returns a success response.
+
+        Args:
+            request (VideoOut.Request): The service request containing video stream details.
+            response (VideoOut.Response): The service response to return success or failure.
+
+        Returns:
+            VideoOut.Response: The response indicating success or failure.
+        """
+        pipeline_str = self.create_pipeline(request)
+        self.get_logger().info(pipeline_str)
+        if self.pipeline is not None:
+            try:
+                self.pipeline.set_state(Gst.State.NULL)
+            except:
+                self.pipeline = None
+        try:
+            self.pipeline = Gst.parse_launch(pipeline_str)
+            self.pipeline.set_state(Gst.State.PLAYING)
+            response.success = True
+        except:
+            response.success = False
+        return response
+
+    def create_source(self, name):
+        """
+        Creates a GStreamer source element based on the camera name.
+
+        Args:
+            name (str): The name of the camera source.
+
+        Returns:
+            str: A GStreamer pipeline source element for the camera.
+        """
+        if name == "test":
+            return "videotestsrc"
+        return f"v4l2src device={self.source_list[name]}"
+
+    def create_pipeline(self, request):
+        """
+        Creates the GStreamer pipeline string based on the service request.
+
+        This function generates the GStreamer pipeline to combine multiple video sources
+        and set up the compositor, applying the required video properties (width, height, position).
+
+        Args:
+            request (VideoOut.Request): The service request containing details of the video sources.
+
+        Returns:
+            str: The GStreamer pipeline string ready for launch.
+        """
+        pipeline = ""
+        compositor = "compositor name=mix"
+        total_width = request.width
+        total_height = request.height
+        i = 0
+        for source in request.sources:
+            name = source.name
+            height = int(source.height * total_height / 100)
+            width = int(source.width * total_width / 100)
+            origin_x = int(source.origin_x * total_width / 100)
+            origin_y = int(source.origin_y * total_height / 100)
+            pipeline += f'{self.create_source(name)} ! nvvidconv ! capsfilter caps="video/x-raw,height={height},width={width}" ! mix.sink_{i} '
+            compositor += f" sink_{i}::xpos={origin_x} sink_{i}::ypos={origin_y} sink_{i}::height={height} sink_{i}::width={width}"
+            i = i + 1
+        video_out = "webrtcsink run-signalling-server=true"
+        if self.web_server:
+            video_out += f" run-web-server=true web-server-host-addr=http://0.0.0.0:8080/ web-server-directory={self.web_server_path}"
+        pipeline += compositor + " ! " + video_out
+        return pipeline
+
+
+def main(args=None):
+    """
+    The main entry point of the program.
+
+    This function initializes the ROS2 system, creates the WebRTCStreamer node,
+    and starts the ROS2 event loop to process incoming requests.
+
+    Args:
+        args (list, optional): Arguments passed from the command line (default is None).
+    """
+    rclpy.init(args=args)
+    node = WebRTCStreamer()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/camera_streaming/config/webrtc.yaml

@@ -0,0 +1,10 @@
+webrtc_node:
+  ros__parameters:
+    camera_name:
+    - "Drive"
+    - "Arm"
+    camera_path:
+    - "/dev/v4l/by-id/usb-Sonix_Technology_Co.__Ltd._USB_2.0_Camera_SN5100-video-index0"
+    - "/dev/video2"
+    web_server: true
+    web_server_path: "/home/cprt/gstreamer/webrtc/gstwebrtc-api/dist"

src/camera_streaming/launch/webRTC.launch.py

@@ -0,0 +1,20 @@
+import os
+import launch_ros.actions
+from ament_index_python.packages import get_package_share_directory
+import launch
+
+
+def generate_launch_description():
+    config_dir = os.path.join(get_package_share_directory("camera_streaming"), "config")
+
+    params_file = os.path.join(config_dir, "webrtc.yaml")
+
+    webrtc_node = launch_ros.actions.Node(
+        package="camera_streaming",
+        executable="webrtc_node",
+        output="log",
+        parameters=[params_file],
+        arguments=["--ros-args", "--log-level", "Info"],
+    )
+
+    return launch.LaunchDescription([webrtc_node])

src/camera_streaming/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>camera_streaming</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">cprt</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/camera_streaming/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/camera_streaming
+[install]
+install_scripts=$base/lib/camera_streaming

src/camera_streaming/setup.py

@@ -0,0 +1,35 @@
+from setuptools import find_packages, setup
+import os
+from glob import glob
+
+package_name = "camera_streaming"
+
+setup(
+    name=package_name,
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        (
+            os.path.join("share", package_name, "launch"),
+            glob(os.path.join("launch", "*launch.[pxy][yma]*")),
+        ),
+        (
+            os.path.join("share", package_name, "config"),
+            glob(os.path.join("config", "*.yaml*")),
+        ),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="Connor",
+    maintainer_email="[email protected]",
+    description="TODO: Package description",
+    license="TODO: License declaration",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "webrtc_node = camera_streaming.webrct_node:main",
+        ],
+    },
+)

src/description/CMakeLists.txt

@@ -10,7 +10,7 @@ find_package(velocity_controllers REQUIRED)
 find_package(xacro REQUIRED)
 
 install(
-  DIRECTORY config launch meshes urdf robots
+  DIRECTORY config launch robots
   DESTINATION share/${PROJECT_NAME}
 )
 

src/interfaces/CMakeLists.txt

@@ -24,9 +24,11 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/ArucoMarkers.msg"
   "msg/PointArray.msg"
   "msg/ArmCmd.msg"
+  "msg/VideoSource.msg"
   "srv/ArmPos.srv"
   "msg/GPIOmsg.msg" #ik its goofy the actual gpio package is GPIO
   "srv/NavToGPSGeopose.srv"
+  "srv/VideoOut.srv"
   DEPENDENCIES 
   builtin_interfaces
   geometry_msgs 

src/interfaces/msg/VideoSource.msg

@@ -0,0 +1,10 @@
+# VideoSource.msg
+
+string name
+
+# Dimensions are percent of total
+int8 width
+int8 height
+
+int8 origin_x
+int8 origin_y

src/interfaces/srv/VideoOut.srv

@@ -0,0 +1,11 @@
+# VideoOut.srv
+
+int32 height
+int32 width
+int8 framerate
+int8 num_sources
+
+VideoSource[] sources
+
+---
+bool success