Using Nvblox with multi-robots

[monochromatics]

Recently I made own environment in isaac sim for nvblox, it working so fine. Now I want to using multi robots with nvblox in Isaac Sim. Like multi-robot navigation in Isaac Sim doc, I'm just adding namespace(carter0) and remapping topics from carter_sim.launch.py.

# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES
# Copyright (c) 2021-2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# SPDX-License-Identifier: Apache-2.0

from email.headerregistry import Group
import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch.actions import GroupAction
from launch_ros.actions import PushRosNamespace

from launch_ros.actions import Node

namespace = LaunchConfiguration("namespace")
use_namespace = LaunchConfiguration("use_namespace")
use_lidar = LaunchConfiguration("use_lidar")
use_depth = LaunchConfiguration("use_depth")

def generate_launch_description():

    ###
    declare_namespace_cmd = DeclareLaunchArgument("namespace", default_value="carter0", description="Top-level namespace")

    declare_use_namespace_cmd = DeclareLaunchArgument(
        "use_namespace", default_value="true", description="Whether to apply a namespace to the navigation stack"
    )

    ###

    params_file_arg = DeclareLaunchArgument(
        'params_file', default_value=os.path.join(
            get_package_share_directory(
                'nvblox_nav2'), 'params/warehouse', 'multi_robot_carter_navigation_params_0.yaml'),
        description='Full path to param file to load')

    use_sim_time_arg = DeclareLaunchArgument(
        'use_sim_time', default_value='True',
        description='Use simulation (Omniverse Isaac Sim) clock if true')

    nvblox_param_arg = DeclareLaunchArgument(
        'nvblox_params_file',
        default_value=os.path.join(
            get_package_share_directory('nvblox_nav2'), 'params/warehouse', 'nvblox_0.yaml'
        )
    )
    use_depth_arg = DeclareLaunchArgument(
        'use_depth', default_value='True',
        description='Use depth as an input for nvblox reconstruction'
    )
    use_lidar_arg = DeclareLaunchArgument(
        'use_lidar', default_value='False',
        description='Use lidar as an input for nvblox reconstruction'
    )
    run_rviz_arg = DeclareLaunchArgument(
        'run_rviz', default_value='True',
        description='Whether to start RVIZ')
    run_nav2_arg = DeclareLaunchArgument(
        'run_nav2', default_value='True',
        description='Whether to run nav2')

    nav2_bringup_launch_dir = os.path.join(
        get_package_share_directory('nav2_bringup'), 'launch')

    rviz_config_dir = os.path.join(get_package_share_directory(
        'nvblox_nav2'), 'config', 'carter_navigation_namespaced.rviz')

    rviz_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(nav2_bringup_launch_dir, 'rviz_launch.py')),
        launch_arguments={'using_namespace': 'True',
                          'autostart': 'True',
                          'rviz_config': rviz_config_dir}.items(),
        condition=IfCondition(LaunchConfiguration('run_rviz')))

    nav2_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(nav2_bringup_launch_dir, 'navigation_launch.py')),
        launch_arguments={'using_namespace': 'True',
                          'use_sim_time': LaunchConfiguration('use_sim_time'),
                          'params_file': LaunchConfiguration('params_file'),
                          'autostart': 'True'}.items(),
        condition=IfCondition(LaunchConfiguration('run_nav2')))

    nvblox_node = Node(
        package='nvblox_ros', executable='nvblox_node',
        parameters=[LaunchConfiguration('nvblox_params_file'),{
                     'use_sim_time': LaunchConfiguration('use_sim_time'),
                     'use_depth': LaunchConfiguration('use_depth'),
                     'use_lidar': LaunchConfiguration('use_lidar')}],
        output='screen',
        remappings=[('depth/image', 'left/depth'),
                    ('depth/camera_info', 'left/camera_info'),
                    ('color/image', 'left/rgb'),
                    ('color/camera_info', 'left/camera_info'),
                    ('pointcloud', 'point_cloud'),
                    ('/tf', 'tf'),
                    ('/tf_static', 'tf_static'),

                    ])

    map_to_odom_publisher = Node(
        package='tf2_ros', executable='static_transform_publisher',
        parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
        output='screen',
        arguments=['0.0', '0.0', '-0.3', '0.0', '0.0', '0.0', 'map',
                   'odom'],
        remappings=[('/tf', '/carter0/tf'),
                    ('/tf_static', '/carter0/tf_static'),
                    ('map', '/carter0/map'),
                    ('odom','/carter0/odom')],
        condition=IfCondition(LaunchConfiguration('run_nav2')))

    carter0_nav2_namespaced = GroupAction(
        actions=[
            PushRosNamespace('carter0'),
            nav2_launch, map_to_odom_publisher,nvblox_node,
        ]
    )

    return LaunchDescription([
        declare_namespace_cmd,
        declare_use_namespace_cmd,
        params_file_arg,
        use_sim_time_arg,
        nvblox_param_arg,
        use_depth_arg,
        use_lidar_arg,
        run_rviz_arg,
        run_nav2_arg,
        rviz_launch,
        #nav2_launch,
        carter0_nav2_namespaced,
        #nvblox_node,
        #map_to_odom_publisher
        ])

