diff --git a/firstTimeInstall.sh b/firstTimeInstall.sh index c9a2240..c19b32f 100755 --- a/firstTimeInstall.sh +++ b/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 diff --git a/src/camera_streaming/camera_streaming/jpegPoC.py b/src/camera_streaming/camera_streaming/jpegPoC.py deleted file mode 100644 index af243c7..0000000 --- a/src/camera_streaming/camera_streaming/jpegPoC.py +++ /dev/null @@ -1,134 +0,0 @@ -import rclpy -import rclpy.logging -import random -from rclpy.node import Node -from interfaces.srv import ChangeRes -import gi -gi.require_version('Gst', '1.0') -from gi.repository import Gst - -pipeline_name = 'p0' - - -class jpegPoC(Node): - def __init__(self): - Gst.init(None) - super().__init__("JpegPoCNode") - self.declare_parameter("port", 7001) - self.target_port = ( - self.get_parameter("port").get_parameter_value().integer_value - ) - self.declare_parameter("host", "127.0.0.1") - self.target_host = ( - self.get_parameter("host").get_parameter_value().string_value - ) - self.declare_parameter("device", "/dev/video0") - self.device = ( - self.get_parameter("device").get_parameter_value().string_value - ) - self.declare_parameter("height", 1080) - self.height = ( - self.get_parameter("height").get_parameter_value().integer_value - ) - self.declare_parameter("width", 1920) - self.width = ( - self.get_parameter("width").get_parameter_value().integer_value - ) - self.declare_parameter("interval", 1000) - self.interval = ( - self.get_parameter("interval").get_parameter_value().integer_value - ) - self.declare_parameter("max_framerate", 60) - self.max_framerate = ( - self.get_parameter("max_framerate").get_parameter_value().integer_value - ) - self.declare_parameter("init_framerate", 1) - init_framerate = ( - self.get_parameter("init_framerate").get_parameter_value().integer_value - ) - self.timer = self.create_timer(self.interval/1000, self.check_bandwidth) - self.srv = self.create_service(ChangeRes, 'change_resolution', self.change_res_callback) - self.frame_rate = init_framerate - self.last_timestamp = None - self.create_pipeline() - - - def create_pipeline(self): - pipeline_string = f'v4l2src device={self.device} do-timestamp=true name=srcElement ! capsfilter name=caps ! videoconvert ! nvvidconv ! nvjpegenc ! rtpjpegpay ! udpsink host={self.target_host} port={self.target_port} sync=false' - self.pipeline = Gst.parse_launch(pipeline_string) - self.get_logger().info(pipeline_string) - self.pipeline.set_state(Gst.State.PLAYING) - src = self.pipeline.get_by_name("srcElement") - src_pad = src.get_static_pad("src") - src_pad.add_probe(Gst.PadProbeType.BUFFER, self.buffer_probe) - - def buffer_probe(self, pad, info): - buffer = info.get_buffer() - - if buffer is None: - return Gst.PadProbeReturn.OK - - if self.last_timestamp is not None: - time_diff = buffer.pts - self.last_timestamp - if time_diff < (Gst.SECOND / self.frame_rate): - self.get_logger().debug(f"Dropping buffer. Time since last buffer: {time_diff:.2f}s") - return Gst.PadProbeReturn.DROP - - self.last_timestamp = buffer.pts - self.get_logger().debug(f"Sending buffer. Time since last buffer: {time_diff:.2f}s") - return Gst.PadProbeReturn.OK - - def change_res_callback(self, request, response): - response.success = True - return response - - def check_bandwidth(self): - # TODO (get bandwidth estimate from link) - # For now, pick random numbers - bandwidth = random.randint(10, 100) - in_use = random.randint(1, bandwidth) - # --------------------- # - usage = in_use / bandwidth - if self.update_bandwidth(usage): - self.get_logger().info(f"Framerate changed. Bandwidth: {bandwidth} In use: {in_use} ({usage * 100:.2f}%), new frame rate: {self.frame_rate}") - return - - def update_bandwidth(self, usage): - # TODO make this more configurable - if usage > 0.95: - self.frame_rate = int(self.frame_rate * 0.5) - elif usage > 0.8: - self.frame_rate = self.frame_rate - 1 - elif usage < 0.5: - self.frame_rate = self.frame_rate + 1 - else: - # Bandwidth usage is fine - return False - - # Update framerate - if self.frame_rate < 1: - self.frame_rate = 1 - elif self.frame_rate > self.max_framerate: - self.frame_rate = self.max_framerate - return True - - def refreshCaps(self): - element = self.pipeline.get_by_name("caps") - if not element: - self.get_logger().error("Element not found") - return - self.pipeline.set_state(Gst.State.NULL) - new_caps = Gst.Caps.from_string(f"image/x-raw, height={self.height}, width={self.width}") - element.set_property("caps", new_caps) - self.pipeline.set_state(Gst.State.PLAYING) - -def main(args=None): - rclpy.init(args=args) - node = jpegPoC() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/camera_streaming/camera_streaming/webrct_node.py b/src/camera_streaming/camera_streaming/webrct_node.py index 6fa1ecf..2ceb7f2 100644 --- a/src/camera_streaming/camera_streaming/webrct_node.py +++ b/src/camera_streaming/camera_streaming/webrct_node.py @@ -1,15 +1,36 @@ import rclpy import rclpy.logging -import random from rclpy.node import Node from interfaces.srv import VideoOut import gi -gi.require_version('Gst', '1.0') 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) @@ -20,60 +41,111 @@ def __init__(self): 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', [""]) + 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 + 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 i in range(0, len(camera_name)): - self.source_list[camera_name[i]] = camera_path[i] + 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 != None: - self.pipeline.set_state(Gst.State.NULL) - self.pipeline = Gst.parse_launch(pipeline_str) - self.pipeline.set_state(Gst.State.PLAYING) - response.success = True + 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 input in request.sources: - name = input.name - height = int(input.height * total_height / 100) - width = int(input.width * total_width / 100) - origin_x = int(input.origin_x * total_width / 100) - origin_y = int(input.origin_y * total_height / 100) - pipeline += f"{self.create_source(name)} ! nvvidconv ! capsfilter caps=\"video/x-raw,height={height},width={width}\" ! mix.sink_{i} " + 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 - videoOut = f"webrtcsink run-signalling-server=true web-server-directory={self.web_server_path}/gstwebrtc-api/dist" + i = i + 1 + video_out = "webrtcsink run-signalling-server=true" if self.web_server: - videoOut += " run-web-server=true web-server-host-addr=http://0.0.0.0:8080/" - pipeline += compositor + " ! " + videoOut + 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) @@ -81,5 +153,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/src/camera_streaming/config/webrtc.yaml b/src/camera_streaming/config/webrtc.yaml index fa9d8de..0665e31 100644 --- a/src/camera_streaming/config/webrtc.yaml +++ b/src/camera_streaming/config/webrtc.yaml @@ -7,4 +7,4 @@ webrtc_node: - "/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" + web_server_path: "/home/cprt/gstreamer/webrtc/gstwebrtc-api/dist" diff --git a/src/camera_streaming/launch/webRTC.launch.py b/src/camera_streaming/launch/webRTC.launch.py index 151e048..f9d1a81 100644 --- a/src/camera_streaming/launch/webRTC.launch.py +++ b/src/camera_streaming/launch/webRTC.launch.py @@ -1,20 +1,20 @@ import os -import launch 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") + 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"]) - + package="camera_streaming", + executable="webrtc_node", + output="log", + parameters=[params_file], + arguments=["--ros-args", "--log-level", "Info"], + ) + return launch.LaunchDescription([webrtc_node]) - \ No newline at end of file diff --git a/src/camera_streaming/setup.py b/src/camera_streaming/setup.py index a3fb32d..6fad4ef 100644 --- a/src/camera_streaming/setup.py +++ b/src/camera_streaming/setup.py @@ -2,30 +2,34 @@ import os from glob import glob -package_name = 'camera_streaming' +package_name = "camera_streaming" setup( name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), + 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*'))) + ("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'], + install_requires=["setuptools"], zip_safe=True, - maintainer='cprt', - maintainer_email='softlead@curovers.ca', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], + maintainer="Connor", + maintainer_email="connor.needham2015@gmail.com", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - 'jpegPoC = camera_streaming.jpegPoC:main', - 'webrtc_node = camera_streaming.webrct_node:main' + "console_scripts": [ + "webrtc_node = camera_streaming.webrct_node:main", ], }, )