add TF publish script and config files

isaac_orbbec_launch/config/matrices_SN1423724335594.yaml

@@ -0,0 +1,18 @@
+front_camera_link_to_left_camera_link:
+  matrix:
+    - [-0.0008235254793943609, -0.003740061191919857, -0.9999921848597361, -34.34830241206723]
+    - [-0.004011672105301799, 0.9999850279707627, -0.003736726043017259, 0.5572258054318848]
+    - [0.9999920390889445, 0.004008566327518529, -0.0008385146904865293, -128.5921189832329]
+    - [0, 0, 0, 1]
+front_camera_link_to_right_camera_link:
+  matrix:
+    - [-0.001192666126293565, -0.009771139382170269, 0.9999510744320814, 130.6339284605396]
+    - [-0.002746366386210473, 0.9999490311697361, 0.009767836599264891, 0.9694098533653659]
+    - [-0.9999955903520988, -0.002734583264504063, -0.001219442429078465, -33.01187382522542]
+    - [0, 0, 0, 1]
+front_camera_link_to_rear_camera_link:
+  matrix:
+    - [-0.999985527795709, 0.0004374834173236661, -0.005409847645595232, 95.08087881303337]
+    - [0.0004633793403886539, 0.999988185039409, -0.00478650799368135, 0.5968307744886588]
+    - [0.005407697890701878, -0.004788938815872843, -0.9999733753620403, -165.248805264056]
+    - [0, 0, 0, 1]

isaac_orbbec_launch/launch/base_static_transforms_publisher.py

@@ -0,0 +1,98 @@
+import rclpy
+from rclpy.node import Node
+from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
+from geometry_msgs.msg import TransformStamped, Quaternion
+import numpy as np
+import yaml
+from scipy.spatial.transform import Rotation as R
+import sys
+import os
+
+def rotation_matrix_to_quaternion(rotation_matrix):
+    r = R.from_matrix(rotation_matrix)
+    q = r.as_quat()
+    return Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
+
+def create_transform(matrix, parent_frame, child_frame):
+    t = TransformStamped()
+    t.header.stamp = rclpy.time.Time().to_msg()
+    t.header.frame_id = parent_frame
+    t.child_frame_id = child_frame
+
+    # Convert translation from mm to m
+    t.transform.translation.x = matrix[0, 3] / 1000.0
+    t.transform.translation.y = matrix[1, 3] / 1000.0
+    t.transform.translation.z = matrix[2, 3] / 1000.0
+
+    rotation_matrix = matrix[:3, :3]
+    q = rotation_matrix_to_quaternion(rotation_matrix)
+
+    t.transform.rotation = q
+
+    return t
+
+def convert_optical_to_vehicle_frame(optical_transform):
+    # Conversion matrix from optical frame to vehicle frame
+    conversion_matrix = np.array([
+        [ 0,  0,  1,  0],
+        [-1,  0,  0,  0],
+        [ 0, -1,  0,  0],
+        [ 0,  0,  0,  1]
+    ])
+
+    return np.linalg.multi_dot([conversion_matrix, optical_transform, np.linalg.inv(conversion_matrix)])
+
+def load_yaml_to_matrices(file_path):
+    if not os.path.exists(file_path):
+        raise FileNotFoundError(f"YAML file not found: {file_path}")
+    with open(file_path, 'r') as file:
+        data = yaml.safe_load(file)
+        matrices = {
+            key: np.array(value['matrix'])
+            for key, value in data.items()
+        }
+    return matrices
+
+class StaticTransformsPublisher(Node):
+    def __init__(self, yaml_path):
+        super().__init__('static_transforms_publisher')
+        self._broadcaster = StaticTransformBroadcaster(self)
+
+        # Load optical frame matrices from YAML file
+        optical_matrices = load_yaml_to_matrices(yaml_path)
+
+        # Identity transforms for other frames
+        map_to_odom = np.eye(4)  # Assuming identity (no translation, no rotation)
+        odom_to_base_link = np.eye(4)  # Assuming identity (no translation, no rotation)
+        base_link_to_front_camera = np.eye(4)  # Assuming identity (no translation, no rotation)
+
+        # Create and send static transform from 'odom' to 'base_link'
+        # Create and send static transform from 'base_link' to 'front_camera_link'
+        transforms = []
+        transforms.append(create_transform(map_to_odom, 'map', 'odom'))
+        transforms.append(create_transform(odom_to_base_link, 'odom', 'base_link'))
+        transforms.append(create_transform(base_link_to_front_camera, 'base_link', 'front_camera_link'))
+
+        # Convert optical frame matrices to vehicle frame
+        vehicle_matrices = {k: convert_optical_to_vehicle_frame(v) for k, v in optical_matrices.items()}
+
+        transforms.append(create_transform(vehicle_matrices['front_camera_link_to_left_camera_link'], 'front_camera_link', 'left_camera_link'))
+        transforms.append(create_transform(vehicle_matrices['front_camera_link_to_right_camera_link'], 'front_camera_link', 'right_camera_link'))
+        transforms.append(create_transform(vehicle_matrices['front_camera_link_to_rear_camera_link'], 'front_camera_link', 'rear_camera_link'))
+
+        self._broadcaster.sendTransform(transforms)
+
+def main():
+    if len(sys.argv) < 2:
+        print("Usage: python static_transforms_publisher.py <path_to_yaml>")
+        sys.exit(1)
+
+    yaml_path = sys.argv[1]
+
+    rclpy.init()
+    node = StaticTransformsPublisher(yaml_path)
+    rclpy.spin(node)
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

