381 lines
16 KiB
Python
381 lines
16 KiB
Python
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
|