I am running an orbbec camera using ISAAC_SOS_Soundationpose, and the ROS topic for the camera section is:
/weng_camera/color/camera_info
/Weng.camera/color/image_raw # rgb image
/weng_camera/color/image_raw/compressed
/weng_camera/color/image_raw/compressedDepth
/weng_camera/color/image_raw/theora
/weng_camera/color/metadata
/weng_camera/depth/camera_info
/Weng.camera/depth/image_raw # depth information, uint16
/weng_camera/depth/image_raw/compressed
/weng_camera/depth/image_raw/compressedDepth
/weng_camera/depth/image_raw/theora
/weng_camera/depth/metadata
/weng_camera/depth_filter_status
/weng_camera/depth_to_color
/weng_mask # mask msg
The isaac_ros_foundationpose. launch. py file is as follows:
import os
from ament_index_python.packages import get_package_share_directory
import launch
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
MESH_FILE_NAME = '/tmp/textured_simple.obj'
TEXTURE_MAP_NAME = '/tmp/texture_map.png'
REFINE_MODEL_NAME = '/tmp/refine_model.onnx'
REFINE_ENGINE_NAME = '/tmp/refine_trt_engine.plan'
SCORE_MODEL_NAME = '/tmp/score_model.onnx'
SCORE_ENGINE_NAME = '/tmp/score_trt_engine.plan'
def generate_launch_description():
"""Generate launch description for testing relevant nodes."""
rviz_config_path = os.path.join(
get_package_share_directory('isaac_ros_foundationpose'),
'rviz', 'foundationpose.rviz')
launch_args = [
DeclareLaunchArgument(
'mesh_file_path',
default_value=MESH_FILE_NAME,
description='The absolute file path to the mesh file'),
DeclareLaunchArgument(
'texture_path',
default_value=TEXTURE_MAP_NAME,
description='The absolute file path to the texture map'),
DeclareLaunchArgument(
'refine_model_file_path',
default_value=REFINE_MODEL_NAME,
description='The absolute file path to the refine model'),
DeclareLaunchArgument(
'refine_engine_file_path',
default_value=REFINE_ENGINE_NAME,
description='The absolute file path to the refine trt engine'),
DeclareLaunchArgument(
'score_model_file_path',
default_value=SCORE_MODEL_NAME,
description='The absolute file path to the score model'),
DeclareLaunchArgument(
'score_engine_file_path',
default_value=SCORE_ENGINE_NAME,
description='The absolute file path to the score trt engine'),
DeclareLaunchArgument(
'mask_height',
default_value='360',
description='The height of the mask generated from the bounding box'),
DeclareLaunchArgument(
'mask_width',
default_value='640',
description='The width of the mask generated from the bounding box'),
DeclareLaunchArgument(
'launch_bbox_to_mask',
default_value='False',
description='Flag to enable bounding box to mask converter'),
DeclareLaunchArgument(
'launch_rviz',
default_value='False',
description='Flag to enable Rviz2 launch'),
]
mesh_file_path = LaunchConfiguration('mesh_file_path')
texture_path = LaunchConfiguration('texture_path')
refine_model_file_path = LaunchConfiguration('refine_model_file_path')
refine_engine_file_path = LaunchConfiguration('refine_engine_file_path')
score_model_file_path = LaunchConfiguration('score_model_file_path')
score_engine_file_path = LaunchConfiguration('score_engine_file_path')
mask_height = LaunchConfiguration('mask_height')
mask_width = LaunchConfiguration('mask_width')
launch_rviz = LaunchConfiguration('launch_rviz')
launch_bbox_to_mask = LaunchConfiguration('launch_bbox_to_mask')
foundationpose_node = ComposableNode(
name='foundationpose',
package='isaac_ros_foundationpose',
plugin='nvidia::isaac_ros::foundationpose::FoundationPoseNode',
parameters=[{
'mesh_file_path': mesh_file_path,
'texture_path': texture_path,
'refine_model_file_path': refine_model_file_path,
'refine_engine_file_path': refine_engine_file_path,
'refine_input_tensor_names': ['input_tensor1', 'input_tensor2'],
'refine_input_binding_names': ['input1', 'input2'],
'refine_output_tensor_names': ['output_tensor1', 'output_tensor2'],
'refine_output_binding_names': ['output1', 'output2'],
'score_model_file_path': score_model_file_path,
'score_engine_file_path': score_engine_file_path,
'score_input_tensor_names': ['input_tensor1', 'input_tensor2'],
'score_input_binding_names': ['input1', 'input2'],
'score_output_tensor_names': ['output_tensor'],
'score_output_binding_names': ['output1'],
}],
# remappings=[
# ('pose_estimation/depth_image', 'depth_registered/image_rect'),
# ('pose_estimation/image', 'rgb/image_rect_color'),
# ('pose_estimation/camera_info', 'rgb/camera_info'),
# ('pose_estimation/segmentation', 'segmentation'),
# ('pose_estimation/output', 'output')])
remappings=[
('pose_estimation/depth_image', '/weng_camera/depth/image_raw'),
('pose_estimation/image', '/weng_camera/color/image_raw'),
('pose_estimation/camera_info', '/weng_camera/color/camera_info'),
('pose_estimation/segmentation', '/weng_mask'),
('pose_estimation/output', '/pose_estimation/pose_output')
])
# rviz_node = Node(
# package='rviz2',
# executable='rviz2',
# name='rviz2',
# arguments=['-d', rviz_config_path],
# condition=IfCondition(launch_rviz))
# detection2_d_to_mask_node = ComposableNode(
# name='detection2_d_to_mask',
# package='isaac_ros_foundationpose',
# plugin='nvidia::isaac_ros::foundationpose::Detection2DToMask',
# parameters=[{
# 'mask_width': mask_width,
# 'mask_height': mask_height
# }])
# foundationpose_bbox_container = ComposableNodeContainer(
# name='foundationpose_container',
# namespace='foundationpose_container',
# package='rclcpp_components',
# executable='component_container_mt',
# composable_node_descriptions=[
# foundationpose_node,
# detection2_d_to_mask_node],
# output='screen',
# condition=IfCondition(launch_bbox_to_mask)
# )
foundationpose_container = ComposableNodeContainer(
name='foundationpose_container',
namespace='foundationpose_container',
package='rclcpp_components',
executable='component_container_mt',
composable_node_descriptions=[foundationpose_node],
output='screen',
condition=UnlessCondition(launch_bbox_to_mask)
)
return launch.LaunchDescription(launch_args + [foundationpose_container])#,foundationpose_bbox_container,rviz_node])
The command I am running is:
ros2 launch isaac_ros_foundationpose isaac_ros_foundationpose.launch.py \
refine_engine_file_path:=${ISAAC_ROS_WS}/isaac_ros_assets/models/foundationpose/refine_trt_engine.plan \
score_engine_file_path:=${ISAAC_ROS_WS}/isaac_ros_assets/models/foundationpose/score_trt_engine.plan \
mesh_file_path:=${ISAAC_ROS_WS}/isaac_ros_assets/isaac_ros_foundationpose/Mustard/textured_simple.obj \
texture_path:=${ISAAC_ROS_WS}/isaac_ros_assets/isaac_ros_foundationpose/Mustard/texture_map.png
log is :
[INFO] [launch]: All log files can be found below /root/.ros/log/2024-11-23-22-31-06-530027-ubuntu-25743
[INFO] [launch]: Default logging verbosity is set to INFO
1732372266.638928 [168] ros2: selected interface "lo" is not multicast-capable: disabling multicast
[INFO] [component_container_mt-1]: process started with pid [25754]
[component_container_mt-1] 1732372266.707343 [168] component_: selected interface "lo" is not multicast-capable: disabling multicast
[component_container_mt-1] [INFO] [1732372266.940053170] [foundationpose_container.foundationpose_container]: Load Library: /home/rykj/workspaces/isaac_ros-dev/install/isaac_ros_foundationpose/lib/libfoundationpose_node.so
[component_container_mt-1] [INFO] [1732372267.025108058] [foundationpose_container.foundationpose_container]: Found class: rclcpp_components::NodeFactoryTemplate<nvidia::isaac_ros::foundationpose::FoundationPoseNode>
[component_container_mt-1] [INFO] [1732372267.025280960] [foundationpose_container.foundationpose_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<nvidia::isaac_ros::foundationpose::FoundationPoseNode>
[component_container_mt-1] [INFO] [1732372267.038546493] [foundationpose]: [NitrosNode] Initializing NitrosNode
[component_container_mt-1] [INFO] [1732372267.039617345] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/std/libgxf_std.so
[component_container_mt-1] [INFO] [1732372267.049223140] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_gxf_helpers.so
[component_container_mt-1] [INFO] [1732372267.056729984] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_sight.so
[component_container_mt-1] [INFO] [1732372267.065538664] [NitrosContext]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_atlas.so
[component_container_mt-1] 2024-11-23 22:31:07.088 WARN external/com_nvidia_gxf/gxf/std/program.cpp@532: No GXF scheduler specified.
[component_container_mt-1] [INFO] [1732372267.091968159] [foundationpose]: [NitrosNode] Starting NitrosNode
[component_container_mt-1] [INFO] [1732372267.100467388] [foundationpose]: [NitrosNode] Loading extensions
[component_container_mt-1] [INFO] [1732372267.100918699] [foundationpose]: [NitrosContext] Loading extension: gxf/lib/multimedia/libgxf_multimedia.so
[component_container_mt-1] [INFO] [1732372267.106860019] [foundationpose]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_message_compositor.so
[component_container_mt-1] [INFO] [1732372267.108943289] [foundationpose]: [NitrosContext] Loading extension: gxf/lib/serialization/libgxf_serialization.so
[component_container_mt-1] [INFO] [1732372267.116877411] [foundationpose]: [NitrosContext] Loading extension: gxf/lib/cuda/libgxf_cuda.so
[component_container_mt-1] [INFO] [1732372267.122749384] [foundationpose]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_depth_image_proc.so
[component_container_mt-1] [INFO] [1732372267.199129677] [foundationpose]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_sgm.so
[component_container_mt-1] [INFO] [1732372267.202170483] [foundationpose]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_messages.so
[component_container_mt-1] [INFO] [1732372267.204521474] [foundationpose]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_ros_messages.so
[component_container_mt-1] [INFO] [1732372267.206283549] [foundationpose]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_tensor_rt.so
[component_container_mt-1] [INFO] [1732372267.291765811] [foundationpose]: [NitrosContext] Loading extension: gxf/lib/libgxf_isaac_foundationpose.so
[component_container_mt-1] [INFO] [1732372267.469290172] [foundationpose]: [NitrosNode] Loading graph to the optimizer
[component_container_mt-1] [INFO] [1732372267.501124297] [foundationpose]: [NitrosNode] Running optimization
[component_container_mt-1] [INFO] [1732372268.775651121] [foundationpose]: [NitrosNode] Obtaining graph IO group info from the optimizer
[component_container_mt-1] [INFO] [1732372268.803494744] [foundationpose]: [NitrosNode] Starting negotiation...
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/foundationpose' in container '/foundationpose_container/foundationpose_container'
[component_container_mt-1] [INFO] [1732372269.806261884] [foundationpose]: [NitrosNode] Starting post negotiation setup
[component_container_mt-1] [INFO] [1732372269.806405825] [foundationpose]: [NitrosNode] Getting data format negotiation results
[component_container_mt-1] [INFO] [1732372269.806441890] [foundationpose]: [NitrosPublisher] Negotiation ended with no results
[component_container_mt-1] [INFO] [1732372269.806480259] [foundationpose]: [NitrosPublisher] Use only the compatible publisher: topic_name="/pose_estimation/pose_output", data_format="nitros_detection3_d_array"
[component_container_mt-1] [INFO] [1732372269.806501444] [foundationpose]: [NitrosPublisher] Negotiation ended with no results
[component_container_mt-1] [INFO] [1732372269.806515685] [foundationpose]: [NitrosPublisher] Use only the compatible publisher: topic_name="/pose_estimation/pose_matrix_output", data_format="nitros_tensor_list_nchw"
[component_container_mt-1] [INFO] [1732372269.806535717] [foundationpose]: [NitrosSubscriber] Negotiation ended with no results
[component_container_mt-1] [INFO] [1732372269.806551110] [foundationpose]: [NitrosSubscriber] Use the compatible subscriber: topic_name="/weng_camera/color/image_raw", data_format="nitros_image_rgb8"
[component_container_mt-1] [INFO] [1732372269.806565670] [foundationpose]: [NitrosSubscriber] Negotiation ended with no results
[component_container_mt-1] [INFO] [1732372269.806577223] [foundationpose]: [NitrosSubscriber] Use the compatible subscriber: topic_name="/weng_camera/color/camera_info", data_format="nitros_camera_info"
[component_container_mt-1] [INFO] [1732372269.806731788] [foundationpose]: [NitrosSubscriber] Negotiation ended with no results
[component_container_mt-1] [INFO] [1732372269.806756493] [foundationpose]: [NitrosSubscriber] Use the compatible subscriber: topic_name="/weng_mask", data_format="nitros_image_mono8"
[component_container_mt-1] [INFO] [1732372269.806772333] [foundationpose]: [NitrosSubscriber] Negotiation ended with no results
[component_container_mt-1] [INFO] [1732372269.806784270] [foundationpose]: [NitrosSubscriber] Use the compatible subscriber: topic_name="/weng_camera/depth/image_raw", data_format="nitros_image_32FC1"
[component_container_mt-1] [INFO] [1732372269.807230461] [foundationpose]: [NitrosNode] Exporting the final graph based on the negotiation results
[component_container_mt-1] [INFO] [1732372279.173162954] [foundationpose]: [NitrosNode] Wrote the final top level YAML graph to "/tmp/isaac_ros_nitros/graphs/JHNRMNMPKF/JHNRMNMPKF.yaml"
[component_container_mt-1] [INFO] [1732372279.173368817] [foundationpose]: [NitrosNode] Loading application
[component_container_mt-1] 2024-11-23 22:31:19.198 WARN external/com_nvidia_gxf/gxf/std/yaml_file_loader.cpp@1077: Using unregistered parameter 'dummy_rx' in component ''.
[component_container_mt-1] 2024-11-23 22:31:19.199 WARN external/com_nvidia_gxf/gxf/std/yaml_file_loader.cpp@1077: Using unregistered parameter 'dummy_rx' in component ''.
[component_container_mt-1] 2024-11-23 22:31:19.200 WARN external/com_nvidia_gxf/gxf/std/yaml_file_loader.cpp@1077: Using unregistered parameter 'dev_id' in component 'stream'.
[component_container_mt-1] [INFO] [1732372279.205081978] [foundationpose]: [NitrosNode] Initializing and running GXF graph
[component_container_mt-1] 2024-11-23 22:31:19.239 WARN external/com_nvidia_gxf/gxf/std/scheduling_terms.cpp@333: 'min_size' parameter in MultiMessageAvailableSchedulingTerm is deprecated. Use 'min_sum' with SumOfAll sampling mode instead
[component_container_mt-1] 2024-11-23 22:31:19.239 WARN external/com_nvidia_gxf/gxf/std/scheduling_terms.cpp@333: 'min_size' parameter in MultiMessageAvailableSchedulingTerm is deprecated. Use 'min_sum' with SumOfAll sampling mode instead
[component_container_mt-1] 2024-11-23 22:31:19.239 WARN external/com_nvidia_gxf/gxf/std/scheduling_terms.cpp@333: 'min_size' parameter in MultiMessageAvailableSchedulingTerm is deprecated. Use 'min_sum' with SumOfAll sampling mode instead
[component_container_mt-1] 2024-11-23 22:31:19.239 WARN external/com_nvidia_gxf/gxf/std/scheduling_terms.cpp@333: 'min_size' parameter in MultiMessageAvailableSchedulingTerm is deprecated. Use 'min_sum' with SumOfAll sampling mode instead
[component_container_mt-1] 2024-11-23 22:31:19.245 WARN external/com_nvidia_gxf/gxf/std/scheduling_terms.cpp@333: 'min_size' parameter in MultiMessageAvailableSchedulingTerm is deprecated. Use 'min_sum' with SumOfAll sampling mode instead
[component_container_mt-1] [INFO] [1732372279.271811802] [foundationpose]: [NitrosNode] Node was started
[component_container_mt-1] 2024-11-23 22:31:19.992 WARN ./gxf/extensions/tensor_rt/tensor_rt_inference.cpp@155: TRT WARNING: Using an engine plan file across different models of devices is not recommended and is likely to affect performance or even cause errors.
[component_container_mt-1] 2024-11-23 22:31:22.010 WARN ./gxf/extensions/tensor_rt/tensor_rt_inference.cpp@155: TRT WARNING: Using an engine plan file across different models of devices is not recommended and is likely to affect performance or even cause errors.
all ros2 topic is :
/clicked_point
/diagnostics
/goal_pose
/initialpose
/parameter_events
/pose_estimation/pose_matrix_output
/pose_estimation/pose_matrix_output/nitros
/pose_estimation/pose_output
/pose_estimation/pose_output/nitros
/rosout
/tf
/tf_static
/weng_camera/color/camera_info
/weng_camera/color/camera_info/nitros
/weng_camera/color/image_raw
/weng_camera/color/image_raw/compressed
/weng_camera/color/image_raw/compressedDepth
/weng_camera/color/image_raw/nitros
/weng_camera/color/image_raw/theora
/weng_camera/color/metadata
/weng_camera/depth/camera_info
/weng_camera/depth/image_raw
/weng_camera/depth/image_raw/compressed
/weng_camera/depth/image_raw/compressedDepth
/weng_camera/depth/image_raw/nitros
/weng_camera/depth/image_raw/theora
/weng_camera/depth/metadata
/weng_camera/depth_filter_status
/weng_camera/depth_to_color
/weng_mask
/weng_mask/nitros
The bug is as follows：
root@ubuntu:/home/# ros2 topic hz /pose_estimation/pose_output
1732372404.991183 [168] ros2: selected interface "lo" is not multicast-capable: disabling multicast
1732372404.991717 [168] ros2: Failed to find a free participant index for domain 168
[ERROR] [1732372404.991898792] [rmw_cyclonedds_cpp]: rmw_create_node: failed to create domain, error Error
>>> [rcutils|error_handling.c:108] rcutils_set_error_state()
This error state is being overwritten:
'error not set, at ./src/rcl/node.c:263'
with this new error message:
'rcl node's rmw handle is invalid, at ./src/rcl/node.c:415'
rcutils_reset_error() should be called after error handling to avoid this.
<<<
[ERROR] [1732372404.992018284] [rcl]: Failed to fini publisher for node: 1
error creating node: rcl node's rmw handle is invalid, at ./src/rcl/node.c:415
I can’t read the output information of the foundation pose.