463 lines
20 KiB
Python
463 lines
20 KiB
Python
"""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),
|
|
]
|
|
)
|