Zed wrapper node crashes when /stop_svo_rec service is called

[abAgriRobot]

Preliminary Checks

• [X] This issue is not a duplicate. Before opening a new issue, please search existing issues.
• [X] This issue is not a question, feature request, or anything other than a bug report directly related to this project.

Description

I'm launching the zed wrapper with two cameras, the bringup part is fine and the nodes and topics are available. However, when recording SVO files using the provided ROS 2 services, the node crashes after receiving the service call to stop the recording.

Steps to Reproduce

1. Launching the launch file for multiple cameras

# Copyright 2023 Stereolabs
#
# 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.

import os

from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    OpaqueFunction,
    IncludeLaunchDescription,
    LogInfo,
)
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, Command, TextSubstitution
from launch_ros.actions import Node


def parse_array_param(param):
    str = param.replace("[", "")
    str = str.replace("]", "")
    str = str.replace(" ", "")
    arr = str.split(",")

    return arr


def launch_setup(context, *args, **kwargs):
    # URDF/xacro file to be loaded by the Robot State Publisher node
    multi_zed_xacro_path = os.path.join(
        get_package_share_directory("my_package"),
        "urdf",
        "zed_multi.urdf.xacro",
    )

    robot_description = Command(["xacro", " ", multi_zed_xacro_path]).perform(context)

    # Robot State Publisher node
    # this will publish the static reference link for a multi-camera configuration
    # and all the joints. See 'urdf/zed_dual.urdf.xacro' as an example
    multi_rsp_node = Node(
        package="robot_state_publisher",
        namespace="zed_multi",
        executable="robot_state_publisher",
        name="zed_multi_state_publisher",
        output="screen",
        parameters=[{"robot_description": robot_description}],
    )

    names = LaunchConfiguration("cam_names")
    models = LaunchConfiguration("cam_models")
    serials = LaunchConfiguration("cam_serials")
    disable_tf = LaunchConfiguration("disable_tf")

    names_arr = parse_array_param(names.perform(context))
    models_arr = parse_array_param(models.perform(context))
    serials_arr = parse_array_param(serials.perform(context))
    disable_tf_val = disable_tf.perform(context)

    num_cams = len(names_arr)

    if num_cams != len(models_arr):
        return [
            LogInfo(
                msg=TextSubstitution(
                    text="The size of the `models` param array must be equal to the size of `names`"
                )
            )
        ]

    if num_cams != len(serials_arr):
        return [
            LogInfo(
                msg=TextSubstitution(
                    text="The size of the `serials` param array must be equal to the size of `names`"
                )
            )
        ]

    # Add the robot_state_publisher node to the list of nodes to be started
    # actions = [multi_rsp_node]
    # Removed the zed multi camera state publisher
    actions = []

    # Set the first camera idx
    cam_idx = 0

    for name in names_arr:
        model = models_arr[cam_idx]
        serial = serials_arr[cam_idx]
        pose = "["

        info = (
            "* Starting a ZED ROS2 node for camera "
            + name
            + "("
            + model
            + "/"
            + serial
            + ")"
        )

        actions.append(LogInfo(msg=TextSubstitution(text=info)))

        # Only the first camera send odom and map TF
        publish_tf = "false"
        if cam_idx == 0:
            if disable_tf_val == "False" or disable_tf_val == "false":
                publish_tf = "true"

        # A different node name is required by the Diagnostic Updated
        node_name = "zed_node_" + str(cam_idx)

        # Add the node
        # ZED Wrapper launch file
        zed_wrapper_launch = IncludeLaunchDescription(
            launch_description_source=PythonLaunchDescriptionSource(
                [
                    get_package_share_directory("zed_wrapper"),
                    "/launch/zed_camera.launch.py",
                ]
            ),
            launch_arguments={
                "camera_name": name,
                "camera_model": model,
                "serial_number": serial,
                "publish_tf": publish_tf,
                "publish_map_tf": publish_tf,
                "node_name": node_name,
            }.items(),
        )

        actions.append(zed_wrapper_launch)

        cam_idx += 1

    return actions


