Error in nav2 path planning of nvblox

[innowing-robotics-intern]

Platform: Jetson Agx orin, jetpack 6, ubuntu22.04

We follow the quickstart and change the realsense_example.launch.py to try nav2.
We use the people mode: ros2 launch nvblox_examples_bringup realsense_example.py :=people
However, the program dies after we pubulish Goal pose.

[rviz2-3] [INFO] [1719218702.687899080] [rviz2]: Setting goal pose: Frame:odom, Position(-0.0620599, 0.373484, 0), Orientation(0, 0, 0.866043, 0.49997) = Angle: 2.09446
[component_container_mt-4] [INFO] [1719218702.692443560] [bt_navigator]: Begin navigating from current location (-0.36, -0.03) to (-0.06, 0.37)
[component_container_mt-4] [INFO] [1719218702.805125339] [controller_server]: Received a goal, begin computing control effort.
[component_container_mt-4] [WARN] [1719218702.805421947] [controller_server]: No goal checker was specified in parameter 'current_goal_checker'. Server will use only plugin loaded stopped_goal_checker . This warning will appear once.
[component_container_mt-4] [INFO] [1719218702.812563612] [controller_server]: Optimizer reset
[component_container_mt-4] [WARN] [1719218702.895972201] [controller_server]: Control loop missed its desired rate of 20.0000Hz
[component_container_mt-4] [WARN] [1719218702.967895861] [controller_server]: Control loop missed its desired rate of 20.0000Hz
[component_container_mt-4] terminate called after throwing an instance of 'std::runtime_error'
[component_container_mt-4]   what():  Taking data from action client but nothing is ready
[ERROR] [component_container_mt-4]: process has died [pid 360163, exit code -6, cmd '/opt/ros/humble/lib/rclcpp_components/component_container_mt --ros-args --log-level info --ros-args -r __node:=nvblox_container --params-file /opt/ros/humble/share/nova_carter_navigation/params/nova_carter_navigation.yaml --params-file /tmp/6c528240-09e3-4a30-86bd-16abb54c1b30.yaml --params-file /tmp/21db3e01-9901-4d65-8751-a8edca861397.yaml --params-file /tmp/c146c626-b74a-4a0c-8ed6-21289898472f.yaml --params-file /tmp/67a36707-c0b4-4488-9d3f-ecae900104ce.yaml --params-file /tmp/43f56e77-3757-4a3b-b921-2a54ad74a098.yaml --params-file /tmp/85336e79-90bf-45b1-b1dd-76e150a0b22a.yaml --params-file /tmp/bc7b66ab-46c5-4e49-bd6e-cd0a4600a6b6.yaml --params-file /tmp/8b6ea6c5-22cd-41b4-9764-fb09d9455540.yaml'].
[INFO] [launch]: process[component_container_mt-4] was required: shutting down launched system
[INFO] [rviz2-3]: sending signal 'SIGINT' to process[rviz2-3]
[INFO] [static_transform_publisher-2]: sending signal 'SIGINT' to process[static_transform_publisher-2]
[INFO] [static_transform_publisher-1]: sending signal 'SIGINT' to process[static_transform_publisher-1]
[rviz2-3] [INFO] [1719218707.449254608] [rclcpp]: signal_handler(signum=2)
[static_transform_publisher-2] [INFO] [1719218707.450418480] [rclcpp]: signal_handler(signum=2)
[static_transform_publisher-1] [INFO] [1719218707.451320209] [rclcpp]: signal_handler(signum=2)
[INFO] [static_transform_publisher-1]: process has finished cleanly [pid 360157]
[INFO] [static_transform_publisher-2]: process has finished cleanly [pid 360159]
[INFO] [rviz2-3]: process has finished cleanly [pid 360161]

This is my realsense_example.launch.py

from isaac_ros_launch_utils.all_types import *
import isaac_ros_launch_utils as lu
from launch import LaunchDescription
from launch_ros.actions import Node

from nvblox_ros_python_utils.nvblox_launch_utils import NvbloxMode, NvbloxCamera, NvbloxPeopleSegmentation
from nvblox_ros_python_utils.nvblox_constants import SEMSEGNET_INPUT_IMAGE_WIDTH, \
    SEMSEGNET_INPUT_IMAGE_HEIGHT, 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(
        '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).',
        cli=True)
    args.add_arg(
        'navigation',
        True,
        description='Whether to enable nav2 for navigation in Isaac Sim.',
        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))))

    # Navigation
    # NOTE: needs to be called before the component container because it modifies params globally
    actions.append(
        lu.include(
            'nvblox_examples_bringup',
            'launch/navigation/nvblox_carter_navigation.launch.py',
            launch_arguments={
                'container_name': NVBLOX_CONTAINER_NAME,
                'mode': args.mode,
            },
            condition=IfCondition(lu.is_true(args.navigation))))
    # tf from base_link to camera_link
    # from launch import LaunchDescription
    # from launch_ros.actions import Node
    actions.append(
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments=['0', '0', '0', '0', '0', '0', '1', 'camera_link', 'base_link'],
            name='static_tf_pub_world_to_base_link'
        )
    )
    # Realsense
    actions.append(
        lu.include(
            'nvblox_examples_bringup',
            'launch/sensors/realsense.launch.py',
            launch_arguments={'container_name': NVBLOX_CONTAINER_NAME},
            condition=UnlessCondition(lu.is_valid(args.rosbag))))

    # Visual SLAM
    actions.append(
        lu.include(
            'nvblox_examples_bringup',
            'launch/perception/vslam.launch.py',
            launch_arguments={
                'container_name': NVBLOX_CONTAINER_NAME,
                'camera': NvbloxCamera.realsense,
            },
            # Delay for 1 second to make sure that the static topics from the rosbag are published.
            delay=1.0,
            ))

    # People segmentation
    actions.append(
        lu.include(
            'nvblox_examples_bringup',
            'launch/perception/segmentation.launch.py',
            launch_arguments={
                'container_name': NVBLOX_CONTAINER_NAME,
                'people_segmentation': args.people_segmentation,
                'input_topic': '/camera/color/image_raw',
                'input_camera_info_topic': '/camera/color/camera_info',
            },
            condition=IfCondition(lu.has_substring(args.mode, NvbloxMode.people))))

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

    # 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': NvbloxCamera.realsense
            }))

    # Container
    actions.append(lu.component_container(NVBLOX_CONTAINER_NAME, log_level=args.log_level))

    return LaunchDescription(actions)

test.log