isaac_orbbec_launch/launch/orbbec_perceptor.launch.py

@@ -15,7 +15,7 @@ def generate_launch_description():
     perceptor_bringup_dir = get_package_share_directory('isaac_ros_perceptor_bringup')
 
     perceptor_config_file = DeclareLaunchArgument(
-        'perceptor_config_file', default_value='params/orbbec_perceptor_detached.yaml',
+        'perceptor_config_file', default_value='param/orbbec_perceptor_detached.yaml',
         description="Perceptor configuration")
     from_bag_arg = DeclareLaunchArgument(
         'from_bag', default_value='False',

isaac_orbbec_launch/param/orbbec_perceptor_detached.yaml

@@ -0,0 +1,101 @@
+nvblox_config:
+  container_name: 'nvblox_container'
+  attach_to_container: false
+  # container_name: 'shared_orbbec_container'
+  # attach_to_container: true
+  node_name: 'nvblox_node'
+  remappings:
+    - depth:
+        image: '/front_camera/depth/image_raw'
+        info: '/front_camera/depth/camera_info'
+      color:
+        image: '/front_camera/color/image_raw'
+        info: '/front_camera/color/camera_info'
+    - depth:
+        image: '/left_camera/depth/image_raw'
+        info: '/left_camera/depth/camera_info'
+      color:
+        image: '/left_camera/color/image_raw'
+        info: '/left_camera/color/camera_info'
+    - depth:
+        image: '/right_camera/depth/image_raw'
+        info: '/right_camera/depth/camera_info'
+      color:
+        image: '/right_camera/color/image_raw'
+        info: '/right_camera/color/camera_info'
+    - depth:
+        image: '/rear_camera/depth/image_raw'
+        info: '/rear_camera/depth/camera_info'
+      color:
+        image: '/rear_camera/color/image_raw'
+        info: '/rear_camera/color/camera_info'
+  config_files:
+    - package: 'isaac_ros_perceptor_bringup'
+      path: 'params/default_nvblox_config.yaml'
+  parameters:
+    - use_lidar: false
+    - input_qos: "SENSOR_DATA"
+    - map_clearing_frame_id: "camera_link"
+    - static_mapper:
+        esdf_slice_height: 0.0
+        esdf_slice_min_height: 0.09
+        esdf_slice_max_height: 0.65
+    - dynamic_mapper:
+        esdf_slice_height: 0.0
+        esdf_slice_min_height: 0.09
+        esdf_slice_max_height: 0.65
+
+cuvslam_config:
+  container_name: 'cuvslam_container'
+  attach_to_container: false
+  # container_name: 'shared_orbbec_container'
+  # attach_to_container: true
+  node_name: 'cuvslam_node'
+  remappings:
+    stereo_images:
+      - left:
+          image: '/front_camera/left_ir/image_raw'
+          info: '/front_camera/left_ir/camera_info'
+          optical_frame: 'front_camera_left_ir_optical_frame'
+        right:
+          image: '/front_camera/right_ir/image_raw'
+          info: '/front_camera/right_ir/camera_info'
+          optical_frame: 'front_camera_right_ir_optical_frame'
+      - left:
+          image: '/rear_camera/left_ir/image_raw'
+          info: '/rear_camera/left_ir/camera_info'
+          optical_frame: 'rear_camera_left_ir_optical_frame'
+        right:
+          image: '/rear_camera/right_ir/image_raw'
+          info: '/rear_camera/right_ir/camera_info'
+          optical_frame: 'rear_camera_right_ir_optical_frame'
+      - left:
+          image: '/left_camera/left_ir/image_raw'
+          info: '/left_camera/left_ir/camera_info'
+          optical_frame: 'left_camera_left_ir_optical_frame'
+        right:
+          image: '/left_camera/right_ir/image_raw'
+          info: '/left_camera/right_ir/camera_info'
+          optical_frame: 'left_camera_right_ir_optical_frame'
+      - left:
+          image: '/right_camera/left_ir/image_raw'
+          info: '/right_camera/left_ir/camera_info'
+          optical_frame: 'right_camera_left_ir_optical_frame'
+        right:
+          image: '/right_camera/right_ir/image_raw'
+          info: '/right_camera/right_ir/camera_info'
+          optical_frame: 'right_camera_right_ir_optical_frame'
+  config_files:
+    - package: 'isaac_ros_perceptor_bringup'
+      path: 'params/default_cuvslam_config.yaml'
+  parameters:
+    - image_jitter_threshold_ms: 67.0
+    - input_qos: "SENSOR_DATA"
+
+common_config:
+  map_frame: 'map'
+  odom_frame: 'odom'
+  robot_frame: 'base_link'
+
+extra_topics:
+  - '/tf_static'