search

stereolabs / zed-ros2-wrapper

ROS 2 wrapper for the ZED SDK
https://www.stereolabs.com/docs/ros2/
Apache License 2.0
134 stars 139 forks source link

Zed wrapper node crashes when /stop_svo_rec service is called #205

Open abAgriRobot opened 3 months ago

abAgriRobot commented 3 months ago

Preliminary Checks

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:
```bash
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])
  1. Call service to Start recording 
    ros2 service call /zed_rear/zed_node_1/start_svo_rec zed_interfaces/srv/StartSvoRec
  2. 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. image

Actual Result

The node crashes after receiving the stop recording service call image 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 commented 2 months ago

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:

We were able to record SVO files without any issue.

However, we came accros the following bug:

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 commented 2 months ago

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

github-actions[bot] commented 1 month ago

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 commented 1 month ago

Just commenting to keep the issue alive

github-actions[bot] commented 2 days ago

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 commented 1 day ago

I haven't had a chance to test the new SDK yet.