ros-drivers / gscam

ROS Camera driver for GStreamer-based video streams.
136 stars 172 forks source link

gscam - multiple cameras (stereo configuration) #94

Closed ghost closed 8 months ago

ghost commented 8 months ago

Hello everyone, I'm working with a Jetson Nano Dev Kit B01 (Ubuntu 18.04 & ROS melodic) and a pair of IMX219 cameras, trying to publish their raw image into the ROS network.

I've written the following launch file, so that I can run a gscam node for each camera at once. The problem is that it won't let me have 2 gscam nodes running at the same time, and I can't seem to find the error.

I've successfully launched a very similar launch file for a single camera (commenting out the "right" group). I've also tried creating a launch file for each camera, but when I launch the second one in a new terminal, one of them fails. The node that fails seems to be randomly selected, in a way that it doesn't appear to have anything to do with the order in which I launched them.

Launch file:

<launch>
    <arg name="left_camera" default="0"/>
    <arg name="right_camera" default="1"/>
    <arg name="width" default="3280"/>
    <arg name="height" default="2464"/>
    <arg name="fps" default="20/1"/>
    <arg name="format" default="NV12"/>

    <group ns="left">
        <node pkg="gscam" type="gscam" name="gscam">
            <param name="camera_name" value="default"/>
            <param name="gscam_config" value="nvarguscamerasrc sensor-id=(int)$(arg left_camera) ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)$(arg format), framerate=(fraction)$(arg fps) ! nvvidconv flip-method=6 ! video/x-raw, format=(string)BGRx ! videoconvert"/>
            <param name="camera_info_url" value="file://$(find my_stereo_camera)/config/calibration/uncalibrated_parameters.ini"/>
            <param name="frame_id" value="/left_frame"/>
            <param name="sync_sink" value="false"/>
            <remap from="camera/image_raw" to="image_raw"/>
            <remap from="camera/camera_info" to="camera_info"/>
        </node>
    </group>
    <group ns="right">
        <node pkg="gscam" type="gscam" name="gscam">
            <param name="camera_name" value="default"/>
            <param name="gscam_config" value="nvarguscamerasrc sensor-id=(int)$(arg right_camera) ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)$(arg format), framerate=(fraction)$(arg fps) ! nvvidconv flip-method=6 ! video/x-raw, format=(string)BGRx ! videoconvert"/>
            <param name="camera_info_url" value="file://$(find my_stereo_camera)/config/calibration/uncalibrated_parameters.ini"/>
            <param name="frame_id" value="/right_frame"/>
            <param name="sync_sink" value="false"/>
            <remap from="camera/image_raw" to="image_raw"/>
            <remap from="camera/camera_info" to="camera_info"/>
        </node>
    </group>
</launch>

Error message:

$ roslaunch my_stereo_camera my_stereo_camera.launch
... logging to /home/jetsonnano/.ros/log/a8531376-7d9e-11ee-a51e-401c83806a8d/roslaunch-jetsonnano-desktop-16019.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://localhost:38005/

SUMMARY
========

PARAMETERS
 * /left/gscam/camera_info_url: file:///home/jets...
 * /left/gscam/camera_name: default
 * /left/gscam/frame_id: /left_frame
 * /left/gscam/gscam_config: nvarguscamerasrc ...
 * /left/gscam/sync_sink: True
 * /right/gscam/camera_info_url: file:///home/jets...
 * /right/gscam/camera_name: default
 * /right/gscam/frame_id: /right_frame
 * /right/gscam/gscam_config: nvarguscamerasrc ...
 * /right/gscam/sync_sink: True
 * /rosdistro: melodic
 * /rosversion: 1.14.13

NODES
  /left/
    gscam (gscam/gscam)
  /right/
    gscam (gscam/gscam)

ROS_MASTER_URI=http://localhost:11311

process[left/gscam-1]: started with pid [16040]
process[right/gscam-2]: started with pid [16041]
Error generated. /dvs/git/dirty/git-master_linux/multimedia/nvgstreamer/gst-nvarguscamera/gstnvarguscamerasrc.cpp, execute:751 Failed to create CaptureSession
[ERROR] [1699383114.684800098]: Could not get gstreamer sample.
[right/gscam-2] process has finished cleanly
log file: /home/jetsonnano/.ros/log/a8531376-7d9e-11ee-a51e-401c83806a8d/right-gscam-2*.log

Any help will be much appreciated :)

ghost commented 8 months ago

Found the error! Apparently, sensor-id doesn't expect an int but a string...

For those who might find it helpful, the working launch file:

<launch>
    <arg name="left_camera" default="0"/>
    <arg name="right_camera" default="1"/>
    <arg name="width" default="3280"/>
    <arg name="height" default="2464"/>
    <arg name="fps" default="20/1"/>
    <arg name="format" default="NV12"/>

    <group ns="left">
        <node pkg="gscam" type="gscam" name="gscam">
            <param name="camera_name" value="default"/>
            <param name="gscam_config" value="nvarguscamerasrc sensor-id=$(arg left_camera) ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)$(arg format), framerate=(fraction)$(arg fps) ! nvvidconv flip-method=6 ! video/x-raw, format=(string)BGRx ! videoconvert"/>
            <param name="camera_info_url" value="file://$(find my_stereo_camera)/config/calibration/uncalibrated_parameters.ini"/>
            <param name="frame_id" value="/left_frame"/>
            <param name="sync_sink" value="false"/>
            <remap from="camera/image_raw" to="image_raw"/>
            <remap from="camera/camera_info" to="camera_info"/>
        </node>
    </group>
    <group ns="right">
        <node pkg="gscam" type="gscam" name="gscam">
            <param name="camera_name" value="default"/>
            <param name="gscam_config" value="nvarguscamerasrc sensor-id=$(arg right_camera) ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)$(arg format), framerate=(fraction)$(arg fps) ! nvvidconv flip-method=6 ! video/x-raw, format=(string)BGRx ! videoconvert"/>
            <param name="camera_info_url" value="file://$(find my_stereo_camera)/config/calibration/uncalibrated_parameters.ini"/>
            <param name="frame_id" value="/right_frame"/>
            <param name="sync_sink" value="false"/>
            <remap from="camera/image_raw" to="image_raw"/>
            <remap from="camera/camera_info" to="camera_info"/>
        </node>
    </group>
</launch>