isaac_ros_visual_slam on jetson orin nano - ashBabu/Utilities GitHub Wiki

ARG BASE_IMAGE
FROM ${BASE_IMAGE}

RUN apt-get update && apt-get install -y \
    ros-humble-isaac-ros-visual-slam \
    ros-humble-isaac-ros-nvblox \
    ros-humble-nvblox-msgs && \
    rm -rf /var/lib/apt/lists/*

  Color        320x180       RGB8        @ 60/30/6 Hz
    Color           |          Y8          @ 60/30/6 Hz
    Color           |          BGRA8       @ 60/30/6 Hz
    Color           |          RGBA8       @ 60/30/6 Hz
    Color           |          BGR8        @ 60/30/6 Hz
    Color           |          YUYV        @ 60/30/6 Hz

Stream Profiles supported by Motion Module
 Supported modes:
    STREAM      FORMAT         FPS
    Accel       MOTION_XYZ32F  @ 250/63 Hz
    Gyro        MOTION_XYZ32F  @ 400/200 Hz

from isaac_ros_launch_utils.all_types import *
import isaac_ros_launch_utils as lu

from nvblox_ros_python_utils.nvblox_launch_utils import NvbloxMode, NvbloxCamera, NvbloxPeopleSegmentation
from nvblox_ros_python_utils.nvblox_constants import NVBLOX_CONTAINER_NAME


def generate_launch_description() -> LaunchDescription:
    args = lu.ArgumentContainer()
    args.add_arg(
        'rosbag', 'None', description='Path to rosbag (running on sensor if not set).', cli=True)
    args.add_arg('rosbag_args', '',
                 description='Additional args for ros2 bag play.', cli=True)
    args.add_arg('log_level', 'info', choices=[
                 'debug', 'info', 'warn'], cli=True)
    args.add_arg('num_cameras', 1,
                 description='How many cameras to use.', cli=True)
    args.add_arg('camera_serial_numbers', '',
                 description='List of the serial no of the extra cameras. (comma separated)',
                 cli=True)
    args.add_arg(
        'multicam_urdf_path',
        lu.get_path('nvblox_examples_bringup',
                    'config/urdf/4_realsense_carter_example_calibration.urdf.xacro'),
        description='Path to a URDF file describing the camera rig extrinsics. Only used in multicam.',
        cli=True)
    args.add_arg(
        'mode',
        default=NvbloxMode.static,
        choices=NvbloxMode.names(),
        description='The nvblox mode.',
        cli=True)
    args.add_arg(
        'people_segmentation',
        default=NvbloxPeopleSegmentation.peoplesemsegnet_vanilla,
        choices=[
            str(NvbloxPeopleSegmentation.peoplesemsegnet_vanilla),
            str(NvbloxPeopleSegmentation.peoplesemsegnet_shuffleseg)
        ],
        description='The  model type of PeopleSemSegNet (only used when mode:=people_segmentation).',
        cli=True)
    args.add_arg(
        'attach_to_container',
        'False',
        description='Add components to an existing component container.',
        cli=True)
    args.add_arg(
        'container_name',
        NVBLOX_CONTAINER_NAME,
        description='Name of the component container.')
    args.add_arg(
        'run_realsense',
        'True',
        description='Launch Realsense drivers')
    args.add_arg(
        'use_foxglove_whitelist',
        True,
        description='Disable visualization of bandwidth-heavy topics',
        cli=True)
    actions = args.get_launch_actions()

    # Globally set use_sim_time if we're running from bag or sim
    actions.append(
        SetParameter('use_sim_time', True, condition=IfCondition(lu.is_valid(args.rosbag))))

    # Single or Multi-realsense
    is_multi_cam = UnlessCondition(lu.is_equal(args.num_cameras, '1'))
    camera_mode = lu.if_else_substitution(
        lu.is_equal(args.num_cameras, '1'),
        str(NvbloxCamera.realsense),
        str(NvbloxCamera.multi_realsense)
    )
    # Only up to 4 Realsenses is supported.
    actions.append(
        lu.assert_condition(
            'Up to 4 cameras have been tested! num_cameras must be less than 5.',
            IfCondition(PythonExpression(['int("', args.num_cameras, '") > 4']))),
    )

    run_rs_driver = UnlessCondition(
        OrSubstitution(lu.is_valid(args.rosbag), lu.is_false(args.run_realsense)))
    # Realsense
    actions.append(
        lu.include(
            'realsense_splitter',
            'launch/realsense.launch.py',
            launch_arguments={
                'container_name': args.container_name,
                'camera_serial_numbers': args.camera_serial_numbers,
                'num_cameras': args.num_cameras,
            },
            condition=run_rs_driver))

    # Visual SLAM
    actions.append(
        lu.include(
            'nvblox_examples_bringup',
            'launch/perception/vslam.launch.py',
            launch_arguments={
                'container_name': args.container_name,
                'camera': camera_mode,
            },
            # Delay for 1 second to make sure that the static topics from the rosbag are published.
            delay=1.0,
        ))
    # People detection for multi-RS
    camera_namespaces = ['camera0', 'camera1', 'camera2', 'camera3']
    camera_input_topics = []
    input_camera_info_topics= []
    output_resized_image_topics = []
    output_resized_camera_info_topics = []
    for ns in camera_namespaces:
        camera_input_topics.append(f'/{ns}/color/image_raw')
        input_camera_info_topics.append(f'/{ns}/color/camera_info')
        output_resized_image_topics.append(f'/{ns}/segmentation/image_resized')
        output_resized_camera_info_topics.append(f'/{ns}/segmentation/camera_info_resized')

    # People segmentation
    actions.append(
        lu.include(
            'nvblox_examples_bringup',
            'launch/perception/segmentation.launch.py',
            launch_arguments={
                'container_name': args.container_name,
                'people_segmentation': args.people_segmentation,
                'namespace_list': camera_namespaces,
                'input_topic_list': camera_input_topics,
                'input_camera_info_topic_list': input_camera_info_topics,
                'output_resized_image_topic_list': output_resized_image_topics,
                'output_resized_camera_info_topic_list': output_resized_camera_info_topics,
                'num_cameras': args.num_cameras,
                # fixing rosbag replay dropping fps
                'one_container_per_camera': True
            },
            condition=IfCondition(lu.has_substring(args.mode, NvbloxMode.people_segmentation))))

    # People detection
    actions.append(
        lu.include(
            'nvblox_examples_bringup',
            'launch/perception/detection.launch.py',
            launch_arguments={
                'namespace_list': camera_namespaces,
                'input_topic_list': camera_input_topics,
                'num_cameras': args.num_cameras,
                'container_name': args.container_name,
                # fixing rosbag replay dropping fps
                'one_container_per_camera': True
            },
            condition=IfCondition(lu.has_substring(args.mode, NvbloxMode.people_detection))))

    # Nvblox
    actions.append(
        lu.include(
            'nvblox_examples_bringup',
            'launch/perception/nvblox.launch.py',
            launch_arguments={
                'container_name': args.container_name,
                'mode': args.mode,
                'camera': camera_mode,
                'num_cameras': args.num_cameras,
            }))

    # TF transforms for multi-realsense
    actions.append(
        lu.add_robot_description(robot_calibration_path=args.multicam_urdf_path,
                                 condition=is_multi_cam)
    )

    # Play ros2bag
    actions.append(
        lu.play_rosbag(
            bag_path=args.rosbag,
            additional_bag_play_args=args.rosbag_args,
            condition=IfCondition(lu.is_valid(args.rosbag))))

    # Visualization
    actions.append(
        lu.include(
            'nvblox_examples_bringup',
            'launch/visualization/visualization.launch.py',
            launch_arguments={
                'mode': args.mode,
                'camera': camera_mode,
                'use_foxglove_whitelist': args.use_foxglove_whitelist,
            }))

    # Container
    # NOTE: By default (attach_to_container:=False) we launch a container which all nodes are
    # added to, however, we expose the option to not launch a container, and instead attach to
    # an already running container. The reason for this is that when running live on multiple
    # realsenses we have experienced unreliability in the bringup of multiple realsense drivers.
    # To (partially) mitigate this issue the suggested workflow for multi-realsenses is to:
    # 1. Launch RS (cameras & splitter) and start a component_container
    # 2. Launch nvblox + cuvslam and attached to the above running component container

    actions.append(
        lu.component_container(
            NVBLOX_CONTAINER_NAME, condition=UnlessCondition(args.attach_to_container),
            log_level=args.log_level))

    return LaunchDescription(actions)

from typing import List, Optional

from isaac_ros_launch_utils.all_types import (
    Action, ComposableNode, LaunchDescription, TimerAction, IfCondition)
import isaac_ros_launch_utils as lu
from nvblox_ros_python_utils.nvblox_constants import NVBLOX_CONTAINER_NAME

EMITTER_FLASHING_CONFIG_FILE_PATH = lu.get_path(
    'nvblox_examples_bringup',
    'config/sensors/realsense_emitter_flashing.yaml')
EMITTER_ON_CONFIG_FILE_PATH = lu.get_path(
    'nvblox_examples_bringup',
    'config/sensors/realsense_emitter_on.yaml')

# By default our behaviour is:
# - Run the splitter on camera0,
# - Don't run the splitter on the remaining cameras.
# NOTE(alexmillane, 16.08.2024): At the moment this is the *only* behaviour we support.


def get_default_run_splitter_list(num_cameras: int) -> List[bool]:
    run_splitter_list = [False] * num_cameras
    run_splitter_list[0] = True
    return run_splitter_list


def get_camera_node(
        camera_name: str,
        config_file_path: str,
        serial_number: Optional[int] = None) -> ComposableNode:
    parameters = []
    parameters.append(config_file_path)
    parameters.append({'camera_name': camera_name})
    if serial_number:
        parameters.append({'serial_no': str(serial_number)})
    realsense_node = ComposableNode(
        name=camera_name,
        namespace='',
        package='realsense2_camera',
        plugin='realsense2_camera::RealSenseNodeFactory',
        parameters=parameters)
    return realsense_node


def get_splitter_node(camera_name: str) -> ComposableNode:
    realsense_splitter_node = ComposableNode(
        namespace=camera_name,
        name='realsense_splitter_node',
        package='realsense_splitter',
        plugin='nvblox::RealsenseSplitterNode',
        parameters=[{
            'input_qos': 'SENSOR_DATA',
            'output_qos': 'SENSOR_DATA'
        }],
        remappings=[
            ('input/infra_1', f'/{camera_name}/infra1/image_rect_raw'),
            ('input/infra_1_metadata', f'/{camera_name}/infra1/metadata'),
            ('input/infra_2', f'/{camera_name}/infra2/image_rect_raw'),
            ('input/infra_2_metadata', f'/{camera_name}/infra2/metadata'),
            ('input/depth', f'/{camera_name}/depth/image_rect_raw'),
            ('input/depth_metadata', f'/{camera_name}/depth/metadata'),
            ('input/pointcloud', f'/{camera_name}/depth/color/points'),
            ('input/pointcloud_metadata', f'/{camera_name}/depth/metadata'),
        ])
    return realsense_splitter_node


def add_cameras(args: lu.ArgumentContainer) -> List[Action]:
    """Adds a camera and (optional) realsense splitter for each camera up to num_cameras."""

    # Serial numbers.
    if args.camera_serial_numbers == '':
        camera_serial_numbers = [None]
    else:
        camera_serial_numbers = str(args.camera_serial_numbers).split(',')
    assert len(camera_serial_numbers) > 0
    # Run splitter list. I.e. a list of bools indicating per-camera if we should run a splitter.
    run_splitter_list = get_default_run_splitter_list(len(camera_serial_numbers))
    assert len(camera_serial_numbers) == len(run_splitter_list)
    # Number of cameras to run
    num_cameras = int(args.num_cameras)
    assert num_cameras <= len(camera_serial_numbers)

    actions = []
    for idx in range(num_cameras):
        camera_serial_number = camera_serial_numbers[idx]
        run_splitter = run_splitter_list[idx]
        nodes = []
        camera_name = f'camera{idx}'
        # Config file
        if run_splitter:
            config_file_path = EMITTER_FLASHING_CONFIG_FILE_PATH
        else:
            config_file_path = EMITTER_ON_CONFIG_FILE_PATH
        # Realsense
        log_message = lu.log_info(
            f'Starting realsense with name: {camera_name}, running splitter: {run_splitter}')
        nodes.append(
            get_camera_node(
                camera_name=camera_name,
                config_file_path=config_file_path,
                serial_number=camera_serial_number,
            ))
        # Splitter
        if run_splitter:
            nodes.append(
                get_splitter_node(
                    camera_name=camera_name,
                ))
        # Note(xinjieyao: 2024/08/24): Multi-rs launch use RealSenseNodeFactory could be unstable
        # Camera node may fail to launch without any ERROR or app crashes
        # Adding delay for cameras after the first camera bringup (including splitter) as temp fix
        actions.append(
            TimerAction(
                period=idx * 10.0, actions=[lu.load_composable_nodes(args.container_name, nodes)]))
        actions.append(log_message)

    return actions


def generate_launch_description() -> LaunchDescription:
    args = lu.ArgumentContainer()
    args.add_arg('container_name', NVBLOX_CONTAINER_NAME)
    args.add_arg('run_standalone', 'False')
    args.add_arg('camera_serial_numbers', '')
    args.add_arg('num_cameras', 1)

    # Adding the cameras
    args.add_opaque_function(add_cameras)
    actions = args.get_launch_actions()
    actions.append(
        lu.component_container(
            args.container_name, condition=IfCondition(lu.is_true(args.run_standalone))))
    return LaunchDescription(actions)

/swapfile  file       15G     0B   -2
/dev/zram0 partition 634M 221.9M    5
/dev/zram1 partition 634M 227.5M    5
/dev/zram2 partition 634M 231.1M    5
/dev/zram3 partition 634M 227.7M    5
/dev/zram4 partition 634M 221.8M    5
/dev/zram5 partition 634M 219.6M    5