test.launch.py
from ament_index_python.packages import get_package_share_directory
import launch
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
REFINE_MODEL_NAME = '/tmp/refine_model.onnx'
SCORE_MODEL_NAME = '/tmp/score_model.onnx'
MESH_FILE_NAME = '/home/rykj/workspaces/isaac_ros-dev/isaac_ros_assets/isaac_ros_foundationpose/oldyouqiang_iphoneCAD_coorWoDian/AR-Code-Object-Capture-app-1730803003.obj'
TEXTURE_MAP_NAME = '/home/rykj/workspaces/isaac_ros-dev/isaac_ros_assets/isaac_ros_foundationpose/oldyouqiang_iphoneCAD_coorWoDian/baked_mesh_34b42ceb_tex0.png'
REFINE_ENGINE_NAME = '/home/rykj/workspaces/isaac_ros-dev/isaac_ros_assets/models/foundationpose/refine_trt_engine.plan'
SCORE_ENGINE_NAME = '/home/rykj/workspaces/isaac_ros-dev/isaac_ros_assets/models/foundationpose/score_trt_engine.plan'
def generate_launch_description():
'''Generate launch description for testing relevant nodes.'''
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'),
]
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')
# Realsense depth is in uint16 and millimeters. Convert to float32 and meters
convert_metric_node = ComposableNode(
package='isaac_ros_depth_image_proc',
plugin='nvidia::isaac_ros::depth_image_proc::ConvertMetricNode',
# remappings=[
# ('image_raw', 'camera/aligned_depth_to_color/image_raw'),
# ('image', 'depth_fp32')
# ]
remappings=[
('image_raw', 'weng_camera/depth/image_raw'),
('image', 'depth_fp32')
]
)
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'],
'refine_iterations': 1,
}],
remappings=[
('pose_estimation/depth_image', 'depth_fp32'),
('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_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')
# ])
foundationpose_container = ComposableNodeContainer(
name='foundationpose_container',
namespace='foundationpose_container',
package='rclcpp_components',
executable='component_container_mt',
composable_node_descriptions=[convert_metric_node, foundationpose_node],
output='screen'
)
return launch.LaunchDescription(launch_args + [foundationpose_container])
log:
[component_container_mt-1] [WARN] [1732507326.122525684] [foundationpose.NitrosSubscriber]: 10.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507326.224459360] [foundationpose.NitrosSubscriber]: 10.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507326.243960398] [foundationpose.NitrosSubscriber]: 10.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507326.306076127] [foundationpose.NitrosSubscriber]: 10.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507343.890402056] [foundationpose.NitrosSubscriber]: 20.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507343.923873879] [foundationpose.NitrosSubscriber]: 20.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507343.998192132] [foundationpose.NitrosSubscriber]: 20.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507344.044737427] [foundationpose.NitrosSubscriber]: 20.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507361.668813725] [foundationpose.NitrosSubscriber]: 30.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507361.715297607] [foundationpose.NitrosSubscriber]: 30.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507361.728535853] [foundationpose.NitrosSubscriber]: 30.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507361.818401999] [foundationpose.NitrosSubscriber]: 30.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507379.370805292] [foundationpose.NitrosSubscriber]: 40.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507379.424199920] [foundationpose.NitrosSubscriber]: 40.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507379.492463957] [foundationpose.NitrosSubscriber]: 40.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507379.562394486] [foundationpose.NitrosSubscriber]: 40.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507397.082216372] [foundationpose.NitrosSubscriber]: 50.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507397.111238233] [foundationpose.NitrosSubscriber]: 50.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507397.137425335] [foundationpose.NitrosSubscriber]: 50.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507397.298665374] [foundationpose.NitrosSubscriber]: 50.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507414.718116991] [foundationpose.NitrosSubscriber]: 60.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507414.771513628] [foundationpose.NitrosSubscriber]: 60.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507414.828119886] [foundationpose.NitrosSubscriber]: 60.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507414.897877408] [foundationpose.NitrosSubscriber]: 60.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507432.432018493] [foundationpose.NitrosSubscriber]: 70.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507432.591204453] [foundationpose.NitrosSubscriber]: 70.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507432.661070998] [foundationpose.NitrosSubscriber]: 70.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507432.720597295] [foundationpose.NitrosSubscriber]: 70.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507450.378287701] [foundationpose.NitrosSubscriber]: 80.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507450.465670303] [foundationpose.NitrosSubscriber]: 80.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507450.591621029] [foundationpose.NitrosSubscriber]: 80.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507450.675975649] [foundationpose.NitrosSubscriber]: 80.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507468.104982631] [foundationpose.NitrosSubscriber]: 90.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507468.270714410] [foundationpose.NitrosSubscriber]: 90.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507468.373252053] [foundationpose.NitrosSubscriber]: 90.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507468.381895279] [foundationpose.NitrosSubscriber]: 90.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507485.836229021] [foundationpose.NitrosSubscriber]: 100.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507485.988435116] [foundationpose.NitrosSubscriber]: 100.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507486.030525730] [foundationpose.NitrosSubscriber]: 100.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507486.228366343] [foundationpose.NitrosSubscriber]: 100.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507503.363124717] [foundationpose.NitrosSubscriber]: 110.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507503.687853497] [foundationpose.NitrosSubscriber]: 110.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507503.770784276] [foundationpose.NitrosSubscriber]: 110.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507503.909129476] [foundationpose.NitrosSubscriber]: 110.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507521.217134727] [foundationpose.NitrosSubscriber]: 120.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507521.679624854] [foundationpose.NitrosSubscriber]: 120.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507521.715721711] [foundationpose.NitrosSubscriber]: 120.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507521.933443661] [foundationpose.NitrosSubscriber]: 120.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507539.011098367] [foundationpose.NitrosSubscriber]: 130.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507539.408926776] [foundationpose.NitrosSubscriber]: 130.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507539.450650618] [foundationpose.NitrosSubscriber]: 130.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507539.569838249] [foundationpose.NitrosSubscriber]: 130.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507556.691866883] [foundationpose.NitrosSubscriber]: 140.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507557.212771645] [foundationpose.NitrosSubscriber]: 140.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507557.223988818] [foundationpose.NitrosSubscriber]: 140.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507557.237861350] [foundationpose.NitrosSubscriber]: 140.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507574.333529229] [foundationpose.NitrosSubscriber]: 150.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507574.783067173] [foundationpose.NitrosSubscriber]: 150.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507574.854899839] [foundationpose.NitrosSubscriber]: 150.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507575.017750726] [foundationpose.NitrosSubscriber]: 150.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507592.035931622] [foundationpose.NitrosSubscriber]: 160.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507592.472886085] [foundationpose.NitrosSubscriber]: 160.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507592.675891336] [foundationpose.NitrosSubscriber]: 160.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507592.868200010] [foundationpose.NitrosSubscriber]: 160.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507609.810180240] [foundationpose.NitrosSubscriber]: 170.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507610.407263249] [foundationpose.NitrosSubscriber]: 170.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507610.493177439] [foundationpose.NitrosSubscriber]: 170.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507610.589478529] [foundationpose.NitrosSubscriber]: 170.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507627.402847227] [foundationpose.NitrosSubscriber]: 180.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507627.989828377] [foundationpose.NitrosSubscriber]: 180.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507628.151890894] [foundationpose.NitrosSubscriber]: 180.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507628.265317842] [foundationpose.NitrosSubscriber]: 180.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507645.051176653] [foundationpose.NitrosSubscriber]: 190.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507645.688555168] [foundationpose.NitrosSubscriber]: 190.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507645.907304125] [foundationpose.NitrosSubscriber]: 190.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507645.920766431] [foundationpose.NitrosSubscriber]: 190.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
[component_container_mt-1] [WARN] [1732507662.820821381] [foundationpose.NitrosSubscriber]: 200.0s passed while waiting to push a message entity (eid=42547) to the receiver mask_receiver
[component_container_mt-1] [WARN] [1732507663.304736394] [foundationpose.NitrosSubscriber]: 200.0s passed while waiting to push a message entity (eid=42079) to the receiver depth_receiver
[component_container_mt-1] [WARN] [1732507663.629557682] [foundationpose.NitrosSubscriber]: 200.0s passed while waiting to push a message entity (eid=42541) to the receiver camera_model_receiver
[component_container_mt-1] [WARN] [1732507663.725470682] [foundationpose.NitrosSubscriber]: 200.0s passed while waiting to push a message entity (eid=42536) to the receiver rgb_image_receiver
Why is foundationpose processing so slow