From a6a6d31780d97e06b0920c7e854c8c1c35ec96fd Mon Sep 17 00:00:00 2001 From: datean Date: Wed, 18 Dec 2024 15:37:14 +0800 Subject: [PATCH] run all nodes in launch.py --- .../base_static_transforms_publisher.py | 12 +- .../launch/orbbec_perceptor.launch.py | 45 +- isaac_orbbec_launch/param/perceptor_odom.rviz | 628 ++++++++++++++++++ 3 files changed, 681 insertions(+), 4 deletions(-) create mode 100644 isaac_orbbec_launch/param/perceptor_odom.rviz diff --git a/isaac_orbbec_launch/launch/base_static_transforms_publisher.py b/isaac_orbbec_launch/launch/base_static_transforms_publisher.py index ac5e4f86..b576046b 100644 --- a/isaac_orbbec_launch/launch/base_static_transforms_publisher.py +++ b/isaac_orbbec_launch/launch/base_static_transforms_publisher.py @@ -7,6 +7,7 @@ import yaml from scipy.spatial.transform import Rotation as R import sys import os +import argparse def rotation_matrix_to_quaternion(rotation_matrix): r = R.from_matrix(rotation_matrix) @@ -87,7 +88,16 @@ def main(): print("Usage: python static_transforms_publisher.py ") sys.exit(1) - yaml_path = sys.argv[1] + # yaml_path = sys.argv[1] + parser = argparse.ArgumentParser(description="Base Static Transforms Publisher") + parser.add_argument('--dev_matrices', type=str, required=True, + help="Path to the device extrinsics matrices file") + + args = parser.parse_args() + yaml_path = args.dev_matrices + + # Use the dev_matrices argument + print(f"Using device matrices file: {args.dev_matrices}") rclpy.init() node = StaticTransformsPublisher(yaml_path) diff --git a/isaac_orbbec_launch/launch/orbbec_perceptor.launch.py b/isaac_orbbec_launch/launch/orbbec_perceptor.launch.py index 0af0c96e..ca7eb52c 100644 --- a/isaac_orbbec_launch/launch/orbbec_perceptor.launch.py +++ b/isaac_orbbec_launch/launch/orbbec_perceptor.launch.py @@ -12,11 +12,22 @@ from launch.actions import GroupAction def generate_launch_description(): orbbec_camera_bringup_dir = get_package_share_directory('orbbec_camera') + isaac_orbbec_launch_dir = get_package_share_directory('isaac_orbbec_launch') perceptor_bringup_dir = get_package_share_directory('isaac_ros_perceptor_bringup') - perceptor_config_file = DeclareLaunchArgument( - 'perceptor_config_file', default_value='param/orbbec_perceptor_detached.yaml', + default_perceptor_config_file = os.path.join(isaac_orbbec_launch_dir, 'param', 'orbbec_perceptor_detached.yaml') + perceptor_odom_rviz_file = os.path.join(isaac_orbbec_launch_dir, 'param', 'perceptor_odom.rviz') + default_dev_matrices_file = os.path.join(isaac_orbbec_launch_dir, 'config', 'matrices_SN1423724335594.yaml') + + perceptor_config_file_arg = DeclareLaunchArgument( + 'perceptor_config_file', default_value=default_perceptor_config_file, description="Perceptor configuration") + perceptor_odom_rviz_file_arg = DeclareLaunchArgument( + 'perceptor_odom_rviz_file', default_value=perceptor_odom_rviz_file, + description="Perceptor configuration") + dev_matrices_arg = DeclareLaunchArgument( + 'dev_matrices', default_value=default_dev_matrices_file, + description='default device extrinsics matrices') from_bag_arg = DeclareLaunchArgument( 'from_bag', default_value='False', description='Whether to run from a bag or live realsense data') @@ -39,6 +50,22 @@ def generate_launch_description(): 'component_container_name': component_container_name_arg}.items(), condition=UnlessCondition(LaunchConfiguration('from_bag'))) + dev_matrices_arg = DeclareLaunchArgument( + 'dev_matrices', default_value=default_dev_matrices_file, + description='default device extrinsics matrices') + base_static_transforms_publisher = ExecuteProcess( + cmd=[ + 'python3', + os.path.join( + get_package_share_directory('isaac_orbbec_launch'), # Adjust this package name + 'launch', + 'base_static_transforms_publisher.py' + ), + '--dev_matrices', LaunchConfiguration('dev_matrices') + ], + output='screen' + ) + perceptor_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([os.path.join( perceptor_bringup_dir, 'launch', 'rgbd_perceptor.launch.py')]), @@ -58,13 +85,25 @@ def generate_launch_description(): output='screen', condition=IfCondition(LaunchConfiguration('from_bag'))) + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', LaunchConfiguration('perceptor_odom_rviz_file')] + ) + return LaunchDescription([ - perceptor_config_file, + perceptor_config_file_arg, + perceptor_odom_rviz_file_arg, + dev_matrices_arg, from_bag_arg, bag_path_arg, shared_orbbec_container, orbbec_launch, + base_static_transforms_publisher, perceptor_launch, bag_play, + rviz2_node, ] ) diff --git a/isaac_orbbec_launch/param/perceptor_odom.rviz b/isaac_orbbec_launch/param/perceptor_odom.rviz new file mode 100644 index 00000000..e3dfcce3 --- /dev/null +++ b/isaac_orbbec_launch/param/perceptor_odom.rviz @@ -0,0 +1,628 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Nvblox1 + - /Stereo Cameras1 + - /Stereo Cameras1/Back Stereo Camera1 + - /Depth Images1/Front Depth1/Topic1 + - /Depth Images1/Left Depth1/Topic1 + - /Depth Images1/Right Depth1/Topic1 + - /Depth Images1/Back Depth1 + - /Depth Images1/Back Depth1/Topic1 + - /Navigation1 + Splitter Ratio: 0.7601010203361511 + Tree Height: 664 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Combined Esdf +Visualization Manager: + Class: "" + Displays: + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.25 + Name: Base Link + Radius: 0.05000000074505806 + Reference Frame: base_link + Value: true + - Alpha: 0.25 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 0; 0; 0 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: false + - Class: rviz_common/Group + Displays: + - Ceiling Height: 1.5 + Class: nvblox_rviz_plugin/NvbloxVoxelBlockLayer + Cut Ceiling: false + Enabled: true + Name: Nvblox Voxels + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nvblox_node/color_layer + Value: true + - Ceiling Height: 1.5 + Class: nvblox_rviz_plugin/NvbloxVoxelBlockLayer + Cut Ceiling: false + Enabled: true + Name: Dynamic Voxels + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nvblox_node/dynamic_occupancy_layer + Value: true + - Class: nvblox_rviz_plugin/NvbloxMesh + Enabled: false + Name: Nvblox Mesh + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nvblox_node/mesh + Value: false + - Class: rviz_default_plugins/Marker + Enabled: false + Name: ESDF Slice Bounds + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nvblox_node/esdf_slice_bounds + Value: false + - Class: rviz_default_plugins/Marker + Enabled: false + Name: Workspace Bounds + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nvblox_node/workspace_bounds + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Static Esdf + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Tiles + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nvblox_node/static_esdf_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Human Esdf + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Tiles + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nvblox_node/dynamic_esdf_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Combined Esdf + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Tiles + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nvblox_node/combined_esdf_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Nvblox + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Front Stereo Camera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /front_camera/left_ir/image_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Left Stereo Camera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /left_camera/left_ir/image_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Right Stereo Camera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /right_camera/left_ir/image_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Back Stereo Camera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /rear_camera/left_ir/image_raw + Value: true + Enabled: true + Name: Stereo Cameras + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 8 + Median window: 5 + Min Value: 0 + Name: Front Depth + Normalize Range: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /front_camera/depth/image_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 8 + Median window: 5 + Min Value: 0 + Name: Left Depth + Normalize Range: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /left_camera/depth/image_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 8 + Median window: 5 + Min Value: 0 + Name: Right Depth + Normalize Range: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /right_camera/depth/image_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 8 + Median window: 5 + Min Value: 0 + Name: Back Depth + Normalize Range: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /rear_camera/depth/image_raw + Value: true + Enabled: true + Name: Depth Images + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.05000000074505806 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0.20000000298023224 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /unsmoothed_plan + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Local Costmap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: false + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Global Costmap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: false + Enabled: true + Name: Navigation + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: false + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/tracking/odometry + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 120; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 3 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.10000000149011612 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/tracking/slam_path + Value: true + Enabled: true + Global Options: + Background Color: 190; 190; 190 + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 25.242799758911133 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -2 + Y: -0.75 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.509796380996704 + Target Frame: base_link + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 3.149994373321533 + Saved: ~ +Window Geometry: + Back Depth: + collapsed: false + Back Stereo Camera: + collapsed: false + Displays: + collapsed: false + Front Depth: + collapsed: false + Front Stereo Camera: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: true + Left Depth: + collapsed: false + Left Stereo Camera: + collapsed: false + QMainWindow State: 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 + Right Depth: + collapsed: false + Right Stereo Camera: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1850 + X: 70 + Y: 27