def generate_launch_description():
    return LaunchDescription(
        [
            DeclareLaunchArgument(
                "cam_names",
                description="An array containing the names of the cameras, e.g. [zed_front,zed_back]",
            ),
            DeclareLaunchArgument(
                "cam_models",
                description="An array containing the names of the cameras, e.g. [zed2i,zed2]",
            ),
            DeclareLaunchArgument(
                "cam_serials",
                description="An array containing the serial numbers of the cameras, e.g. [35199186,23154724]",
            ),
            DeclareLaunchArgument(
                "disable_tf",
                default_value="False",
                description="If `True` disable TF broadcasting for all the cameras in order to fuse visual odometry information externally.",
            ),
            OpaqueFunction(function=launch_setup),
        ]
    )

The previous file is launched from another launch file:

from launch import LaunchDescription
from launch.actions.include_launch_description import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource


def generate_launch_description():
    # Driver: ZEDX cameras -------------------------------------------------------------
    path = [MY_PKG_PATH, "/zed_multi_camera.launch.py"]
    args = [
        ("cam_names", "[zed_front,zed_rear]"),
        ("cam_models", "[zedx,zedx]"),
        ("cam_serials", "[40969433,47010842]"),
    ]
    zedx_camera_drivers = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(path), launch_arguments=args
    )
    # ----------------------------------------------------------------------------------

    return LaunchDescription([zedx_camera_drivers])

3. Call service to Start recording

ros2 service call /zed_rear/zed_node_1/start_svo_rec zed_interfaces/srv/StartSvoRec

6. Call service to Stop recording

ros2 service call /zed_rear/zed_node_1/stop_svo_rec std_srvs/srv/Trigger

Expected Result

N.B I have added extra INFO messages in order to follow the different calls that happen when the service is called. This is the only modification I did to the original code.

After calling the stop service, the recorded SVO is saved to disk and the node is ready to record again.

Actual Result

The node crashes after receiving the stop recording service call This behavior is not reproducible all the times, sometimes the recording process is completed without any issue, but the norm is that the node crashes after receiving the stop recording call.

It seems that the issue is due to the call to mZed.disableRecording() inside the method ZedCamera::stopSvoRecording()

ZED Camera model

ZED X

Environment

OS: Ubuntu 20.04.4 LTS CPU: model name : ARMv8 Processor rev 0 (v8l) GPU: NVIDIA Jetson AGX Xavier (JAX) Zed SKD: ZED SDK Version: 4.0.8 - Build 80393_b12da209

[abAgriRobot]

As an update, since this issue was quite blocking for our application, we decided to implement a ROS 2 node to record the SVO files for the two cameras using the SDK directly. Following the examples:

• Multiple camera recording
• Recording with Multiple Cameras

We were able to record SVO files without any issue.

However, we came accros the following bug:

• ZED SDK 4.0.8 C++ disableRecording() Not Working?

Since we were using the is_recording flag for starting and stopping the recording. This issue only allowed us to record once, after that the camera would remain with is_recording=true and I it would prevent us from starting a new recording. We added our own flag to keep track of the actual status of the cameras and with this we were able to record multiple SVO files for each camera, starting and stopping the recording without issues.

Once thing that we noticed is that due to the disableRecording bug, if the logic to check the actual status of the camera fails, and we start a new recording while there is an ongoing recording session running, then the node would try to start two new threads for the SVO recording and this would crash the node since the cameras were already in use. Maybe this could be related to the issue with the wrapper. We didn't investigate much further into this issue since with the correct logic this situation is avoided and the recording process is completed as expected.

[Myzhar]

Hi @abAgriRobot the disableRecording issue will be addressed by the ZED SDK v4.1 which will be released very soon.

[github-actions[bot]]

This issue is stale because it has been open 30 days with no activity. Remove stale label or comment otherwise it will be automatically closed in 5 days

[abAgriRobot]

Just commenting to keep the issue alive

[github-actions[bot]]

This issue is stale because it has been open 30 days with no activity. Remove stale label or comment otherwise it will be automatically closed in 5 days