now I get list from ros2 topic list:

/carter0/behavior_server/transition_event
/carter0/behavior_tree_log
/carter0/bond
/carter0/bt_navigator/transition_event
/carter0/clicked_point
/carter0/clock
/carter0/cmd_vel
/carter0/cmd_vel_nav
/carter0/controller_server/transition_event
/carter0/cost_cloud
/carter0/evaluation
/carter0/global_costmap/costmap
/carter0/global_costmap/costmap_raw
/carter0/global_costmap/costmap_updates
/carter0/global_costmap/footprint
/carter0/global_costmap/global_costmap/transition_event
/carter0/global_costmap/published_footprint
/carter0/goal_pose
/carter0/initialpose
/carter0/left/camera_info
/carter0/left/depth
/carter0/left/rgb
/carter0/local_costmap/costmap
/carter0/local_costmap/costmap_raw
/carter0/local_costmap/costmap_updates
/carter0/local_costmap/footprint
/carter0/local_costmap/local_costmap/transition_event
/carter0/local_costmap/published_footprint
/carter0/local_plan
/carter0/marker
/carter0/nvblox_node/map_slice
/carter0/nvblox_node/map_slice_bounds
/carter0/nvblox_node/map_slice_bounds_array
/carter0/nvblox_node/mesh
/carter0/nvblox_node/mesh_marker
/carter0/nvblox_node/pointcloud
/carter0/odom
/carter0/plan
/carter0/plan_smoothed
/carter0/planner_server/transition_event
/carter0/point_cloud
/carter0/pose
/carter0/received_global_plan
/carter0/scan
/carter0/smoother_server/transition_event
/carter0/speed_limit
/carter0/tf
/carter0/tf_static
/carter0/transform
/carter0/transformed_global_plan
/carter0/velocity_smoother/transition_event
/carter0/waypoint_follower/transition_event
/clock
/diagnostics
/parameter_events
/rosout

I can't using rqt_graph(I have no idea to install it inside docker container), I'm sorry. in more detail, It can run, but I can't see any meshes, only blank space and tf marker. I can using nav2's 2D goal pose in that empty space, and robot moves.

Is there any specific changes need to adding namespace to nvblox node?

regards. Lee.

[alexmillane]

Thanks for your interest in nvblox, and great that you want to get multiple robots going, that's cool!

So can you just confirm that all the image topics are correctly subscribed to in their new carter0 namespace?

One potential issue is the tf tree. When nvblox gets a depth image, it looks up the camera pose based on the frame_id in the header. I wonder if the depth images output from Isaac Sim have headers that indicate the new namespace. Could you check this?

In short though, if the topics are wired up correctly, and the poses exist on the tf tree, nvblox should operate just fine within a namespace and the multi-robot setup should work. So it's just a matter of getting everything wired up.