Isaac ROS Visual Slam is unable to visualize a point cloud from Gazebo simulation camera stream

Robotics & Edge Computing Robotics - Isaac Isaac ROS
1

Hi,

I have been following this isaac ros vslam tutorial. I have successfully been able to visualize the point cloud in RViz using a realsense camera by running ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_realsense.launch.py.

However, when I remap the camera stream from gazebo to visual node, I am unable to replicate the point cloud in RViz. Here is the node bringup:

visual_slam_node = ComposableNode(
        name='visual_slam_node',
        package='isaac_ros_visual_slam',
        plugin='nvidia::isaac_ros::visual_slam::VisualSlamNode',
        parameters=[{
                    'enable_image_denoising': False,
                    'use_sim_time': True,
                    'rectified_images': False,
                    'enable_imu_fusion': True,
                    'gyro_noise_density': 0.000244,
                    'gyro_random_walk': 0.000019393,
                    'accel_noise_density': 0.001862,
                    'accel_random_walk': 0.003,
                    'calibration_frequency': 200.0,
                    'image_jitter_threshold_ms': 22.00,
                    'base_frame': 'realsense_front_link',
                    'imu_frame': 'realsense_front_accel_frame',
                    'enable_slam_visualization': True,
                    'enable_landmarks_view': True,
                    'enable_observations_view': True,
                    'camera_optical_frames': [
                        'realsense_front_infra1_frame',
                        'realsense_front_infra2_frame',
                    ],
                    }],
        remappings=[('visual_slam/image_0', '/front_d435_ired1'),
                    ('visual_slam/camera_info_0', '/front_d435_ired1_info'),
                    ('visual_slam/image_1', '/front_d435_ired2'),
                    ('visual_slam/camera_info_1', '/front_d435_ired2_info'),
                    ('visual_slam/imu', '/realsense_front/imu')]
    )

    visual_slam_launch_container = ComposableNodeContainer(
        name='visual_slam_launch_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[visual_slam_node],
        output='screen'
    )

Here is the gazebo bridge:

bridge = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
                   '/realsense_front/imu@sensor_msgs/msg/Imu[gz.msgs.IMU',
                   '/front_camera@sensor_msgs/msg/Image@gz.msgs.Image',
                   '/front_camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
                   '/front_d435_ired1@sensor_msgs/msg/Image@gz.msgs.Image',
                   '/front_d435_ired1_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
                   '/front_d435_ired2@sensor_msgs/msg/Image@gz.msgs.Image',
                   '/front_d435_ired2@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
                   ],
        output='screen'
    )

Any tips how I can proceed?

2

Hi @huzaifa1

Thank you write this topic.
All Isaac ROS demos, support to stream all topics in and out from the docker.

I suggest to check if there are network issues between your host and your docker or if your are working hardware in the loop between your device and your desktop with Gazebo.

Open a new docker container instance and check if all topics are available:

cd ${ISAAC_ROS_WS}/src/isaac_ros_common && \
./scripts/run_dev.sh

and after

ros2 topic list

Check if these nodes are available on the docker container before running your launch script; if it is true, also check with

ros2 topic info visual_slam/image_0

if your topic is streaming an output

Let me know if this hint helps you.

Raffaello

3

When I run Gazebo with the robot and the sensors (IMU and Realsense with 2 Infrared feeds), this is the list of topics available inside docker using ros2 topic list:

/clock
/front_camera
/front_camera_info
/front_d435_ired1
/front_d435_ired1_info
/front_d435_ired2
/joint_states
/parameter_events
/realsense_front/imu
/robot_description
/rosout
/tf
/tf_static

With Gazebo and the topics running, I run the following visual slam node:

visual_slam_node = ComposableNode(
        name='visual_slam_node',
        package='isaac_ros_visual_slam',
        plugin='nvidia::isaac_ros::visual_slam::VisualSlamNode',
        parameters=[{
                    'enable_image_denoising': False,
                    'use_sim_time': True,
                    'rectified_images': False,
                    'enable_imu_fusion': True,
                    'gyro_noise_density': 0.000244,
                    'gyro_random_walk': 0.000019393,
                    'accel_noise_density': 0.001862,
                    'accel_random_walk': 0.003,
                    'calibration_frequency': 200.0,
                    'image_jitter_threshold_ms': 22.00,
                    'base_frame': 'realsense_front_link',
                    'imu_frame': 'realsense_front_accel_frame',
                    'enable_slam_visualization': True,
                    'enable_landmarks_view': True,
                    'enable_observations_view': True,
                    'camera_optical_frames': [
                        'realsense_front_infra1_frame',
                        'realsense_front_infra2_frame',
                    ],
                    }],
        remappings=[('visual_slam/image_0', '/front_d435_ired1'),
                    ('visual_slam/camera_info_0', '/front_d435_ired1_info'),
                    ('visual_slam/image_1', '/front_d435_ired2'),
                    ('visual_slam/camera_info_1', '/front_d435_ired2_info'),
                    ('visual_slam/imu', '/realsense_front/imu')]
    )

    visual_slam_launch_container = ComposableNodeContainer(
        name='visual_slam_launch_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=[visual_slam_node],
        output='screen'
    )

Here is what the node graph looks like in rqt:

rosgraph
rosgraph3762×4299 1.07 MB

How should I go about getting the odometry from the visual slam node?

Thanks

4

The error seemed to be fixed with the following setup of the Gazebo topics:

bridge = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
                   '/realsense_front/imu@sensor_msgs/msg/Imu[gz.msgs.IMU',
                   '/realsense_front/camera/image@sensor_msgs/msg/Image@gz.msgs.Image',
                   '/realsense_front/camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
                   '/realsense_front/infra_left/image@sensor_msgs/msg/Image@gz.msgs.Image',
                   '/realsense_front/infra_left/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
                   '/realsense_front/infra_right/image@sensor_msgs/msg/Image@gz.msgs.Image',
                   '/realsense_front/infra_right/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',

                   ],
        output='screen'
    )

And then remapping them accordingly in the VSLAM node:

visual_slam_node = ComposableNode(
        name='visual_slam_node',
        package='isaac_ros_visual_slam',
        plugin='nvidia::isaac_ros::visual_slam::VisualSlamNode',
        pparameters=[{
                    'use_sim_time': True,
                    'enable_image_denoising': True,
                    'rectified_images': True,
                    'enable_slam_visualization': True,
                    'enable_observations_view': True,
                    'enable_landmarks_view': True,
                    
                    }],
        remappings=[('visual_slam/image_0', '/realsense_front/infra_left/image'),
                    ('visual_slam/camera_info_0', '/realsense_front/infra_left/camera_info'),
                    ('visual    _slam/image_1', '/realsense_front/infra_right/image'),
                    ('visual_slam/camera_info_1', '/realsense_front/infra_right/camera_info'),
                    ('visual_slam/imu', '/realsense_front/imu')]
    )

I am able to get feedback from the VSLAM node and odometry is published. However, right now it is acting very erratic and doesn’t resemble the simulated odometry at all.