search

stereolabs / zed-ros2-wrapper

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

Zed2, ROS2, Jetson: Some topics output, but most do not. #56

Closed peredwardsson closed 3 years ago

peredwardsson commented 3 years ago

Preliminary Checks

Description

On starting the node, all topics that are advertised are displayed. Only a fraction of them actually publish anything. Critically, depth images or stereo images are not published, while IMU and zed_node/stereo/image_rect_color are published.

Steps to Reproduce

In a Docker environment running on Jetson AGX Xavier, build ZED SDK and Zed Wrapper:

ARG BASE_IMAGE=nvcr.io/nvidia/l4t-base:r32.5.0
FROM ${BASE_IMAGE}
[...]
RUN apt-get update -y && apt-get install --no-install-recommends lsb-release wget less udev sudo  build-essential cmake -y && \
    wget -q --no-check-certificate -O ZED_SDK_Linux_JP.run https://download.stereolabs.com/zedsdk/3.5/jp45/jetsons && \
    chmod +x ZED_SDK_Linux_JP.run ; ./ZED_SDK_Linux_JP.run silent skip_tools && \
    rm -rf /usr/local/zed/resources/* \
    rm -rf ZED_SDK_Linux_JP.run && \
    rm -rf /var/lib/apt/lists/*
[...]
ARG ROS_ENVIRONMENT=${ROS_ROOT}/install/setup.bash
RUN source ${ROS_ENVIRONMENT} && \
    cd ${ROS_ROOT} && \
    mkdir src/slam && \
    rosinstall_generator --exclude-path ${ROS_ROOT}/src --rosdistro ${ROS_DISTRO} \
    diagnostic_updater \
    xacro \
    > ros2.${ROS_DISTRO}.zed.rosinstall && \
    cat ros2.${ROS_DISTRO}.zed.rosinstall && \
    vcs import src/slam < ros2.${ROS_DISTRO}.zed.rosinstall && \
    apt-get update && \
    rosdep install --from-paths src/slam --ignore-src --rosdistro ${ROS_DISTRO} -y --skip-keys "Pangolin libopencv-dev libopencv-contrib-dev libopencv-imgproc-dev python-opencv python3-opencv" && \
    rm -rf /var/lib/apt/lists/* && \
    apt-get clean && \
    colcon build --merge-install --base-paths src/slam

RUN source ${ROS_ENVIRONMENT} && \
    cd ${ROS_ROOT} && \
    git clone https://github.com/stereolabs/zed-ros2-wrapper src/slam/zed-ros2-wrapper && \
    apt-get update && \
    rosdep install --from-paths src/slam --ignore-src --rosdistro ${ROS_DISTRO} -y --skip-keys "rtabmap find_object_2d Pangolin libopencv-dev libopencv-contrib-dev libopencv-imgproc-dev python-opencv python3-opencv" && \
    rm -rf /var/lib/apt/lists/* && \
    apt-get clean && \
    colcon build --merge-install --base-paths src/slam/zed-ros2-wrapper --cmake-args=-DCMAKE_BUILD_TYPE=Release

  1. Start node with the following config:

    /**:
ros__parameters:
    general:
        svo_file:                   ''
        svo_loop:                   false                               # Enable loop mode when using an SVO as input source
        svo_realtime:               false                               # if true SVO will be played trying to respect the original framerate eventually skipping frames, otherwise every frame will be processed respecting the `pub_frame_rate` setting
        debug_mode:                 false
        camera_timeout_sec:         5
        camera_max_reconnect:       5
        camera_flip:                false
        zed_id:                     0
        serial_number:              0
        resolution:                 2                                   # '0': HD2K, '1': HD1080, '2': HD720, '3': VGA
        sdk_verbose:                true
        grab_frame_rate:            30                                  # ZED SDK internal grabbing rate
        pub_frame_rate:             15.0                                # [DYNAMIC] - frequency of publishing of visual images and depth images
        gpu_id:                     0
        camera_model:               'zed2'
        camera_name:                'zed2'

    video:
        extrinsic_in_camera_frame:  false                               # if `false` extrinsic parameter in `camera_info` will use ROS native frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y DOWN) [`true` use old behavior as for version < v3.1]
        img_downsample_factor:      0.5                                 # Resample factor for image data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices
        brightness:                 4                                   # [DYNAMIC]
        contrast:                   4                                   # [DYNAMIC]
        hue:                        0                                   # [DYNAMIC]
        saturation:                 4                                   # [DYNAMIC]
        sharpness:                  4                                   # [DYNAMIC]
        gamma:                      8                                   # [DYNAMIC] - Requires SDK >=v3.1
        auto_exposure_gain:         true                                # [DYNAMIC]
        exposure:                   80                                  # [DYNAMIC]
        gain:                       80                                  # [DYNAMIC]
        auto_whitebalance:          true                                # [DYNAMIC]
        whitebalance_temperature:   42                                  # [DYNAMIC] - [28,65] works only if `auto_whitebalance` is false
        qos_history:                1                                   # '1': KEEP_LAST - '2': KEEP_ALL
        qos_depth:                  1                                   # Queue size if using KEEP_LAST
        qos_reliability:            2                                   # '1': RELIABLE - '2': BEST_EFFORT -
        qos_durability:             2                                   # '1': TRANSIENT_LOCAL - '2': VOLATILE

    depth:
        quality:                    3                                   # '0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA - Note: if '0' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...)
        sensing_mode:               0                                   # '0': STANDARD, '1': FILL
        depth_stabilization:        true                                # Forces positional tracking to start if 'true'
        openni_depth_mode:          false                               # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters]
        depth_downsample_factor:    0.5                                 # Resample factor for depth data matrices [0.01,1.0] The SDK works with native data sizes, but publishes rescaled matrices (depth map, point cloud, ...)
        point_cloud_freq:           10.0                                # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value)
        depth_confidence:           50                                  # [DYNAMIC]
        depth_texture_conf:         100                                 # [DYNAMIC]
        qos_history:                1                                   # '1': KEEP_LAST - '2': KEEP_ALL
        qos_depth:                  1                                   # Queue size if using KEEP_LAST
        qos_reliability:            2                                   # '1': RELIABLE - '2': BEST_EFFORT -
        qos_durability:             2                                   # '1': TRANSIENT_LOCAL - '2': VOLATILE
        min_depth:                  0.3                                 # Min: 0.2, Max: 3.0 - Default 0.7 - Note: reducing this value wil require more computational power and GPU memory
        max_depth:                  20.0                                # Max: 40.0

    pos_tracking:
        pos_tracking_enabled:       true                                # True to enable positional tracking from start
        publish_tf:                 false                                # publish `odom -> base_link` TF
        publish_map_tf:             false                                # publish `map -> odom` TF
        base_frame:                 'base_link'                         # use the same name as in the URDF file
        map_frame:                  'map'
        odometry_frame:             'odom'
        area_memory_db_path:        ''
        area_memory:                true                                # Enable to detect loop closure
        floor_alignment:            true                               # Enable to automatically calculate camera/floor offset
        initial_base_pose:          [0.0,0.0,0.0, 0.0,0.0,0.0]          # Initial position of the `base_frame` -> [X, Y, Z, R, P, Y]
        init_odom_with_first_valid_pose: true                           # Enable to initialize the odometry with the first valid pose
        path_pub_rate:              2.0                                 # [DYNAMIC] - Camera trajectory publishing frequency
        path_max_count:             -1                                  # use '-1' for unlimited path size
        two_d_mode:                 true                               # Force navigation on a plane. If true the Z value will be fixed to "fixed_z_value", roll and pitch to zero
        fixed_z_value:              0.00                                # Value to be used for Z coordinate if `two_d_mode` is true
        qos_history:                1                                   # '1': KEEP_LAST - '2': KEEP_ALL
        qos_depth:                  1                                   # Queue size if using KEEP_LAST
        qos_reliability:            2                                   # '1': RELIABLE - '2': BEST_EFFORT -
        qos_durability:             2                                   # '1': TRANSIENT_LOCAL - '2': VOLATILE
        imu_fusion:                 true            # enable/disable IMU fusion. When set to false, only the optical odometry will be used.
        publish_imu_tf:             true            # enable/disable the static IMU TF broadcasting

    mapping:
        mapping_enabled:            false                               # True to enable mapping and fused point cloud pubblication
        resolution:                 0.1                                # maps resolution in meters [0.01f, 0.2f]
        max_mapping_range:          20.0                                  # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0]
        fused_pointcloud_freq:      0.5                                 # frequency of the publishing of the fused colored point cloud
        qos_history:                1                                   # '1': KEEP_LAST - '2': KEEP_ALL
        qos_depth:                  1                                   # Queue size if using KEEP_LAST
        qos_reliability:            2                                   # '1': RELIABLE - '2': BEST_EFFORT -
        qos_durability:             2                                   # '1': TRANSIENT_LOCAL - '2': VOLATILE

    object_detection:
        od_enabled:                 false           # True to enable Object Detection [only ZED 2]
        confidence_threshold:       50.0            # [DYNAMIC] - Minimum value of the detection confidence of an object [0,100]
        model:                      0               # '0': MULTI_CLASS_BOX - '1': MULTI_CLASS_BOX_ACCURATE - '2': HUMAN_BODY_FAST - '3': HUMAN_BODY_ACCURATE - '4': MULTI_CLASS_BOX_MEDIUM - '5': HUMAN_BODY_MEDIUM 
        mc_people:                  true            # [DYNAMIC] - Enable/disable the detection of persons for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models
        mc_vehicle:                 true            # [DYNAMIC] - Enable/disable the detection of vehicles for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models
        mc_bag:                     true            # [DYNAMIC] - Enable/disable the detection of bags for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models
        mc_animal:                  true            # [DYNAMIC] - Enable/disable the detection of animals for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models
        mc_electronics:             true            # [DYNAMIC] - Enable/disable the detection of electronic devices for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models
        mc_fruit_vegetable:         true            # [DYNAMIC] - Enable/disable the detection of fruits and vegetables for 'MULTI_CLASS_BOX' and 'MULTI_CLASS_BOX_ACCURATE' models
        body_fitting:               false           # Enable/disable body fitting for 'HUMAN_BODY_FAST' and 'HUMAN_BODY_ACCURATE' models
        qos_history:                1               # '1': KEEP_LAST - '2': KEEP_ALL
        qos_depth:                  1               # Queue size if using KEEP_LAST
        qos_reliability:            2               # '1': RELIABLE - '2': BEST_EFFORT -
        qos_durability:             2               # '1': TRANSIENT_LOCAL - '2': VOLATILE

    sensors:
        sensors_image_sync:         false           # Synchronize Sensors messages with latest published video/depth message
        sensors_pub_rate:           200.            # frequency of publishing of sensors data. MAX: 400. - MIN: grab rate
        qos_history:                1               # '1': KEEP_LAST - '2': KEEP_ALL
        qos_depth:                  1               # Queue size if using KEEP_LAST
        qos_reliability:            2               # '1': RELIABLE - '2': BEST_EFFORT -
        qos_durability:             2               # '1': TRANSIENT_LOCAL - '2': VOLATILE
  2. Observe the (quite long) launch process: 
    # ros2 launch hugo rtabmap.py 
[INFO] [launch]: All log files can be found below /root/.ros/log/2021-09-22-09-53-37-177059-hugo_ada-4460
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ekf_node-1]: process started with pid [4463]
[INFO] [odrive_ros2_uart-2]: process started with pid [4464]
[INFO] [zed_wrapper-3]: process started with pid [4465]
[INFO] [robot_state_publisher-4]: process started with pid [4466]
[INFO] [rgbd_sync-5]: process started with pid [4467]
[INFO] [rtabmap-6]: process started with pid [4468]
[INFO] [point_cloud_xyz-7]: process started with pid [4477]
[ekf_node-1] RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED is now ignored.  Please set RCUTILS_LOGGING_USE_STDOUT and RCUTILS_LOGGING_BUFFERED_STREAM to control the stream and the buffering of log messages.
[robot_state_publisher-4] RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED is now ignored.  Please set RCUTILS_LOGGING_USE_STDOUT and RCUTILS_LOGGING_BUFFERED_STREAM to control the stream and the buffering of log messages.
[robot_state_publisher-4] 1632304417.559410313 [localization.robot_state_publisher] [WARN] use_tf_static is deprecated and will be removed in the future
[robot_state_publisher-4] Parsing robot urdf xml string.
[robot_state_publisher-4] Link camera_back had 0 children
[robot_state_publisher-4] Link camera_front had 1 children
[robot_state_publisher-4] Link zed2_camera_center had 2 children
[robot_state_publisher-4] Link zed2_left_camera_frame had 1 children
[robot_state_publisher-4] Link zed2_left_camera_optical_frame had 0 children
[robot_state_publisher-4] Link zed2_right_camera_frame had 1 children
[robot_state_publisher-4] Link zed2_right_camera_optical_frame had 0 children
[robot_state_publisher-4] Link caster_left had 0 children
[robot_state_publisher-4] Link caster_right had 0 children
[robot_state_publisher-4] Link laser_back had 0 children
[robot_state_publisher-4] Link laser_front had 0 children
[robot_state_publisher-4] 1632304417.575684903 [localization.robot_state_publisher] [INFO] got segment base_link
[robot_state_publisher-4] 1632304417.575842123 [localization.robot_state_publisher] [INFO] got segment camera_back
[robot_state_publisher-4] 1632304417.575909933 [localization.robot_state_publisher] [INFO] got segment camera_front
[robot_state_publisher-4] 1632304417.575974543 [localization.robot_state_publisher] [INFO] got segment caster_left
[robot_state_publisher-4] 1632304417.576028977 [localization.robot_state_publisher] [INFO] got segment caster_right
[robot_state_publisher-4] 1632304417.576086866 [localization.robot_state_publisher] [INFO] got segment laser_back
[robot_state_publisher-4] 1632304417.576141844 [localization.robot_state_publisher] [INFO] got segment laser_front
[robot_state_publisher-4] 1632304417.576206710 [localization.robot_state_publisher] [INFO] got segment zed2_camera_center
[robot_state_publisher-4] 1632304417.576268984 [localization.robot_state_publisher] [INFO] got segment zed2_left_camera_frame
[robot_state_publisher-4] 1632304417.576323897 [localization.robot_state_publisher] [INFO] got segment zed2_left_camera_optical_frame
[robot_state_publisher-4] 1632304417.576381979 [localization.robot_state_publisher] [INFO] got segment zed2_right_camera_frame
[robot_state_publisher-4] 1632304417.576436221 [localization.robot_state_publisher] [INFO] got segment zed2_right_camera_optical_frame
[zed_wrapper-3] RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED is now ignored.  Please set RCUTILS_LOGGING_USE_STDOUT and RCUTILS_LOGGING_BUFFERED_STREAM to control the stream and the buffering of log messages.
[zed_wrapper-3] 1632304417.585981396 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_depth_image_transport/compressed_depth_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[zed_wrapper-3] 1632304417.586106232 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_image_transport/compressed_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[zed_wrapper-3] 1632304417.586934000 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/src/workspace/install/theora_image_transport/share/theora_image_transport/theora_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[zed_wrapper-3] 1632304417.589518172 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_depth_image_transport/compressed_depth_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[zed_wrapper-3] 1632304417.589604799 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_image_transport/compressed_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[zed_wrapper-3] 1632304417.589991914 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/src/workspace/install/theora_image_transport/share/theora_image_transport/theora_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[zed_wrapper-3] 1632304417.651718780 [sensors.zed_node] [INFO] ********************************
[zed_wrapper-3] 1632304417.652116328 [sensors.zed_node] [INFO]       ZED Camera Component 
[zed_wrapper-3] 1632304417.652244715 [sensors.zed_node] [INFO] ********************************
[zed_wrapper-3] 1632304417.652327502 [sensors.zed_node] [INFO]  * namespace: /sensors
[zed_wrapper-3] 1632304417.652381007 [sensors.zed_node] [INFO]  * node name: zed_node
[zed_wrapper-3] 1632304417.652428145 [sensors.zed_node] [INFO] ********************************
[zed_wrapper-3] 1632304417.652995457 [sensors.zed_node] [INFO] *** DEBUG parameters ***
[zed_wrapper-3] 1632304417.653113285 [sensors.zed_node] [INFO] *** GENERAL parameters ***
[zed_wrapper-3] 1632304417.653533873 [sensors.zed_node] [INFO]  * Camera model: zed2 - ZED 2
[zed_wrapper-3] 1632304417.653937789 [sensors.zed_node] [INFO]  * SDK Verbose: 1
[zed_wrapper-3] 1632304417.654344841 [sensors.zed_node] [INFO]  * SVO: 
[zed_wrapper-3] 1632304417.654699347 [sensors.zed_node] [INFO]  * SVO Loop: FALSE
[zed_wrapper-3] 1632304417.656247681 [sensors.zed_node] [INFO]  * SVO Realtime: FALSE
[zed_wrapper-3] 1632304417.656731567 [sensors.zed_node] [INFO]  * Camera name: zed2
[zed_wrapper-3] 1632304417.657113690 [sensors.zed_node] [INFO]  * Camera ID: 0
[zed_wrapper-3] 1632304417.657504294 [sensors.zed_node] [INFO]  * Camera SN: 0
[zed_wrapper-3] 1632304417.658259548 [sensors.zed_node] [INFO]  * Camera reconnection temptatives: 5
[zed_wrapper-3] 1632304417.658575941 [sensors.zed_node] [INFO]  * Camera framerate: 30
[zed_wrapper-3] 1632304417.658990001 [sensors.zed_node] [INFO]  * GPU ID: 0
[zed_wrapper-3] 1632304417.659346780 [sensors.zed_node] [INFO]  * Camera resolution: 2 - HD720
[zed_wrapper-3] 1632304417.659653541 [sensors.zed_node] [INFO]  * Camera self calibration: TRUE
[zed_wrapper-3] 1632304417.659973454 [sensors.zed_node] [INFO]  * Camera flip: FALSE
[zed_wrapper-3] 1632304417.660387578 [sensors.zed_node] [INFO] *** VIDEO parameters ***
[zed_wrapper-3] 1632304417.660727460 [sensors.zed_node] [INFO]  * Use old extrinsic parameters: 0
[zed_wrapper-3] 1632304417.661043725 [sensors.zed_node] [INFO]  * [DYN] Image downsample factor: 0.5 
[zed_wrapper-3] 1632304417.661385175 [sensors.zed_node] [INFO]  * [DYN] Brightness: 4
[zed_wrapper-3] 1632304417.661727393 [sensors.zed_node] [INFO]  * [DYN] Contrast: 4
[zed_wrapper-3] 1632304417.662124077 [sensors.zed_node] [INFO]  * [DYN] Hue: 0
[zed_wrapper-3] 1632304417.662447703 [sensors.zed_node] [INFO]  * [DYN] Saturation: 4
[zed_wrapper-3] 1632304417.662768608 [sensors.zed_node] [INFO]  * [DYN] Sharpness: 4
[zed_wrapper-3] 1632304417.663146955 [sensors.zed_node] [INFO]  * [DYN] Gamma: 8
[zed_wrapper-3] 1632304417.663481205 [sensors.zed_node] [INFO]  * [DYN] Auto Exposure/Gain: TRUE
[zed_wrapper-3] 1632304417.663821471 [sensors.zed_node] [INFO]  * [DYN] Exposure: 80
[zed_wrapper-3] 1632304417.664170313 [sensors.zed_node] [INFO]  * [DYN] Gain: 80
[zed_wrapper-3] 1632304417.664503187 [sensors.zed_node] [INFO]  * [DYN] Auto White Balance: TRUE
[zed_wrapper-3] 1632304417.664831964 [sensors.zed_node] [INFO]  * [DYN] White Balance Temperature: 42
[zed_wrapper-3] 1632304417.665168230 [sensors.zed_node] [INFO]  * Video QoS History: KEEP_LAST
[zed_wrapper-3] 1632304417.665528529 [sensors.zed_node] [INFO]  * Video QoS History depth: 1
[zed_wrapper-3] 1632304417.665876251 [sensors.zed_node] [INFO]  * Video QoS Reliability: BEST_EFFORT
[zed_wrapper-3] 1632304417.666266022 [sensors.zed_node] [INFO]  * Video QoS Durability: VOLATILE
[zed_wrapper-3] 1632304417.666355113 [sensors.zed_node] [INFO] *** DEPTH parameters ***
[zed_wrapper-3] 1632304417.666678067 [sensors.zed_node] [INFO]  * Depth quality: 1 - PERFORMANCE
[zed_wrapper-3] 1632304417.667022781 [sensors.zed_node] [INFO]  * Depth downsample factor: 0.5 
[zed_wrapper-3] 1632304417.668122173 [sensors.zed_node] [INFO]  * Depth Sensing Mode: 1 - FILL
[zed_wrapper-3] 1632304417.668509224 [sensors.zed_node] [INFO]  * Depth Stabilization: TRUE
[zed_wrapper-3] 1632304417.668829426 [sensors.zed_node] [INFO]  * OpenNI mode (16bit point cloud): FALSE
[zed_wrapper-3] 1632304417.669579656 [sensors.zed_node] [INFO]  * [DYN] Depth Confidence: 50
[zed_wrapper-3] 1632304417.669925074 [sensors.zed_node] [INFO]  * [DYN] Depth Texture Confidence: 100
[zed_wrapper-3] 1632304417.670286812 [sensors.zed_node] [INFO]  * Depth QoS History: KEEP_LAST
[zed_wrapper-3] 1632304417.670627750 [sensors.zed_node] [INFO]  * Depth QoS History depth: 1
[zed_wrapper-3] 1632304417.671027026 [sensors.zed_node] [INFO]  * Depth QoS Reliability: BEST_EFFORT
[zed_wrapper-3] 1632304417.671442110 [sensors.zed_node] [INFO]  * Depth QoS Durability: TRANSIENT_LOCAL
[zed_wrapper-3] 1632304417.671536001 [sensors.zed_node] [INFO] *** POSITIONAL TRACKING parameters ***
[zed_wrapper-3] 1632304417.671884619 [sensors.zed_node] [INFO]  * Positional tracking enabled: TRUE
[zed_wrapper-3] 1632304417.672243446 [sensors.zed_node] [INFO]  * Base frame id: base_link
[zed_wrapper-3] 1632304417.672592736 [sensors.zed_node] [INFO]  * Map frame id: map
[zed_wrapper-3] 1632304417.672944906 [sensors.zed_node] [INFO]  * Odometry frame id: odom
[zed_wrapper-3] 1632304417.673295893 [sensors.zed_node] [INFO]  * Broadcast Odometry TF: FALSE
[zed_wrapper-3] 1632304417.673716641 [sensors.zed_node] [INFO]  * [DYN] Path publishing rate: 2
[zed_wrapper-3] 1632304417.674051851 [sensors.zed_node] [INFO]  * Path history lenght: -1
[zed_wrapper-3] 1632304417.674469623 [sensors.zed_node] [INFO]  * Initial pose: [0,0,0,0,0,0,]
[zed_wrapper-3] 1632304417.674835330 [sensors.zed_node] [INFO]  * Area Memory: TRUE
[zed_wrapper-3] 1632304417.675280399 [sensors.zed_node] [INFO]  * Area Memory DB: 
[zed_wrapper-3] 1632304417.675982851 [sensors.zed_node] [INFO]  * Floor Alignment: TRUE
[zed_wrapper-3] 1632304417.676346478 [sensors.zed_node] [INFO]  * Init Odometry with first valid pose data: TRUE
[zed_wrapper-3] 1632304417.676657879 [sensors.zed_node] [INFO]  * 2D mode: TRUE
[zed_wrapper-3] 1632304417.677114693 [sensors.zed_node] [INFO]  * Fixed Z value: 0
[zed_wrapper-3] 1632304417.677453582 [sensors.zed_node] [INFO]  * Pose/Odometry QoS History: KEEP_LAST
[zed_wrapper-3] 1632304417.677777176 [sensors.zed_node] [INFO]  * Pose/Odometry QoS History depth: 1
[zed_wrapper-3] 1632304417.678128418 [sensors.zed_node] [INFO]  * Pose/Odometry QoS Reliability: BEST_EFFORT
[zed_wrapper-3] 1632304417.678464844 [sensors.zed_node] [INFO]  * Pose/Odometry QoS Durability: VOLATILE
[zed_wrapper-3] 1632304417.678538254 [sensors.zed_node] [INFO] *** SENSORS STACK parameters ***
[zed_wrapper-3] 1632304417.678953114 [sensors.zed_node] [INFO]  * Sensors Camera Sync: TRUE
[zed_wrapper-3] 1632304417.680450918 [sensors.zed_node] [INFO]  * Sensors publishing rate: 200 Hz
[zed_wrapper-3] 1632304417.680851282 [sensors.zed_node] [INFO]  * Sensors QoS History: KEEP_LAST
[zed_wrapper-3] 1632304417.681263550 [sensors.zed_node] [INFO]  * Sensors QoS History depth: 1
[zed_wrapper-3] 1632304417.681615913 [sensors.zed_node] [INFO]  * Sensors QoS Reliability: BEST_EFFORT
[zed_wrapper-3] 1632304417.682015252 [sensors.zed_node] [INFO]  * Sensors QoS Durability: VOLATILE
[zed_wrapper-3] 1632304417.682100119 [sensors.zed_node] [INFO] *** Spatial Mapping parameters ***
[zed_wrapper-3] 1632304417.682441057 [sensors.zed_node] [INFO]  * Spatial Mapping Enabled: FALSE
[zed_wrapper-3] 1632304417.683897611 [sensors.zed_node] [INFO]  * Sensors QoS History: KEEP_LAST
[zed_wrapper-3] 1632304417.684213653 [sensors.zed_node] [INFO]  * Sensors QoS History depth: 1
[zed_wrapper-3] 1632304417.684538334 [sensors.zed_node] [INFO]  * Sensors QoS Reliability: BEST_EFFORT
[zed_wrapper-3] 1632304417.684913353 [sensors.zed_node] [INFO]  * Sensors QoS Durability: VOLATILE
[zed_wrapper-3] 1632304417.684986411 [sensors.zed_node] [INFO] *** OBJECT DETECTION parameters ***
[zed_wrapper-3] 1632304417.685291316 [sensors.zed_node] [INFO]  * Object Detection enabled: FALSE
[zed_wrapper-3] 1632304417.685634238 [sensors.zed_node] [INFO]  * OD min. confidence: 50
[zed_wrapper-3] 1632304417.686003753 [sensors.zed_node] [INFO]  * Object Detection model: 0 - MULTI CLASS BOX FAST
[zed_wrapper-3] 1632304417.686331859 [sensors.zed_node] [INFO]  * MultiClassBox people: TRUE
[zed_wrapper-3] 1632304417.686679901 [sensors.zed_node] [INFO]  * MultiClassBox vehicles: TRUE
[zed_wrapper-3] 1632304417.687043144 [sensors.zed_node] [INFO]  * MultiClassBox bags: TRUE
[zed_wrapper-3] 1632304417.687416915 [sensors.zed_node] [INFO]  * MultiClassBox animals: TRUE
[zed_wrapper-3] 1632304417.687781213 [sensors.zed_node] [INFO]  * MultiClassBox electronics: TRUE
[zed_wrapper-3] 1632304417.688146440 [sensors.zed_node] [INFO]  * MultiClassBox fruits and vegetables: TRUE
[zed_wrapper-3] 1632304417.688466353 [sensors.zed_node] [INFO]  * Skeleton fitting: FALSE
[zed_wrapper-3] 1632304417.688804059 [sensors.zed_node] [INFO]  * Obj. Det. QoS History: KEEP_LAST
[zed_wrapper-3] 1632304417.689105700 [sensors.zed_node] [INFO]  * Obj. Det. QoS History depth: 1
[zed_wrapper-3] 1632304417.689573010 [sensors.zed_node] [INFO]  * Obj. Det. QoS Reliability: BEST_EFFORT
[zed_wrapper-3] 1632304417.689886843 [sensors.zed_node] [INFO]  * Obj. Det. QoS Durability: VOLATILE
[zed_wrapper-3] 1632304417.690144131 [sensors.zed_node] [INFO] *** SERVICES ***
[zed_wrapper-3] 1632304417.692003705 [sensors.zed_node] [INFO]  * '/sensors/zed_node/reset_odometry'
[zed_wrapper-3] 1632304417.693423235 [sensors.zed_node] [INFO]  * '/sensors/zed_node/reset_pos_tracking'
[zed_wrapper-3] 1632304417.695982702 [sensors.zed_node] [INFO]  * '/sensors/zed_node/set_pose'
[zed_wrapper-3] 1632304417.697611294 [sensors.zed_node] [INFO]  * '/sensors/zed_node/enable_obj_det'
[zed_wrapper-3] 1632304417.698987526 [sensors.zed_node] [INFO]  * '/sensors/zed_node/enable_mapping'
[zed_wrapper-3] 1632304417.700605525 [sensors.zed_node] [INFO]  * '/sensors/zed_node/start_svo_rec'
[zed_wrapper-3] 1632304417.701787960 [sensors.zed_node] [INFO]  * '/sensors/zed_node/stop_svo_rec'
[zed_wrapper-3] 1632304417.703195873 [sensors.zed_node] [INFO]  * '/sensors/zed_node/toggle_svo_pause'
[zed_wrapper-3] 1632304417.703318021 [sensors.zed_node] [INFO] ***** STARTING CAMERA *****
[zed_wrapper-3] 1632304417.703401671 [sensors.zed_node] [INFO] SDK Version: 3.5.3 - Build 28371_2336704c
[zed_wrapper-3] 1632304417.737327210 [sensors.zed_node] [INFO] *** CAMERA OPENING ***
[odrive_ros2_uart-2] RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED is now ignored.  Please set RCUTILS_LOGGING_USE_STDOUT and RCUTILS_LOGGING_BUFFERED_STREAM to control the stream and the buffering of log messages.
[point_cloud_xyz-7] RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED is now ignored.  Please set RCUTILS_LOGGING_USE_STDOUT and RCUTILS_LOGGING_BUFFERED_STREAM to control the stream and the buffering of log messages.
[point_cloud_xyz-7] 1632304418.519125382 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_depth_image_transport/compressed_depth_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[point_cloud_xyz-7] 1632304418.519230825 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_image_transport/compressed_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[point_cloud_xyz-7] 1632304418.519867388 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/src/workspace/install/theora_image_transport/share/theora_image_transport/theora_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[point_cloud_xyz-7] 1632304418.522058493 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_depth_image_transport/compressed_depth_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[point_cloud_xyz-7] 1632304418.522111294 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_image_transport/compressed_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[point_cloud_xyz-7] 1632304418.522371462 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/src/workspace/install/theora_image_transport/share/theora_image_transport/theora_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[point_cloud_xyz-7] 1632304418.623756338 [pointcloud] [INFO] Approximate time sync = true
[rgbd_sync-5] RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED is now ignored.  Please set RCUTILS_LOGGING_USE_STDOUT and RCUTILS_LOGGING_BUFFERED_STREAM to control the stream and the buffering of log messages.
[rgbd_sync-5] 1632304418.641516126 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_depth_image_transport/compressed_depth_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[rgbd_sync-5] 1632304418.641830535 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_image_transport/compressed_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[rgbd_sync-5] 1632304418.645006309 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/src/workspace/install/theora_image_transport/share/theora_image_transport/theora_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[rgbd_sync-5] 1632304418.651911824 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_depth_image_transport/compressed_depth_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[rgbd_sync-5] 1632304418.652026036 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_image_transport/compressed_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[rgbd_sync-5] 1632304418.652421887 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/src/workspace/install/theora_image_transport/share/theora_image_transport/theora_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[odrive_ros2_uart-2] 1632304418.689967986 [actuators.esc.odrive_interface_UART] [INFO] Starting odrive node
[rtabmap-6] RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED is now ignored.  Please set RCUTILS_LOGGING_USE_STDOUT and RCUTILS_LOGGING_BUFFERED_STREAM to control the stream and the buffering of log messages.
[rtabmap-6] 1632304418.781888648 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_depth_image_transport/compressed_depth_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[rtabmap-6] 1632304418.782004331 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_image_transport/compressed_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[rtabmap-6] 1632304418.784719099 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/src/workspace/install/theora_image_transport/share/theora_image_transport/theora_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[rtabmap-6] 1632304418.788157856 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_depth_image_transport/compressed_depth_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[rtabmap-6] 1632304418.788227747 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/opt/ros/foxy/install/share/compressed_image_transport/compressed_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[rtabmap-6] 1632304418.788539756 [pluginlib.ClassLoader] [ERROR] Skipped loading plugin with error: XML Document '/src/workspace/install/theora_image_transport/share/theora_image_transport/theora_plugins.xml' has no Root Element. This likely means the XML is malformed or missing..
[odrive_ros2_uart-2] 1632304418.803815086 [actuators.esc.odrive_interface_UART] [INFO] Connecting to odrive...
[odrive_ros2_uart-2] 1632304418.815775439 [actuators.esc.odrive_interface_UART] [INFO] Connected to ODrive with serial number b'lu\r\n'
[rgbd_sync-5] 1632304418.829513860 [rbgd_sync] [INFO] rbgd_sync: approx_sync = true
[rgbd_sync-5] 1632304418.834636987 [rbgd_sync] [INFO] rbgd_sync: queue_size  = 10
[rgbd_sync-5] 1632304418.836435184 [rbgd_sync] [INFO] rbgd_sync: depth_scale = 1.000000
[rgbd_sync-5] 1632304418.838055007 [rbgd_sync] [INFO] rbgd_sync: compressed_rate = 0.000000
[rtabmap-6] 1632304418.901692595 [rtabmap] [INFO] rtabmap: subscribe_depth = false
[rtabmap-6] 1632304418.902238275 [rtabmap] [INFO] rtabmap: subscribe_rgb = false
[rtabmap-6] 1632304418.902326598 [rtabmap] [INFO] rtabmap: subscribe_stereo = false
[rtabmap-6] 1632304418.902384680 [rtabmap] [INFO] rtabmap: subscribe_rgbd = true (rgbd_cameras=1)
[rtabmap-6] 1632304418.902437897 [rtabmap] [INFO] rtabmap: subscribe_odom_info = false
[rtabmap-6] 1632304418.902488331 [rtabmap] [INFO] rtabmap: subscribe_user_data = false
[rtabmap-6] 1632304418.902579629 [rtabmap] [INFO] rtabmap: subscribe_scan = false
[rtabmap-6] 1632304418.902633519 [rtabmap] [INFO] rtabmap: subscribe_scan_cloud = false
[rtabmap-6] 1632304418.902682992 [rtabmap] [INFO] rtabmap: queue_size    = 10
[rtabmap-6] 1632304418.902738898 [rtabmap] [INFO] rtabmap: approx_sync   = false
[rtabmap-6] 1632304418.909669598 [rtabmap] [INFO] rtabmap(maps): map_filter_radius          = 0.000000
[rtabmap-6] 1632304418.909814115 [rtabmap] [INFO] rtabmap(maps): map_filter_angle           = 30.000000
[rtabmap-6] 1632304418.909879300 [rtabmap] [INFO] rtabmap(maps): map_cleanup                = true
[rtabmap-6] 1632304418.909949511 [rtabmap] [INFO] rtabmap(maps): map_always_update          = true
[rtabmap-6] 1632304418.910012040 [rtabmap] [INFO] rtabmap(maps): map_empty_ray_tracing      = true
[rtabmap-6] 1632304418.910065706 [rtabmap] [INFO] rtabmap(maps): cloud_output_voxelized     = true
[rtabmap-6] 1632304418.910115659 [rtabmap] [INFO] rtabmap(maps): cloud_subtract_filtering   = false
[rtabmap-6] 1632304418.910164653 [rtabmap] [INFO] rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[rgbd_sync-5] 1632304418.938951805 [rbgd_sync] [INFO] 
[rgbd_sync-5] rbgd_sync subscribed to (approx sync):
[rgbd_sync-5]    /sensors/zed_node/stereo/image_rect_color,
[rgbd_sync-5]    /sensors/zed_node/depth/depth_registered,
[rgbd_sync-5]    rgb/camera_info
[rtabmap-6] 1632304418.968051095 [rtabmap] [INFO] rtabmap: frame_id      = base_link
[rtabmap-6] 1632304418.969301276 [rtabmap] [INFO] rtabmap: odom_frame_id = odom
[rtabmap-6] 1632304418.969377950 [rtabmap] [INFO] rtabmap: map_frame_id  = map
[rtabmap-6] 1632304418.969428128 [rtabmap] [INFO] rtabmap: use_action_for_goal  = false
[rtabmap-6] 1632304418.969474433 [rtabmap] [INFO] rtabmap: tf_delay      = 0.050000
[rtabmap-6] 1632304418.969534339 [rtabmap] [INFO] rtabmap: tf_tolerance  = 0.100000
[rtabmap-6] 1632304418.969584260 [rtabmap] [INFO] rtabmap: odom_sensor_sync   = false
[rtabmap-6] 1632304419.039643995 [rtabmap] [INFO] Setting RTAB-Map parameter "Grid/RayTracing"="True"
[rtabmap-6] 1632304419.239532221 [rtabmap] [INFO] RTAB-Map detection rate = 1.000000 Hz
[rtabmap-6] 1632304419.239868007 [rtabmap] [INFO] rtabmap: Using database from "/root/.ros/rtabmap.db" (1924 MB).
[zed_wrapper-3] 1632304420.082662717 [sensors.zed_node] [INFO] ZED SDK running on GPU #0
[zed_wrapper-3] 1632304420.082983334 [sensors.zed_node] [INFO]  * Camera Model  -> ZED 2
[zed_wrapper-3] 1632304420.083161996 [sensors.zed_node] [INFO]  * Serial Number -> 22535431
[zed_wrapper-3] 1632304420.083325617 [sensors.zed_node] [INFO]  * Input type    -> USB input type
[zed_wrapper-3] 1632304420.083414483 [sensors.zed_node] [INFO]  * Camera FW Version -> 1523
[zed_wrapper-3] 1632304420.083490613 [sensors.zed_node] [INFO]  * Sensors FW Version -> 776
[zed_wrapper-3] 1632304420.083730205 [sensors.zed_node] [INFO] *** TF FRAMES ***
[zed_wrapper-3] 1632304420.083830016 [sensors.zed_node] [INFO]  * Map           -> map
[zed_wrapper-3] 1632304420.083891969 [sensors.zed_node] [INFO]  * Odometry          -> odom
[zed_wrapper-3] 1632304420.083974244 [sensors.zed_node] [INFO]  * Base          -> base_link
[zed_wrapper-3] 1632304420.084034726 [sensors.zed_node] [INFO]  * Camera            -> zed2_camera_center
[zed_wrapper-3] 1632304420.084092103 [sensors.zed_node] [INFO]  * Left          -> zed2_left_camera_frame
[zed_wrapper-3] 1632304420.084146505 [sensors.zed_node] [INFO]  * Left Optical      -> zed2_left_camera_optical_frame
[zed_wrapper-3] 1632304420.084206955 [sensors.zed_node] [INFO]  * RGB           -> zed2_left_camera_frame
[zed_wrapper-3] 1632304420.084285133 [sensors.zed_node] [INFO]  * RGB Optical       -> zed2_left_camera_frame
[zed_wrapper-3] 1632304420.084370192 [sensors.zed_node] [INFO]  * Right         -> zed2_right_camera_frame
[zed_wrapper-3] 1632304420.084450706 [sensors.zed_node] [INFO]  * Right Optical     -> zed2_right_camera_optical_frame
[zed_wrapper-3] 1632304420.084524948 [sensors.zed_node] [INFO]  * Depth         -> zed2_left_camera_frame
[zed_wrapper-3] 1632304420.084582550 [sensors.zed_node] [INFO]  * Depth Optical     -> zed2_left_camera_optical_frame
[zed_wrapper-3] 1632304420.084637304 [sensors.zed_node] [INFO]  * Point Cloud       -> zed2_left_camera_optical_frame
[zed_wrapper-3] 1632304420.084691161 [sensors.zed_node] [INFO]  * Disparity     -> zed2_left_camera_frame
[zed_wrapper-3] 1632304420.084745499 [sensors.zed_node] [INFO]  * Disparity Optical -> zed2_left_camera_optical_frame
[zed_wrapper-3] 1632304420.084821949 [sensors.zed_node] [INFO]  * Confidence        -> zed2_left_camera_frame
[zed_wrapper-3] 1632304420.084881727 [sensors.zed_node] [INFO]  * Confidence Optical    -> zed2_left_camera_optical_frame
[zed_wrapper-3] 1632304420.084936961 [sensors.zed_node] [INFO]  * IMU           -> zed2_imu_link
[zed_wrapper-3] 1632304420.084991234 [sensors.zed_node] [INFO]  * Barometer     -> zed2_camera_center
[zed_wrapper-3] 1632304420.085057316 [sensors.zed_node] [INFO]  * Magnetometer      -> zed2_imu_link
[zed_wrapper-3] 1632304420.085113670 [sensors.zed_node] [INFO]  * Left Temperature      -> zed2_left_camera_frame
[zed_wrapper-3] 1632304420.085168359 [sensors.zed_node] [INFO]  * Right Temperature -> zed2_right_camera_frame
[zed_wrapper-3] 1632304420.085269034 [sensors.zed_node] [INFO] *** PUBLISHED TOPICS ***
[zed_wrapper-3] 1632304420.097426260 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/rgb/image_rect_color
[zed_wrapper-3] 1632304420.100676533 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/rgb/image_rect_gray
[zed_wrapper-3] 1632304420.100860155 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/rgb/camera_info
[zed_wrapper-3] 1632304420.103807154 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/rgb_raw/image_raw_color
[zed_wrapper-3] 1632304420.106584293 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/rgb_raw/image_raw_gray
[zed_wrapper-3] 1632304420.106814924 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/rgb_raw/camera_info
[zed_wrapper-3] 1632304420.110523546 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/left/image_rect_color
[zed_wrapper-3] 1632304420.113522772 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/left/image_rect_gray
[zed_wrapper-3] 1632304420.113672184 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/left/camera_info
[zed_wrapper-3] 1632304420.116469228 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/left_raw/image_raw_color
[zed_wrapper-3] 1632304420.119097498 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/left_raw/image_raw_gray
[zed_wrapper-3] 1632304420.119251326 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/left_raw/camera_info
[zed_wrapper-3] 1632304420.121708488 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/right/image_rect_color
[zed_wrapper-3] 1632304420.124350326 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/right/image_rect_gray
[zed_wrapper-3] 1632304420.124508283 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/right/camera_info
[zed_wrapper-3] 1632304420.127130313 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/right_raw/image_raw_color
[zed_wrapper-3] 1632304420.130021055 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/right_raw/image_raw_gray
[zed_wrapper-3] 1632304420.130171876 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/right_raw/camera_info
[zed_wrapper-3] 1632304420.132841875 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/depth/depth_registered
[zed_wrapper-3] 1632304420.134124793 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/depth/camera_info
[zed_wrapper-3] 1632304420.135812268 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/stereo/image_rect_color
[zed_wrapper-3] 1632304420.137303800 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/stereo_raw/image_raw_color
[zed_wrapper-3] 1632304420.138592798 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/confidence/confidence_map
[zed_wrapper-3] 1632304420.140780447 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/disparity/disparity_image
[zed_wrapper-3] 1632304420.142287372 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/point_cloud/cloud_registered
[zed_wrapper-3] 1632304420.143838043 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/pose
[zed_wrapper-3] 1632304420.145257797 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/pose_with_covariance
[zed_wrapper-3] 1632304420.147661068 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/odom
[zed_wrapper-3] 1632304420.149159225 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/path_map
[zed_wrapper-3] 1632304420.150368637 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/path_odom
[zed_wrapper-3] 1632304420.151871690 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/imu/data
[zed_wrapper-3] 1632304420.153112239 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/imu/data_raw
[zed_wrapper-3] 1632304420.154259377 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/temperature/imu
[zed_wrapper-3] 1632304420.155523767 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/imu/mag
[zed_wrapper-3] 1632304420.156718138 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/atm_press
[zed_wrapper-3] 1632304420.157728600 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/temperature/left
[zed_wrapper-3] 1632304420.158688117 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/temperature/right
[zed_wrapper-3] 1632304420.160155328 [sensors.zed_node] [INFO] Advertised on topic: /sensors/zed_node/left_cam_imu_transform [LATCHED]
[zed_wrapper-3] 1632304420.160262404 [sensors.zed_node] [INFO] Camera-IMU Translation: 
[zed_wrapper-3]  -0.002 -0.023 -0.002
[zed_wrapper-3] 1632304420.161459623 [sensors.zed_node] [INFO] Camera-IMU Rotation: 
[zed_wrapper-3]  7FE70BCB08
[zed_wrapper-3] 0.999988 0.004578 -0.001661
[zed_wrapper-3] -0.004582 0.999987 -0.002388
[zed_wrapper-3] 0.001650 0.002396 0.999996
[zed_wrapper-3] 
[zed_wrapper-3] 1632304420.161953302 [sensors.zed_node] [INFO] Published static TF: 'zed2_imu_link' -> 'zed2_left_camera_frame'
[zed_wrapper-3] 1632304420.215573011 [sensors.zed_node] [INFO] *** Starting Positional Tracking ***
[zed_wrapper-3] 1632304420.215856123 [sensors.zed_node] [INFO]  * Waiting for valid static transformations...
[zed_wrapper-3] 1632304420.216848473 [sensors.zed_node] [INFO] Static transform Sensor to Base [zed2_left_camera_frame -> base_link]
[zed_wrapper-3] 1632304420.216958172 [sensors.zed_node] [INFO]  * Translation: {-0.075,0.055,-0.175}
[zed_wrapper-3] 1632304420.217029630 [sensors.zed_node] [INFO]  * Rotation: {0.000,-0.000,0.000}
[zed_wrapper-3] 1632304420.217420394 [sensors.zed_node] [INFO] Static transform Sensor to Camera Center [zed2_left_camera_frame -> zed2_camera_center]
[zed_wrapper-3] 1632304420.217490860 [sensors.zed_node] [INFO]  * Translation: {0.000,-0.060,0.000}
[zed_wrapper-3] 1632304420.217550126 [sensors.zed_node] [INFO]  * Rotation: {0.000,-0.000,0.000}
[zed_wrapper-3] 1632304420.217656977 [sensors.zed_node] [INFO] Static transform Camera Center to Base [zed2_camera_center -> base_link]
[zed_wrapper-3] 1632304420.217749268 [sensors.zed_node] [INFO]  * Translation: {-0.075,0.115,-0.175}
[zed_wrapper-3] 1632304420.217862935 [sensors.zed_node] [INFO]  * Rotation: {0.000,-0.000,0.000}
[zed_wrapper-3] 1632304420.318056735 [sensors.zed_node] [INFO] Initial ZED left camera pose (ZED pos. tracking): 
[zed_wrapper-3] 1632304420.319524874 [sensors.zed_node] [INFO]  * T: [0.075,-0.0545,0.175]
[zed_wrapper-3] 1632304420.319671023 [sensors.zed_node] [INFO]  * Q: [0,0,0,1]
[zed_wrapper-3] 1632304420.842024146 [sensors.zed_node] [INFO] Odometry aligned to last tracking pose
[...]
  3. Run ros2 topic hz /sensors/zed_node/depth/depth_registered. Observe lack of output.
  4. Run ros2 topic hz /sensors/zed_node/rgb/image_rect_color. Revel at the correct amount of output.

Expected Result

I expect that any advertised topic produce data. In particular, that stereo, depth, rectified and associated camera infos are published.

Actual Result

Some topics produce data, but most do not.

ZED Camera model

ZED2

Environment

OS: Ubuntu 18.04, from docker image above
CPU: ARM
GPU: Nvidia Jetson AGX Xavier
Other info: ROS2, Foxy. Many packages are built before Zed is built, but they have been left out for brevity. Do tell me if you want a full list.

Anything else?

For the record, running ros2 launch zed_wrapper zed2.launch.py gives the same behavior.

Myzhar commented 3 years ago

Hi @peredwardsson I just tried ros2 launch zed_wrapper zed2.launch.py as double check and it works as expected. Are you starting it inside the container? Can you run the command ros2 doctor while the node is running to see if there is something wrong in your configuration?

peredwardsson commented 3 years ago

Yes, running it inside of container. I believe it has worked previously, so I also suspect some configuration issue. ros2 doctor output:

# ros2 doctor
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/__init__.py: 108: UserWarning: Fail to call PackageCheck class functions.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /actuators/esc/motor_current_axis0.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /actuators/esc/motor_current_axis1.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /actuators/esc/odom_odrive.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /actuators/esc/vbus_voltage.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /actuators/esc/vel_estimate.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 56: UserWarning: Subscriber without publisher detected on /actuators/odom_odrive.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /cloud.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /cloud_ground.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /cloud_map.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /cloud_obstacles.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 56: UserWarning: Subscriber without publisher detected on /cmd_vel.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /diagnostics.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /global_path.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /global_path_nodes.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 56: UserWarning: Subscriber without publisher detected on /global_pose.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 56: UserWarning: Subscriber without publisher detected on /goal.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 56: UserWarning: Subscriber without publisher detected on /goal_node.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /goal_out.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /goal_reached.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 56: UserWarning: Subscriber without publisher detected on /gps/fix.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /grid_prob_map.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 56: UserWarning: Subscriber without publisher detected on /imu.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /info.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 56: UserWarning: Subscriber without publisher detected on /initialpose.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /labels.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /landmarks.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /local_grid_empty.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /local_grid_ground.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /local_grid_obstacle.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /local_path.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /local_path_nodes.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 56: UserWarning: Subscriber without publisher detected on /localization/joint_states.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /localization/odometry/filtered.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /localization/robot_description.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 56: UserWarning: Subscriber without publisher detected on /localization/set_pose.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /localization_pose.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /map.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /mapData.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /mapGraph.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /mapPath.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /octomap_empty_space.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /octomap_global_frontier_space.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /octomap_grid.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /octomap_ground.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /octomap_obstacles.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /octomap_occupied_space.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /rgbd_image/compressed.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/rtabmap/depth.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/atm_press.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/confidence/confidence_map.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/imu/data.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/imu/data_raw.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/imu/mag.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/left/camera_info.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/left/image_rect_color.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/left/image_rect_gray.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/left_cam_imu_transform.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/left_raw/camera_info.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/left_raw/image_raw_color.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/left_raw/image_raw_gray.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/path_map.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/path_odom.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/point_cloud/cloud_registered.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/pose.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/pose_with_covariance.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/rgb/image_rect_color.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/rgb/image_rect_gray.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/rgb_raw/camera_info.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/rgb_raw/image_raw_color.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/rgb_raw/image_raw_gray.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/right/camera_info.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/right/image_rect_color.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/right/image_rect_gray.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/right_raw/camera_info.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/right_raw/image_raw_color.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/right_raw/image_raw_gray.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/stereo_raw/image_raw_color.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/temperature/imu.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/temperature/left.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 53: UserWarning: Publisher without subscriber detected on /sensors/zed_node/temperature/right.
/opt/ros/foxy/install/lib/python3.6/site-packages/ros2doctor/api/topic.py: 56: UserWarning: Subscriber without publisher detected on /user_data_async.

All 3 checks passed
Myzhar commented 3 years ago

It's strange that the depth topic is not present in the warning list. Can you please post the output of the command $ ros2 topic list?

peredwardsson commented 3 years ago

It was probably an issue with image_transport which is used to create some of the publishers. Observing the source code, I noticed that topics that had publishers created with that package's publisher creation functions. Reinstalling image_transport seems to have fixed it.

Myzhar commented 3 years ago

image_transport is a problem until they do not add the fix in the official release