isaac_ros_data_replayer#

Source code available on GitHub.

Overview#

The isaac_ros_data_replayer package enables replaying sensor data recorded using the isaac_ros_data_recorder package. This package also enables reliable extraction of stereo cameras in different formats, including depth using AI-based perception. Supported formats include MP4, PNG, and PFM. Stereo camera frames are extracted in synchronized pairs while depth frames are aligned to the left stereo imager.

Quickstart#

Set Up Development Environment#

  1. Set up your development environment by following the instructions in getting started.

  2. (Optional) Install dependencies for any sensors you want to use by following the sensor-specific guides.

    Note

    We strongly recommend installing all sensor dependencies before starting any quickstarts. Some sensor dependencies require restarting the development environment during installation, which will interrupt the quickstart process.

Download Quickstart Assets#

  1. Download quickstart data from NGC:

    NVIDIA Internal:

    The NGC CLI must be set up once per system.

    1. Install the NGC CLI from here.

      Note

      Jetson Only: Do not edit the ~/.bash_profile file as suggested by the official NGC CLI instructions.

      Instead, add the NGC CLI to your PATH through ~/.bashrc:

      s="export PATH=\"\$PATH:$(pwd)/ngc-cli\""
      f="$HOME/.bashrc"; grep -qxF "$s" $f || echo "$s" | tee -a $f && source $f
      
    2. Generate an API key here.

    3. Run ngc config set:

      When prompted, enter the API key you generated earlier.

      ngc config set
      
    4. To confirm that the NGC CLI is set up correctly, run the following command:

      grep -E "^apikey\s*=\s*" ~/.ngc/config
      

      You should see an output with a valid API key:

      apikey = <your_api_key>
      

    Make sure required libraries are installed.

    sudo apt-get install -y curl jq tar
    

    Then, run these commands to download the asset from NGC:

    NGC_ORG="nvstaging"
    NGC_TEAM="isaac"
    PACKAGE_NAME="isaac_ros_data_replayer"
    NGC_RESOURCE="isaac_ros_data_replayer_assets"
    NGC_FILENAME="quickstart.tar.gz"
    MAJOR_VERSION=3
    MINOR_VERSION=3
    NGC_CONFIG_FILE="$HOME/.ngc/config"
    API_KEY=$(grep -E "^apikey\s*=\s*" $NGC_CONFIG_FILE | \
        awk -F "=" '{print $2}' | tr -d '[:space:]')
    TOKEN=$(curl -s -u "\$oauthtoken":"$API_KEY" -H 'Accept:application/json' \
        "https://authn.nvidia.com/token?service=ngc"\
    "&scope=group/ngc:$NGC_ORG&group/ngc:$NGC_ORG/$NGC_TEAM" | \
        jq -r '.token')
    VERSION_REQ_URL="https://api.ngc.nvidia.com/v2/org/$NGC_ORG/team/$NGC_TEAM/resources/$NGC_RESOURCE/versions"
    AVAILABLE_VERSIONS=$(curl -s -H "Authorization: Bearer ${TOKEN}" \
        -H "Accept: application/json" "$VERSION_REQ_URL")
    LATEST_VERSION_ID=$(echo $AVAILABLE_VERSIONS | jq -r "
        .recipeVersions[]
        | .versionId as \$v
        | \$v | select(test(\"^\\\\d+\\\\.\\\\d+\\\\.\\\\d+$\"))
        | split(\".\") | {major: .[0]|tonumber, minor: .[1]|tonumber, patch: .[2]|tonumber}
        | select(.major == $MAJOR_VERSION and .minor <= $MINOR_VERSION)
        | \$v
        " | sort -V | tail -n 1
    )
    if [ -z "$LATEST_VERSION_ID" ]; then
        echo "No corresponding version found for Isaac ROS $MAJOR_VERSION.$MINOR_VERSION"
        echo "Found versions:"
        echo $AVAILABLE_VERSIONS | jq -r '.recipeVersions[].versionId'
    else
        mkdir -p ${ISAAC_ROS_WS}/isaac_ros_assets && \
        FILE_REQ_URL="https://api.ngc.nvidia.com/v2/org/$NGC_ORG/team/$NGC_TEAM/resources/\
    $NGC_RESOURCE/versions/$LATEST_VERSION_ID/files/$NGC_FILENAME" && \
        curl -LO --request GET "${FILE_REQ_URL}" \
            -H "Authorization: Bearer ${TOKEN}" \
            -H "Content-Type: application/json" && \
        tar -xf ${NGC_FILENAME} -C ${ISAAC_ROS_WS}/isaac_ros_assets && \
        rm ${NGC_FILENAME}
    fi
    

Build isaac_ros_data_replayer#

  1. Launch the Docker container using the run_dev.py script:

    cd ${ISAAC_ROS_WS}/src/isaac_ros_common && \
      ./scripts/run_dev.py
    
  2. Install the prebuilt Debian package:

    NVIDIA Internal: Run these commands to add the internal apt repository:

    sudo apt install curl -y
    k="/usr/share/keyrings/nvidia-isaac-ros.gpg"
    curl -fsSL https://isaac.download.nvidia.com/isaac-ros/repos.key | sudo gpg --dearmor | sudo tee -a $k > /dev/null
    f="/etc/apt/sources.list.d/nvidia-isaac-ros.list" && sudo touch $f
    s="deb [signed-by=$k] https://urm.nvidia.com/artifactory/sw-isaac-staging-debian-local jammy release-3.3"
    grep -qxF "$s" $f || echo "$s" | sudo tee -a $f
    
    pin_content=$'package: *\nPin: origin isaac.download.nvidia.com\nPin-Priority: 400'
    echo "$pin_content" | sudo tee /etc/apt/preferences.d/isaac-ros
    
    sudo apt-get update
    
    sudo apt-get install -y ros-humble-isaac-ros-data-replayer
    source /opt/ros/humble/setup.bash
    
  3. Install the required assets:

    sudo apt-get install -y ros-humble-isaac-ros-ess-models-install
    source /opt/ros/humble/setup.bash
    
    ros2 run isaac_ros_ess_models_install install_ess_models.sh --eula
    

Run Launch File#

ros2 launch isaac_ros_data_replayer data_replayer.launch.py \
   rosbag:=${ISAAC_ROS_WS}/isaac_ros_assets/isaac_ros_data_replayer/quickstart

Replay a single camera#

ros2 launch isaac_ros_data_replayer data_replayer.launch.py \
   rosbag:=${ISAAC_ROS_WS}/isaac_ros_assets/isaac_ros_data_replayer/quickstart \
   enabled_stereo_cameras:=front_stereo_camera enabled_fisheye_cameras:=None enable_3d_lidar:=False

Visualize#

  1. Open a new terminal inside the Docker container:

    cd ${ISAAC_ROS_WS}/src/isaac_ros_common && \
       ./scripts/run_dev.py
    
  2. Run RViz to visualize data:

    rviz2
    
    data_replayer_rviz

Data Extraction#

To extract synchronized camera frames from a rosbag, run:

ros2 run isaac_ros_data_replayer data_extraction.py \
   --rosbag ${ISAAC_ROS_WS}/isaac_ros_assets/isaac_ros_data_replayer/quickstart \
   --camera front_stereo_camera --output ${ISAAC_ROS_WS} \
   --engine_file_path ${ISAAC_ROS_WS}/isaac_ros_assets/models/dnn_stereo_disparity/dnn_stereo_disparity_v4.1.0_onnx/ess.engine

By default, rectified camera frames will be extracted alongside disparity and depth images generated by ESS. To extract rectified images without depth or disparity, run:

ros2 run isaac_ros_data_replayer data_extraction.py \
   --rosbag ${ISAAC_ROS_WS}/isaac_ros_assets/isaac_ros_data_replayer/quickstart \
   --camera front_stereo_camera --output ${ISAAC_ROS_WS} \
   --mode rectify

To extract raw images, run:

ros2 run isaac_ros_data_replayer data_extraction.py \
   --rosbag ${ISAAC_ROS_WS}/isaac_ros_assets/isaac_ros_data_replayer/quickstart \
   --camera front_stereo_camera --output ${ISAAC_ROS_WS} \
   --mode raw

Rosbag Conversion#

The isaac_ros_data_recorder records camera streams as sensor_msgs/msg/CompressedImage messages encoded with H.264 encoding, and 3D LIDAR data as hesai_ros_driver/msg/UdpFrame.

The rosbag_converter tool converts rosbags from isaac_ros_data_recorder so that camera streams are stored uncompressed as sensor_msgs/msg/Image messages and 3D LIDAR data as sensor_msgs/msg/PointCloud2 messages (with fields x, y, z, intensity, ring, timestamp). For additional details refer to isaac_ros_hesai documentation.

ros2 run isaac_ros_data_replayer rosbag_converter.py \
   -i ${ISAAC_ROS_WS}/isaac_ros_assets/isaac_ros_data_replayer/quickstart \
   -o ${ISAAC_ROS_WS}/uncompressed_rosbag/ \
   --all

Note

Disk space requirements: The rosbag_converter tool requires free disk space of at least 100 times the size of the original rosbag. Specific requirements may vary and depend on the number of sensors recorded in the original rosbag, among other things.

Note

The rosbag_converter tool is expected to be used on x86_64, with sufficient disk space under the /tmp directory. The original rosbag needs to be copied to the x86_64 machine where the conversion will be performed.

Troubleshooting#

Isaac ROS Troubleshooting#

For solutions to problems with Isaac ROS, see troubleshooting.

Not all camera frames extracted from recording#

Data extraction extracts synchronized camera frames from a recording. If the number of frames from the left and right camera are not equal, then some frames will be ignored to keep the left and right cameras in sync.

Not enough free disk space to run the rosbag_converter tool#

Symptom#

Execution of the rosbag_converter tool crashes with a python error, including but not limited to:

RuntimeError: Exception on parsing info file: invalid node; first invalid key: "version"
[ros2run]: Process exited with failure 1

Solution#

Free additional disk space before executing the rosbag_converter tool.

Alternatively, consider reducing the size of the input rosbag, or consider converting only the subset of topics needed for your application. Note that the argument -a or --all converts all the possible topics in the input rosbag, but the arguments --topics_decode_h264 and --topics_udp_to_pc2 allow for the conversion of specific topics.

Make sure to use the tool on a x86_64 machine.

API#

Usage#

ros2 launch isaac_ros_data_replayer data_replayer.launch.py rosbag:=<rosbag>

ROS Launch Arguments#

ROS Launch Argument

Default Value

Description

rosbag

None

Path to recording.

enabled_stereo_cameras

front_stereo_camera back_stereo_camera left_stereo_camera right_stereo_camera

Stereo cameras enabled for replay.

enabled_fisheye_cameras

front_fisheye_camera back_fisheye_camera left_fisheye_camera right_fisheye_camera

Fisheye cameras enabled for replay.

enable_3d_lidar

True

Enable 3D lidar for replay.

replay_loop

False

Loop replay.

replay_rate

1.0

Extraction rate; 1.0 for real-time, <1.0 for sub-real-time, >1.0 for super-real-time.

replay_delay

None

Delay to start replay.

replay_additional_args

--disable-keyboard-controls

Additional arguments for replay.

ROS Topics Published#

ROS Topic

Interface

Description

/rosout

rcl_interfaces/msg/Log

Console logs.

/tf

tf2_msgs/msg/TFMessage

Movable transforms on the robot.

/tf_static

tf2_msgs/msg/TFMessage

Fixed transforms on the robot.

/front_stereo_camera/left/image_raw

sensor_msgs/msg/Image

Front stereo camera left camera stream.

/front_stereo_camera/left/camera_info

sensor_msgs/msg/CameraInfo

Front stereo camera left camera intrinsics.

/front_stereo_camera/right/image_raw

sensor_msgs/msg/Image

Front stereo camera right camera stream.

/front_stereo_camera/right/camera_info

sensor_msgs/msg/CameraInfo

Front stereo camera right camera intrinsics.

/back_stereo_camera/left/image_raw

sensor_msgs/msg/Image

Back stereo camera left camera stream.

/back_stereo_camera/left/camera_info

sensor_msgs/msg/CameraInfo

Back stereo camera left camera intrinsics.

/back_stereo_camera/right/image_raw

sensor_msgs/msg/Image

Back stereo camera right camera stream.

/back_stereo_camera/right/camera_info

sensor_msgs/msg/CameraInfo

Back stereo camera right camera intrinsics.

/left_stereo_camera/left/image_raw

sensor_msgs/msg/Image

Left stereo camera left camera stream.

/left_stereo_camera/left/camera_info

sensor_msgs/msg/CameraInfo

Left stereo camera left camera intrinsics.

/left_stereo_camera/right/image_raw

sensor_msgs/msg/Image

Left stereo camera right camera stream.

/left_stereo_camera/right/camera_info

sensor_msgs/msg/CameraInfo

Left stereo camera right camera intrinsics.

/right_stereo_camera/left/image_raw

sensor_msgs/msg/Image

Right stereo camera left camera stream.

/right_stereo_camera/left/camera_info

sensor_msgs/msg/CameraInfo

Right stereo camera left camera intrinsics.

/right_stereo_camera/right/image_raw

sensor_msgs/msg/Image

Right stereo camera right camera stream.

/right_stereo_camera/right/camera_info

sensor_msgs/msg/CameraInfo

Right stereo camera right camera intrinsics.

/front_fisheye_camera/left/image_raw

sensor_msgs/msg/Image

Front fisheye camera stream.

/front_fisheye_camera/left/camera_info

sensor_msgs/msg/CameraInfo

Front fisheye camera intrinsics.

/back_fisheye_camera/left/image_raw

sensor_msgs/msg/Image

Back fisheye camera stream.

/back_fisheye_camera/left/camera_info

sensor_msgs/msg/CameraInfo

Back fisheye camera intrinsics.

/left_fisheye_camera/left/image_raw

sensor_msgs/msg/Image

Left fisheye camera stream.

/left_fisheye_camera/left/camera_info

sensor_msgs/msg/CameraInfo

Left fisheye camera intrinsics.

/right_fisheye_camera/left/image_raw

sensor_msgs/msg/Image

Right fisheye camera stream.

/right_fisheye_camera/left/camera_info

sensor_msgs/msg/CameraInfo

Right fisheye camera intrinsics.

/front_2d_lidar/scan

sensor_msgs/msg/LaserScan

Front 2D lidar scan.

/back_2d_lidar/scan

sensor_msgs/msg/LaserScan

Back 2D lidar scan.

/front_3d_lidar/lidar_points

sensor_msgs/msg/PointCloud2

Front 3D lidar point cloud.

/front_stereo_imu/imu

sensor_msgs/msg/Imu

Front stereo camera inertial measurement unit.

/chassis/imu

sensor_msgs/msg/Imu

Chassis inertial measurement unit.

/chassis/odom

nav_msgs/msg/Odometry

Chassis odometry.

/chassis/battery_state

sensor_msgs/msg/BatteryState

Chassis battery state.

Usage#

ros2 run isaac_ros_data_replayer data_extraction.py --rosbag <rosbag> --camera <camera> --output <output>

ROS Run Arguments#

ROS Run Argument

Default Value

Description

--rosbag

None

Path to recording.

--camera

None

Camera to extract that is, front_stereo_camera.

--output

None

Directory to store extracted data.

--min_disparity

3.0

Minimum disparity value for normalization.

--max_disparity

200.0

Maximum disparity value for normalization.

--threshold

0.0

ESS threshold.

--engine_file_path

isaac_ros_assets/models/dnn_stereo_disparity/dnn_stereo_disparity_v4.1.0_onnx/ess.engine

ESS engine file path.

--mode

depth

Image extraction mode (‘raw’, ‘rectify’, or ‘depth’)

ROS Topics Subscribed#

ROS Topic

Interface

Description

/<camera>/left/image_compressed

sensor_msgs/msg/CompressedImage

Left camera stream.

/<camera>/left/camera_info

sensor_msgs/msg/CameraInfo

Left camera intrinsics.

/<camera>/right/image_compressed

sensor_msgs/msg/CompressedImage

Right camera stream.

/<camera>/right/camera_info

sensor_msgs/msg/CameraInfo

Right camera intrinsics.

Usage#

ros2 run isaac_ros_data_replayer rosbag_converter.py --input_rosbag <input_rosbag> --output_rosbag <output_rosbag> --all

ROS Run Arguments#

ROS Run Argument

Default Value

Description

-i, --input_rosbag

None

Path to the input rosbag (recording from the isaac_ros_data_recorder).

-o, --output_rosbag

None

Path to the output (converted) rosbag.

-a, --all

None

Whether to convert all possible topics in the input rosbag. If provided, it takes priority over other arguments listing topics.

--topics_decode_h264

[]

List of image topics with sensor_msgs/msg/CompressedImage h264 messages to be decoded as sensor_msgs/msg/Image.

--topics_udp_to_pc2

[]

List of LIDAR topics with hesai_ros_driver/msg/UdpFrame packets to be converted to sensor_msgs/msg/PointCloud2.

ROS Topics Subscribed#

ROS Topic

Interface

Description

/<camera>/left/image_compressed

sensor_msgs/msg/CompressedImage

Left camera stream.

/<camera>/left/camera_info

sensor_msgs/msg/CameraInfo

Left camera intrinsics.

/<camera>/right/image_compressed

sensor_msgs/msg/CompressedImage

Right camera stream.

/<camera>/right/camera_info

sensor_msgs/msg/CameraInfo

Right camera intrinsics.

/<lidar>/lidar_packets

hesai_ros_driver/msg/UdpFrame

LIDAR UDP packets.