from pathlib import Path from xml.etree import ElementTree import yaml PACKAGE_ROOT = Path(__file__).resolve().parents[1] def test_fastdds_profile_has_capacity_for_full_resolution_images() -> None: root = ElementTree.parse( PACKAGE_ROOT / "config" / "fastdds_large_images.xml" ).getroot() namespace = {"dds": "http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles"} profiles = root.find("dds:profiles", namespace) assert profiles is not None segment = profiles.find( ".//dds:transport_descriptor[dds:type='SHM']/dds:segment_size", namespace, ) assert segment is not None assert int(segment.text) >= 64 * 1024 * 1024 participant = profiles.find("dds:participant", namespace) assert participant is not None assert participant.attrib["is_default_profile"] == "true" def test_three_camera_launches_force_fastdds_large_image_transport() -> None: for launch_name in ( "three_camera_calibration.launch.py", "three_camera_extrinsics.launch.py", ): launch_text = (PACKAGE_ROOT / "launch" / launch_name).read_text() assert 'name="RMW_IMPLEMENTATION"' in launch_text assert 'value="rmw_fastrtps_cpp"' in launch_text assert 'name="FASTDDS_DEFAULT_PROFILES_FILE"' in launch_text assert 'name="FASTRTPS_DEFAULT_PROFILES_FILE"' in launch_text def test_front_tag_parameters_match_namespaced_detector() -> None: config = yaml.safe_load( (PACKAGE_ROOT / "config" / "front_tags.yaml").read_text() ) detector = config["/apriltag/apriltag"]["ros__parameters"] calibration = config["g20_thumb_calibration"]["ros__parameters"] assert detector["tag"]["ids"] == calibration["tag_ids"] assert detector["tag"]["frames"] == calibration["tag_frames"] assert detector["tag"]["sizes"] == calibration["tag_sizes_m"] assert detector["tag"]["ids"] == [0, 1, 2, 3] assert detector["qos_profile"] == "sensor_data" assert detector["detector"]["decimate"] == 1.5 assert detector["detector"]["refine"] is True assert detector["detector"]["debug"] is False def test_three_camera_tag_ids_and_topics_use_eleven_unique_tags() -> None: tags = yaml.safe_load( (PACKAGE_ROOT / "config" / "three_camera_tags.yaml").read_text() ) expected = { "front": [0, 1, 2, 3, 10], "side": [4, 5, 6, 7], "top": [8, 9], } all_ids = set() for view, ids in expected.items(): key = f"/g20_calibration/{view}/apriltag/apriltag" parameters = tags[key]["ros__parameters"] assert parameters["tag"]["ids"] == ids assert parameters["size"] == 0.016 assert parameters["tag"]["sizes"] == [0.016] * len(ids) assert parameters["qos_profile"] == "sensor_data" assert parameters["detector"]["decimate"] == 1.5 all_ids.update(ids) assert all_ids == set(range(11)) assert tags[ "/g20_calibration/side/apriltag/apriltag" ]["ros__parameters"]["tag"]["frames"][0] == "side_base" def test_right_19_tag_config_matches_the_physical_layout() -> None: tags = yaml.safe_load( ( PACKAGE_ROOT / "config" / "three_camera_tags_g20_right_19.yaml" ).read_text() ) expected = { "front": [0, 1, 2, 3, 10, 11, 12, 13], "side": [4, 5, 6, 7, 14, 15, 16, 17, 18], "top": [8, 9], } all_ids: set[int] = set() for view, ids in expected.items(): parameters = tags[ f"/g20_calibration/{view}/apriltag/apriltag" ]["ros__parameters"] assert parameters["tag"]["ids"] == ids expected_sizes = [0.016 for _tag_id in ids] assert parameters["tag"]["sizes"] == expected_sizes all_ids.update(ids) assert all_ids == set(range(19)) side_frames = tags[ "/g20_calibration/side/apriltag/apriltag" ]["ros__parameters"]["tag"]["frames"] assert side_frames == [ "side_base", "ring_pip", "pinky_pip", "pinky_dip", "ring_dip", "middle_pip", "middle_dip", "index_pip", "index_dip", ] def test_three_camera_calibration_has_hard_preflight_and_three_rounds() -> None: config = yaml.safe_load( (PACKAGE_ROOT / "config" / "three_camera_calibration.yaml").read_text() ) parameters = config["g20_calibration"]["ros__parameters"] assert parameters["tag_size_m"] == 0.016 assert parameters["tag_size_override_ids"] == [7, 14, 16, 18] assert parameters["tag_size_overrides_m"] == [0.016] * 4 assert parameters["baseline_command_u8"] == [ 255, 255, 255, 255, 255, 255, 127, 127, 127, 127, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, ] assert parameters["setting_topic"] == "/g20/cb_hand_setting_cmd" assert parameters["normal_calibration_speed"] == 15 assert parameters["index_roll_calibration_speed"] == 5 assert parameters["index_flex_calibration_speed"] == 10 assert parameters["adaptive_formal_speed_enabled"] is True assert parameters["adaptive_formal_speed_max_scale"] == 1.5 assert parameters["adaptive_formal_speed_minimum_bins"] == 64 assert parameters["adaptive_formal_speed_maximum_bin_gap"] == 8 assert parameters["right_thumb_yaw_255_endpoint_tolerance_u8"] == 5.0 assert parameters["speed_setting_settle_seconds"] >= 0.2 assert parameters["top_pnp_invalid_reset_seconds"] == 1.0 assert parameters["pnp_group_initialization_frames"] == 8 assert parameters["pnp_group_normal_alignment_scale_deg"] == 5.0 assert parameters["pnp_group_maximum_normal_alignment_deg"] == 15.0 assert parameters["thumb_ip_pnp_coupling_multiplier"] == 1.03 assert parameters["thumb_ip_pnp_coupling_scale_deg"] == 3.0 assert parameters["thumb_ip_pnp_maximum_coupling_residual_deg"] == 7.5 assert parameters["baseline_hold_seconds"] == 0.5 assert parameters["minimum_baseline_hold_frames"] == 10 assert parameters["directional_zero_maximum_branch_gap_deg"] == 2.0 assert parameters["directional_zero_maximum_branch_gap_range_deg"] == 0.3 assert ( parameters["cross_view_roll_maximum_branch_gap_difference_deg"] == 0.3 ) assert parameters["cross_view_roll_diagnostic_finger"] == "" assert parameters["repetitions"] == 3 assert parameters["g20_right_19_repetitions"] >= 4 assert parameters["validation_enabled"] is False assert parameters["combination_validation_enabled"] is False assert parameters["combination_validation_frames"] >= 10 assert parameters["combination_maximum_position_p95_m"] <= 0.003 assert parameters["combination_maximum_orientation_p95_deg"] <= 2.0 assert parameters["minimum_detection_rate"] == 0.95 assert parameters["minimum_detection_hz"] == 15.0 assert parameters["minimum_state_span_u8"] >= 240.0 assert parameters["endpoint_tolerance_u8"] == 2.0 assert parameters["thumb_yaw_zero_endpoint_tolerance_u8"] == 4.0 assert parameters["pinky_pip_zero_endpoint_tolerance_u8"] == 5.0 assert parameters["minimum_sweep_bins"] >= 32 assert parameters["maximum_bin_gap"] <= 16 assert parameters["automatic_sweep_retry_limit"] == 2 assert parameters["automatic_fit_retry_limit"] == 2 assert parameters["motor_stall_timeout_seconds"] == 2.0 assert parameters["motor_stall_startup_grace_seconds"] == 1.0 assert parameters["motor_stall_minimum_progress_u8"] == 1.0 assert parameters["automatic_motion_retry_limit"] == 2 assert parameters["provisional_warning_ratio"] == 1.25 assert parameters["retry_speed_scales"] == [0.8, 0.6] assert parameters["retry_endpoint_hold_seconds"] == [0.75, 1.0] assert parameters["position_timeout_seconds"] >= 20.0 assert parameters["maximum_state_image_skew_ms"] <= 50.0 assert parameters["axis_maximum_rotation_circle_difference_deg"] <= 1.0 assert parameters["axis_maximum_pose_line_rms_m"] <= 0.001 assert parameters["passive_axis_maximum_plane_rms_m"] == 0.004 assert parameters["active_maximum_rotation_orthogonal_rms_deg"] == 2.5 assert parameters["passive_maximum_rotation_orthogonal_rms_deg"] == 7.5 assert parameters["zero_maximum_axis_cycle_difference_deg"] <= 0.75 assert parameters["zero_maximum_axis_cone_mismatch_deg"] <= 5.0 assert parameters["zero_maximum_observability_condition_number"] >= 1.0 assert parameters["zero_maximum_offset_deg"] <= 20.0 assert parameters["zero_finger_maximum_offset_deg"] <= 3.0 assert parameters["mechanical_endpoint_maximum_offset_deg"] == 5.0 assert parameters["image_trajectory_maximum_radial_rms_px"] <= 2.0 assert parameters["image_trajectory_maximum_radial_p95_px"] <= 3.5 assert parameters["image_trajectory_minimum_radius_px"] >= 20.0 assert parameters["trajectory_maximum_cycle_travel_difference_deg"] <= 3.0 assert parameters["passive_maximum_cycle_travel_difference_deg"] <= 10.0 assert parameters["passive_maximum_monotonic_correction_deg"] <= 3.0 assert parameters["maximum_hysteresis_deg"] <= 2.0 assert parameters["passive_maximum_hysteresis_deg"] <= 2.0 assert parameters["command_maximum_direction_gap_deg"] <= 2.0 assert parameters["maximum_validation_mae_deg"] <= 1.0 assert parameters["maximum_validation_p95_deg"] <= 2.0 assert parameters["maximum_validation_error_deg"] <= 3.0 assert parameters["zero_maximum_confidence_half_width_deg"] <= 1.5 for view in ("front", "side", "top"): assert parameters[f"{view}_camera_info_topic"].startswith( f"/g20_calibration/{view}/" ) assert parameters[f"{view}_detections_topic"] == ( f"/g20_calibration/{view}/apriltag/detections" ) def test_trial_uses_centre_trajectory_and_thirty_pixel_tags() -> None: config = yaml.safe_load( (PACKAGE_ROOT / "config" / "calibration.yaml").read_text() ) parameters = config["g20_thumb_calibration"]["ros__parameters"] assert parameters["angle_estimation_mode"] == "trajectory_center_3d" assert parameters["passive_ip_multiplier"] == 1.02 assert parameters["pnp_minimum_valid_rate"] == 0.95 assert parameters["pnp_maximum_reprojection_error_px"] <= 1.5 assert parameters["pnp_reprojection_tie_px"] == 1.5 assert parameters["pnp_tracker_reset_seconds"] == 5.0 assert parameters["pnp_group_relative_rotation_scale_deg"] == 5.0 assert parameters["pnp_group_relative_translation_scale_m"] == 0.01 assert parameters["pnp_group_reprojection_weight"] == 0.05 assert parameters["pnp_trajectory_reprojection_scale_px"] == 0.1 assert parameters["pnp_rigid_rotation_scale_deg"] == 5.0 assert parameters["pnp_rigid_p95_accepted_drift_deg"] == 8.0 assert parameters["pnp_rigid_maximum_accepted_drift_deg"] == 15.0 assert ( parameters["pnp_rigid_p95_accepted_distance_drift_m"] <= 0.003 ) assert ( parameters["pnp_rigid_maximum_accepted_distance_drift_m"] <= 0.006 ) assert parameters["pnp_maximum_pose_jump_deg"] <= 35.0 assert parameters["trajectory_maximum_plane_rms_m"] <= 0.004 assert parameters["trajectory_maximum_radial_rms_m"] <= 0.004 assert parameters["trajectory_minimum_radius_m"] >= 0.005 assert parameters["trajectory_minimum_arc_deg"] >= 15.0 assert ( parameters["trajectory_maximum_root_role_disagreement_deg"] <= 5.0 ) assert parameters["trajectory_maximum_anchor_drift_m"] <= 0.005 assert parameters["trajectory_static_translation_outlier_m"] <= 0.005 assert ( parameters["trajectory_maximum_static_translation_rms_m"] <= 0.002 ) assert parameters["minimum_edge_pixels"] == 30.0 assert parameters["maximum_static_std_deg"] == 3.0 assert parameters["minimum_pose_inlier_rate"] == 0.90 assert parameters["repetitions"] == 1 assert parameters["command_step"] == 8 assert parameters["scan_mode"] == "continuous" assert parameters["continuous_motion_mode"] == "endpoint" assert parameters["auto_start_tip"] is True assert parameters["minimum_detection_hz"] == 15.0 assert parameters["stable_frames"] == 5 assert parameters["capture_frames"] == 8 assert parameters["maximum_stable_spread_deg"] == 3.0 assert parameters["maximum_stable_translation_spread_m"] <= 0.003 assert parameters["validation_command_count"] == 5 assert parameters["continuous_minimum_bins"] >= 32 assert parameters["continuous_maximum_bin_gap"] <= 16 assert parameters["continuous_segment_minimum_seconds"] >= 0.1 assert parameters["continuous_segment_timeout_seconds"] >= 5.0 assert parameters["continuous_prepare_timeout_seconds"] >= 30.0 def test_cmc_pitch_zero_config_uses_three_trajectory_circle_rounds() -> None: config = yaml.safe_load( (PACKAGE_ROOT / "config" / "cmc_pitch_zero.yaml").read_text() ) parameters = config["g20_thumb_cmc_pitch_zero"]["ros__parameters"] assert parameters["t0_id"] == 0 assert parameters["t3_id"] == 1 assert parameters["joint_name"] == "thumb_cmc_pitch" assert parameters["motor_index"] == 0 assert parameters["zero_command_u8"] == 255 assert parameters["measure_travel"] is False assert parameters["baseline_command_u8"] == [ 255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255, ] assert parameters["repetitions"] == 3 assert parameters["zero_capture_frames"] == 30 assert parameters["trajectory_command_u8"] <= 64 assert parameters["trajectory_minimum_state_span_u8"] >= 160.0 assert parameters["trajectory_minimum_bins"] >= 18 assert parameters["trajectory_minimum_arc_deg"] >= 20.0 assert parameters["trajectory_maximum_radial_rms_px"] <= 2.0 assert parameters["trajectory_maximum_p95_radial_error_px"] <= 3.5 assert parameters["minimum_detection_rate"] == 0.95 assert parameters["minimum_edge_pixels"] == 40.0 assert parameters["maximum_static_position_rms_px"] <= 1.5 assert "maximum_axis_alignment_deg" not in parameters assert "maximum_static_std_deg" not in parameters assert "maximum_anchor_drift_deg" not in parameters assert parameters["camera_alignment_enabled"] is True assert parameters["camera_alignment_max_angle_deg"] <= 0.5 assert parameters["camera_alignment_max_vertical_offset_px"] <= 12.0 assert parameters["camera_alignment_required_frames"] >= 10 assert parameters["camera_alignment_minimum_detection_rate"] <= 0.8 def test_cmc_roll_config_measures_full_endpoint_travel() -> None: config = yaml.safe_load( (PACKAGE_ROOT / "config" / "cmc_roll_zero_travel.yaml").read_text() ) parameters = config[ "g20_thumb_cmc_roll_calibration" ]["ros__parameters"] assert parameters["t0_id"] == 0 assert parameters["t3_id"] == 1 assert parameters["joint_name"] == "thumb_cmc_roll" assert parameters["motor_index"] == 5 assert parameters["zero_command_u8"] == 255 assert parameters["trajectory_command_u8"] == 0 assert parameters["measure_travel"] is True assert parameters["baseline_command_u8"][5] == 255 assert parameters["repetitions"] == 3 assert parameters["zero_capture_frames"] == 30 assert parameters["trajectory_minimum_state_span_u8"] >= 240.0 assert parameters["trajectory_minimum_bins"] >= 30 assert parameters["minimum_travel_deg"] >= 20.0 assert parameters["maximum_travel_difference_deg"] <= 1.0 assert parameters["maximum_round_difference_deg"] == 1.0 assert "maximum_axis_alignment_deg" not in parameters assert "maximum_static_std_deg" not in parameters assert "maximum_anchor_drift_deg" not in parameters assert parameters["camera_alignment_enabled"] is True assert parameters["camera_alignment_max_angle_deg"] <= 0.5 assert parameters["camera_alignment_max_vertical_offset_px"] <= 12.0 assert parameters["camera_alignment_required_frames"] >= 10 assert parameters["camera_alignment_minimum_detection_rate"] <= 0.8