"""Launch Profile-declared Hikrobot views and one calibration owner.""" from __future__ import annotations from datetime import datetime import hashlib from pathlib import Path import re from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, ExecuteProcess, LogInfo, OpaqueFunction, SetEnvironmentVariable, ) from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode from launch_ros.parameter_descriptions import ParameterValue def _launch_stack(context): from linkerhand_calibration.product import ( ProductCalibrationContract, ) from linkerhand_calibration.profiles import load_hand_profile from linkerhand_calibration.runtime.adapters.ros_topics import sdk_topics from linkerhand_calibration.runtime.diagnostic_capture import resolve_diagnostic_capture from linkerhand_calibration.runtime.observation_scope import required_observation_views model = LaunchConfiguration("model").perform(context).strip().upper() hand_type = LaunchConfiguration("hand_type").perform(context).lower() if hand_type not in {"left", "right"}: raise RuntimeError("hand_type must be left or right") tag_layout = LaunchConfiguration("tag_layout").perform(context).lower() try: profile_path = LaunchConfiguration("profile_config").perform(context).strip() if not profile_path: raise ValueError("online calibration requires a protected YAML Profile; use calibrate_hand --config") expected = LaunchConfiguration("profile_config_expected_sha256").perform(context).strip() if not expected or hashlib.sha256(Path(profile_path).read_bytes()).hexdigest() != expected: raise ValueError("Profile changed between product validation and launch") contract = ProductCalibrationContract(declarative=load_hand_profile(profile_path)) key = contract.typed_profile.key if (key.model, key.side, key.layout) != (model, hand_type, tag_layout): raise ValueError("launch identity differs from the protected Profile") except ValueError as error: raise RuntimeError(str(error)) from error diagnostic = resolve_diagnostic_capture( contract.typed_profile, LaunchConfiguration("diagnostic_capture").perform(context).strip(), ) views = required_observation_views(contract.typed_profile, diagnostic) requested_tag_config = LaunchConfiguration("tag_config").perform(context) if not requested_tag_config: raise RuntimeError("tag_config is required; use calibrate_hand --config") tag_config = Path(requested_tag_config).expanduser().resolve() if not tag_config.is_file(): raise RuntimeError(f"tag config does not exist: {tag_config}") topic_prefix = f"/{model.lower()}" uses_hcan = contract.typed_profile.sdk_adapter == "o12_hcan_sdk" if contract.typed_profile.sdk_adapter not in {"legacy_byte_sdk", "o12_hcan_sdk"}: raise RuntimeError("SDK adapter has no ROS launch binding") topics = sdk_topics(contract.typed_profile) command_topic, state_topic = topics.command, topics.feedback requested_source = LaunchConfiguration("source_urdf_path").perform(context) if not requested_source: raise RuntimeError("source_urdf_path is required; use calibrate_hand --config") source_urdf = Path(requested_source).expanduser().resolve() if not source_urdf.is_file(): raise RuntimeError(f"source URDF does not exist: {source_urdf}") expected_source_hash = LaunchConfiguration( "source_urdf_expected_sha256" ).perform(context).strip().lower() if contract.typed_profile.artifacts.publish_corrected_urdf: if re.fullmatch(r"[0-9a-f]{64}", expected_source_hash) is None: raise RuntimeError( "this profile requires source_urdf_expected_sha256 confirmed " "by the CAD/hardware owner" ) actual_source_hash = hashlib.sha256(source_urdf.read_bytes()).hexdigest() if actual_source_hash != expected_source_hash: raise RuntimeError( "source_urdf_expected_sha256 does not match source_urdf_path" ) hand_serial = LaunchConfiguration("serial_number").perform(context) if ( not hand_serial or hand_serial == "UNSET" or re.fullmatch(r"[A-Za-z0-9_.-]+", hand_serial) is None or hand_serial in {".", ".."} ): raise RuntimeError("serial_number must be a safe non-empty hand serial") requested_session = LaunchConfiguration("session_dir").perform(context) output_root = Path( LaunchConfiguration("output_root").perform(context) ).expanduser().resolve() if requested_session: session_dir = Path(requested_session).expanduser().resolve() else: session_dir = ( output_root / hand_serial / datetime.now().strftime("%Y%m%d_%H%M%S") ) session_dir.mkdir(parents=True, exist_ok=True) camera_serials = { view: LaunchConfiguration(f"{view}_camera_serial").perform(context) for view in views } if any(not serial for serial in camera_serials.values()): raise RuntimeError("all required observation camera serial numbers are required") if len(set(camera_serials.values())) != len(views): raise RuntimeError("required observation camera serial numbers must be unique") cameras = [] components = [] raw_topics = [] info_topics = [] detection_topics = [] calibration_namespace = contract.typed_profile.namespace for view in views: namespace = f"{calibration_namespace}/{view}/camera" raw_topic = f"{namespace}/image_raw" info_topic = f"{namespace}/camera_info" rect_topic = f"{namespace}/image_rect" detector_namespace = f"{calibration_namespace}/{view}/apriltag" detection_topic = f"{detector_namespace}/detections" raw_topics.append(raw_topic) info_topics.append(info_topic) detection_topics.append(detection_topic) cameras.append( Node( package="linkerhand_calibration", executable="hikrobot_camera_node", name="hikrobot_camera", namespace=namespace, output="screen", emulate_tty=True, condition=IfCondition(LaunchConfiguration("start_cameras")), parameters=[ { "serial_number": LaunchConfiguration( f"{view}_camera_serial" ), "expected_model": LaunchConfiguration("camera_model"), "camera_name": LaunchConfiguration( f"{view}_camera_name" ), "frame_id": ( f"{model.lower()}_calibration_{view}_optical_frame" ), "image_width": 1624, "image_height": 1240, "timestamp_journal_path": str(Path(LaunchConfiguration("session_dir").perform(context)) / f"camera_timing_{view}.jsonl"), "frame_rate": ParameterValue( LaunchConfiguration("camera_frame_rate"), value_type=float, ), "exposure_time_us": ParameterValue( LaunchConfiguration("exposure_time_us"), value_type=float, ), "gain_db": ParameterValue( LaunchConfiguration("gain_db"), value_type=float ), "auto_exposure": ParameterValue( LaunchConfiguration("auto_exposure"), value_type=bool ), "camera_info_url": LaunchConfiguration( f"{view}_camera_info_url" ), } ], ) ) components.extend( [ ComposableNode( package="image_proc", plugin="image_proc::RectifyNode", name=f"rectify_{view}", namespace=namespace, remappings=[ ("image", raw_topic), ("camera_info", info_topic), ("image_rect", rect_topic), ], parameters=[{"queue_size": 1}], extra_arguments=[{"use_intra_process_comms": True}], ), ComposableNode( package="apriltag_ros", plugin="AprilTagNode", name="apriltag", namespace=detector_namespace, # Detector settings belong to the protected model YAML. # A generic launch default must not silently replace its # full-resolution setting for the small calibration Tags. parameters=[str(tag_config)], remappings=[ ("image_rect", rect_topic), ("camera_info", info_topic), ], extra_arguments=[{"use_intra_process_comms": True}], ), ] ) vision = ComposableNodeContainer( name=f"{model.lower()}_calibration_vision", namespace="/", package="rclcpp_components", executable="component_container_mt", composable_node_descriptions=components, output="screen", emulate_tty=True, ) sdk = ( Node( package="linkerhand_calibration", executable="o12_sdk_bridge", name="o12_sdk_bridge", output="screen", condition=IfCondition(LaunchConfiguration("start_sdk")), parameters=[{ "vendor_config": LaunchConfiguration("vendor_sdk_config"), "vendor_config_sha256": LaunchConfiguration("sdk_config_expected_sha256"), "vendor_python_package": LaunchConfiguration("vendor_sdk_python_package"), "vendor_package_sha256": LaunchConfiguration("sdk_package_expected_sha256"), "hand_type": hand_type, "topic_prefix": f"/{model.lower()}/{hand_type}", }], ) if uses_hcan else Node( package="linker_hand_ros2_sdk", executable="linker_hand_sdk", name="linker_hand_sdk", output="screen", condition=IfCondition(LaunchConfiguration("start_sdk")), parameters=[{ "hand_type": hand_type, "hand_joint": model, "can": LaunchConfiguration("can_interface"), "modbus": "None", "topic_prefix": topic_prefix, "move_on_startup": False, "startup_speed": ParameterValue( LaunchConfiguration("calibration_speed"), value_type=int ), "startup_torque": 80, # Match 30 Hz cameras so state/image p95 skew stays below 50 ms. "state_poll_rate": 30.0, # Calibration does not consume measured joint velocity. A # G20 velocity read sends another five synchronous CAN # queries, so keep it off the trajectory-critical path. "velocity_poll_rate": 1.0, # G20 sends an endpoint and L6 streams a bounded trajectory. # Keep polling the real motor state during either command path; # otherwise the SDK republishes stale state and creates large # command-unit holes in the trajectory bins. "defer_state_reads_while_commanding": False, "repeat_position_commands": False, "is_touch": False, }], ) ) calibration = Node( package="linkerhand_calibration", executable="three_camera_calibration_node", name=f"{model.lower()}_calibration", output="screen", emulate_tty=True, arguments=[ "--profile-id", contract.typed_profile.key.profile_id, "--profile-config", profile_path, "--profile-sha256", expected, ], parameters=[ LaunchConfiguration("calibration_config"), { "serial_number": hand_serial, "session_dir": str(session_dir), "resume_raw_samples_path": LaunchConfiguration( "resume_raw_samples_path" ), "command_topic": command_topic, "state_topic": state_topic, "camera_extrinsics_file": LaunchConfiguration( "camera_extrinsics_file" ), "source_urdf_path": str(source_urdf), "source_urdf_expected_sha256": LaunchConfiguration( "source_urdf_expected_sha256" ), "camera_extrinsics_expected_sha256": LaunchConfiguration( "camera_extrinsics_expected_sha256" ), "calibration_config_expected_sha256": LaunchConfiguration( "calibration_config_expected_sha256" ), "tag_config_expected_sha256": LaunchConfiguration( "tag_config_expected_sha256" ), "sdk_config_expected_sha256": LaunchConfiguration( "sdk_config_expected_sha256" ), "sdk_package_expected_sha256": LaunchConfiguration("sdk_package_expected_sha256"), "profile_config_expected_sha256": LaunchConfiguration( "profile_config_expected_sha256" ), "commands_enabled": ParameterValue( LaunchConfiguration("commands_enabled"), value_type=bool ), "diagnostic_capture": ParameterValue( LaunchConfiguration("diagnostic_capture"), value_type=str ), }, ], ) bag = ExecuteProcess( condition=IfCondition(LaunchConfiguration("record_bag")), cmd=[ "ros2", "bag", "record", "--storage", "mcap", "--storage-preset-profile", "zstd_fast", "--max-bag-size", "10737418240", "--output", str(session_dir / "rosbag"), *raw_topics, *info_topics, *detection_topics, command_topic, state_topic, *( [ f"/{model.lower()}/{hand_type}/calibration_health", ] if uses_hcan else [] ), f"{calibration_namespace}/status", ], output="screen", ) return [ LogInfo( msg=( f"{model} {hand_type} {tag_layout} calibration session: {session_dir}; " f"source_urdf={source_urdf}" ) ), LogInfo( msg=( "Camera mapping: " + " ".join(f"{view}={serial}" for view, serial in camera_serials.items()) ) ), *cameras, vision, Node(package="linkerhand_calibration", executable="tag_border_filter_node", name="tag_border_filter", output="screen", arguments=["--profile-config", profile_path, "--profile-sha256", expected, "--views", *views]), sdk, calibration, bag, ] def generate_launch_description() -> LaunchDescription: package_share = Path( get_package_share_directory("linkerhand_calibration") ) return LaunchDescription( [ # Camera processes publish ~2 MB frames across DDS. Force the # matching RMW and provide both current and legacy profile names # so the configured 64 MB shared-memory segment is actually used. SetEnvironmentVariable( name="RMW_IMPLEMENTATION", value="rmw_fastrtps_cpp", ), SetEnvironmentVariable( name="FASTDDS_DEFAULT_PROFILES_FILE", value=str(package_share / "config" / "fastdds_large_images.xml"), ), SetEnvironmentVariable( name="FASTRTPS_DEFAULT_PROFILES_FILE", value=str(package_share / "config" / "fastdds_large_images.xml"), ), DeclareLaunchArgument("model", default_value=""), DeclareLaunchArgument("hand_type", default_value=""), DeclareLaunchArgument("tag_layout", default_value=""), DeclareLaunchArgument("serial_number", default_value="UNSET"), DeclareLaunchArgument("camera_model", default_value="MV-CS020-10U"), DeclareLaunchArgument("camera_frame_rate", default_value="30.0"), DeclareLaunchArgument("exposure_time_us", default_value="5000.0"), DeclareLaunchArgument("gain_db", default_value="0.0"), DeclareLaunchArgument("auto_exposure", default_value="false"), DeclareLaunchArgument("can_interface", default_value="can0"), DeclareLaunchArgument("calibration_speed", default_value="15"), DeclareLaunchArgument("camera_extrinsics_file", default_value=""), DeclareLaunchArgument( "source_urdf_path", default_value="" ), DeclareLaunchArgument( "source_urdf_expected_sha256", default_value="" ), DeclareLaunchArgument( "camera_extrinsics_expected_sha256", default_value="" ), DeclareLaunchArgument( "calibration_config_expected_sha256", default_value="" ), DeclareLaunchArgument( "tag_config_expected_sha256", default_value="" ), DeclareLaunchArgument("vendor_sdk_config", default_value=""), DeclareLaunchArgument("vendor_sdk_python_package", default_value=""), DeclareLaunchArgument("sdk_package_expected_sha256", default_value=""), DeclareLaunchArgument( "sdk_config_expected_sha256", default_value="" ), DeclareLaunchArgument( "profile_config_expected_sha256", default_value="" ), DeclareLaunchArgument("profile_config", default_value=""), DeclareLaunchArgument("commands_enabled", default_value="true"), DeclareLaunchArgument("diagnostic_capture", default_value=""), DeclareLaunchArgument("start_cameras", default_value="true"), DeclareLaunchArgument("start_sdk", default_value="true"), DeclareLaunchArgument("record_bag", default_value="false"), DeclareLaunchArgument( "output_root", default_value=str(Path.cwd() / "calibration_output"), ), DeclareLaunchArgument("session_dir", default_value=""), DeclareLaunchArgument("resume_raw_samples_path", default_value=""), DeclareLaunchArgument( "calibration_config", default_value="", ), DeclareLaunchArgument( "tag_config", default_value="", ), OpaqueFunction(function=_launch_stack), ] )