"""Launch the complete front-camera G20 thumb calibration stack.""" from __future__ import annotations from datetime import datetime 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): serial_number = LaunchConfiguration("serial_number").perform(context) if not serial_number or serial_number == "UNSET": raise RuntimeError( "serial_number is required, for example serial_number:=G20_LEFT_001" ) if ( re.fullmatch(r"[A-Za-z0-9_.-]+", serial_number) is None or serial_number in {".", ".."} ): raise RuntimeError( "serial_number may contain only letters, digits, dot, underscore and dash" ) requested_session = LaunchConfiguration("session_dir").perform(context) output_root = Path(LaunchConfiguration("output_root").perform(context)).resolve() if requested_session: session_dir = Path(requested_session).expanduser().resolve() else: timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") session_dir = output_root / serial_number / timestamp session_dir.mkdir(parents=True, exist_ok=True) bag_path = session_dir / "rosbag" calibration_config = LaunchConfiguration("calibration_config").perform(context) tag_config = LaunchConfiguration("tag_config").perform(context) use_roi_text = LaunchConfiguration("use_roi").perform(context).strip().lower() if use_roi_text not in {"true", "false"}: raise RuntimeError("use_roi must be true or false") use_roi = use_roi_text == "true" roi_values = {} for name in ("roi_x", "roi_y", "roi_width", "roi_height"): text = LaunchConfiguration(name).perform(context) try: roi_values[name] = int(text) except ValueError as error: raise RuntimeError(f"{name} must be an integer") from error if roi_values["roi_x"] < 0 or roi_values["roi_y"] < 0: raise RuntimeError("roi_x and roi_y must be non-negative") if roi_values["roi_width"] <= 0 or roi_values["roi_height"] <= 0: raise RuntimeError("roi_width and roi_height must be positive") try: image_width = int(LaunchConfiguration("image_width").perform(context)) image_height = int(LaunchConfiguration("image_height").perform(context)) except ValueError as error: raise RuntimeError("image_width and image_height must be integers") from error if image_width <= 0 or image_height <= 0: raise RuntimeError("image_width and image_height must be positive") if use_roi: if ( roi_values["roi_x"] + roi_values["roi_width"] > image_width or roi_values["roi_y"] + roi_values["roi_height"] > image_height ): raise RuntimeError( "ROI lies outside camera image " f"{image_width}x{image_height}" ) camera = Node( package="linkerhand_calibration", executable="hikrobot_camera_node", name="hikrobot_camera", namespace="/camera/camera/color", output="screen", emulate_tty=True, condition=IfCondition(LaunchConfiguration("start_camera")), parameters=[ { "serial_number": LaunchConfiguration("camera_serial_number"), "expected_model": LaunchConfiguration("camera_model"), "camera_name": LaunchConfiguration("camera_name"), "frame_id": LaunchConfiguration("camera_frame_id"), "image_width": ParameterValue( LaunchConfiguration("image_width"), value_type=int ), "image_height": ParameterValue( LaunchConfiguration("image_height"), value_type=int ), "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("camera_info_url"), } ], ) vision_components = [] if use_roi: processed_image_raw_topic = "/g20_thumb_roi/image_raw" processed_camera_info_topic = "/g20_thumb_roi/camera_info" processed_image_rect_topic = "/g20_thumb_roi/image_rect" vision_components.append( ComposableNode( package="image_proc", plugin="image_proc::CropDecimateNode", name="crop_color_roi", namespace="/g20_thumb_roi", remappings=[ ("in/image_raw", "/camera/camera/color/image_raw"), ("in/camera_info", "/camera/camera/color/camera_info"), ("out/image_raw", processed_image_raw_topic), ("out/camera_info", processed_camera_info_topic), ], parameters=[ { "queue_size": 5, "decimation_x": 1, "decimation_y": 1, "offset_x": roi_values["roi_x"], "offset_y": roi_values["roi_y"], "width": roi_values["roi_width"], "height": roi_values["roi_height"], } ], extra_arguments=[{"use_intra_process_comms": True}], ) ) rectifier_namespace = "/g20_thumb_roi" rectifier_name = "rectify_color_roi" else: processed_image_raw_topic = "/camera/camera/color/image_raw" processed_camera_info_topic = "/camera/camera/color/camera_info" processed_image_rect_topic = "/camera/camera/color/image_rect" rectifier_namespace = "/camera/camera/color" rectifier_name = "rectify_color" vision_components.append( ComposableNode( package="image_proc", plugin="image_proc::RectifyNode", name=rectifier_name, namespace=rectifier_namespace, remappings=[ ("image", processed_image_raw_topic), ("camera_info", processed_camera_info_topic), ("image_rect", processed_image_rect_topic), ], parameters=[{"queue_size": 1}], extra_arguments=[{"use_intra_process_comms": True}], ) ) vision_components.append( ComposableNode( package="apriltag_ros", plugin="AprilTagNode", name="apriltag", namespace="/apriltag", parameters=[ tag_config, { "detector.decimate": ParameterValue( LaunchConfiguration("apriltag_decimate"), value_type=float, ) }, ], remappings=[ ("image_rect", processed_image_rect_topic), ("camera_info", processed_camera_info_topic), ], extra_arguments=[{"use_intra_process_comms": True}], ) ) vision_container = ComposableNodeContainer( name="g20_thumb_vision_container", namespace="/", package="rclcpp_components", executable="component_container_mt", composable_node_descriptions=vision_components, output="screen", emulate_tty=True, ) sdk = Node( package="linker_hand_ros2_sdk", executable="linker_hand_sdk", name="linker_hand_sdk", output="screen", condition=IfCondition(LaunchConfiguration("start_sdk")), parameters=[ { "hand_type": "left", "hand_joint": "G20", "can": LaunchConfiguration("can_interface"), "modbus": "None", "topic_prefix": "/g20", "move_on_startup": False, "startup_speed": ParameterValue( LaunchConfiguration("calibration_speed"), value_type=int, ), "startup_torque": 80, "state_poll_rate": 10.0, "repeat_position_commands": False, "is_touch": False, } ], ) calibration = Node( package="linkerhand_calibration", executable="calibration_node", name="g20_thumb_calibration", output="screen", parameters=[ calibration_config, tag_config, { "serial_number": serial_number, "session_dir": str(session_dir), "commands_enabled": LaunchConfiguration("commands_enabled"), "calibration_speed": ParameterValue( LaunchConfiguration("calibration_speed"), value_type=int, ), "continuous_motion_mode": ParameterValue( LaunchConfiguration("continuous_motion_mode"), value_type=str, ), "angle_estimation_mode": ParameterValue( LaunchConfiguration("angle_estimation_mode"), value_type=str, ), "camera_serial_number": LaunchConfiguration("camera_serial_number"), "rosbag_path": str(bag_path), "camera_info_topic": processed_camera_info_topic, "image_topic": processed_image_rect_topic, "publish_debug_image": LaunchConfiguration( "publish_debug_image" ), }, ], ) bag = ExecuteProcess( condition=IfCondition(LaunchConfiguration("record_bag")), cmd=[ "ros2", "bag", "record", "--storage", "mcap", "--storage-preset-profile", "zstd_fast", "--max-bag-size", "10737418240", "--output", str(bag_path), processed_image_raw_topic, processed_camera_info_topic, "/apriltag/detections", "/tf", "/g20/cb_left_hand_control_cmd", "/g20/cb_left_hand_state", "/g20/cb_left_hand_info", "/g20_thumb_calibration/status", ], output="screen", ) actions = [ LogInfo(msg=f"G20 thumb calibration session: {session_dir}"), LogInfo( msg=( "G20 thumb image ROI: " f"x={roi_values['roi_x']}, y={roi_values['roi_y']}, " f"width={roi_values['roi_width']}, " f"height={roi_values['roi_height']}" if use_roi else "G20 thumb image ROI: disabled" ) ), camera, vision_container, ] actions.extend([sdk, calibration, bag]) return actions def generate_launch_description() -> LaunchDescription: package_share = Path( get_package_share_directory("linkerhand_calibration") ) default_output = str(Path.cwd() / "calibration_output") return LaunchDescription( [ SetEnvironmentVariable( name="FASTRTPS_DEFAULT_PROFILES_FILE", value=str( package_share / "config" / "fastdds_large_images.xml" ), ), DeclareLaunchArgument("serial_number", default_value="UNSET"), DeclareLaunchArgument( "camera_serial_number", default_value="DB2163742" ), DeclareLaunchArgument( "camera_model", default_value="MV-CS020-10UM" ), DeclareLaunchArgument( "camera_name", default_value="hikrobot_front_DB2163742" ), DeclareLaunchArgument( "camera_frame_id", default_value="camera_color_optical_frame" ), DeclareLaunchArgument("image_width", default_value="1624"), DeclareLaunchArgument("image_height", default_value="1240"), 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( "camera_info_url", default_value=str( Path.home() / ".ros" / "camera_info" / "hikrobot_DB2163742.yaml" ), ), DeclareLaunchArgument( "publish_debug_image", default_value="false" ), DeclareLaunchArgument("use_roi", default_value="false"), DeclareLaunchArgument("roi_x", default_value="128"), DeclareLaunchArgument("roi_y", default_value="192"), DeclareLaunchArgument("roi_width", default_value="1024"), DeclareLaunchArgument("roi_height", default_value="528"), DeclareLaunchArgument("can_interface", default_value="can0"), DeclareLaunchArgument("calibration_speed", default_value="15"), DeclareLaunchArgument( "continuous_motion_mode", default_value="endpoint" ), DeclareLaunchArgument( "angle_estimation_mode", default_value="trajectory_center_3d", ), DeclareLaunchArgument("apriltag_decimate", default_value="1.5"), DeclareLaunchArgument("commands_enabled", default_value="true"), DeclareLaunchArgument("start_camera", default_value="true"), DeclareLaunchArgument("start_sdk", default_value="true"), DeclareLaunchArgument("record_bag", default_value="false"), DeclareLaunchArgument("output_root", default_value=default_output), DeclareLaunchArgument("session_dir", default_value=""), DeclareLaunchArgument( "calibration_config", default_value=str(package_share / "config" / "calibration.yaml"), ), DeclareLaunchArgument( "tag_config", default_value=str(package_share / "config" / "front_tags.yaml"), ), OpaqueFunction(function=_launch_stack), ] )