diff --git a/isaac_orbbec_launch/config/matrices_SN1423724335594.yaml b/isaac_orbbec_launch/config/matrices_SN1423724335594.yaml new file mode 100644 index 00000000..ab00e874 --- /dev/null +++ b/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] diff --git a/isaac_orbbec_launch/launch/base_static_transforms_publisher.py b/isaac_orbbec_launch/launch/base_static_transforms_publisher.py new file mode 100644 index 00000000..ac5e4f86 --- /dev/null +++ b/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 ") + sys.exit(1) + + yaml_path = sys.argv[1] + + rclpy.init() + node = StaticTransformsPublisher(yaml_path) + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/isaac_orbbec_launch/launch/orbbec_perceptor.launch.py b/isaac_orbbec_launch/launch/orbbec_perceptor.launch.py index f00f1e40..0af0c96e 100644 --- a/isaac_orbbec_launch/launch/orbbec_perceptor.launch.py +++ b/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', diff --git a/isaac_orbbec_launch/param/orbbec_perceptor_detached.yaml b/isaac_orbbec_launch/param/orbbec_perceptor_detached.yaml new file mode 100644 index 00000000..b2303fce --- /dev/null +++ b/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'