acAubo_visual_servo_from_Yikai/control.py

import signal
from typing import Any, Sequence

import control_core as _core

"""
对外兼容入口。
这个文件主要做两件事：
1. 暴露和旧版本一致的函数名，方便 UI 或外部脚本继续调用；
2. 在真正进入核心逻辑前，把这里可修改的配置同步到 `control_core.py`。
"""


# -------------------- 设备连接参数 --------------------
ROBOT_IP = "192.168.192.100"
ROBOT_PORT = 30004
M_PI = _core.M_PI

AGV_IP = "192.168.192.100"
AGV_PORT = 30104
AGV_SPEED_FORWARD = 0
AGV_SPEED_STOP = 0.0
AGV_RUN_DISTANCE = 5.0


# -------------------- 运行时长与检测参数 --------------------
TOTAL_DURATION = 300
AGV_STOP_TIMEOUT = 10
AGV_PICK_SETTLE_DELAY = 1.0
YOLO_MODEL_PATH = "best.pt"
YOLO_DETECT_CONF = 0.5
PICK_CONFIDENCE_THRESHOLD = 0.7


# -------------------- 位姿与轨迹参数 --------------------
# Home 是机械臂的初始/复位关节位。
HOME_JOINTS = [-1.5247, 1.0899, 2.4671, 1.2761, 1.5113, 0.0001]

# 视觉算出的目标点会再叠加这两个偏移：
# APPROACH_Y_OFFSET 用于先到预抓取点；
# LIFT_Z_OFFSET 用于修正真正执行抓取时的高度。
APPROACH_Y_OFFSET = 0.15
LIFT_Z_OFFSET = 0.035
PICKUP_X_OFFSET = 0.0
PICKUP_Y_OFFSET = -0.015
PICK_ANGLE_ORIENTATION_INDEX = 4
PICK_ANGLE_SIGN = 1.0
MAX_PICK_ANGLE_DEG = 30.0

# 当前项目只使用一个放置位，仍保留列表结构以兼容 control_core。
place_positions = [[-0.6203, 0.026, 0.1078, 2.9923, 0.0366, -1.4157]]


# -------------------- 夹爪与机械臂运动参数 --------------------
GRIPPER_CLOSE_IO = 1
GRIPPER_OPEN_IO = 0
GRIPPER_ACTION_DELAY = 2.0
PICK_POINT_ARRIVAL_DELAY = 1.0
SCISSORS_ENABLED = True

# 这里沿用底层 SDK 的弧度制。
ARM_SPEED = 150 * (M_PI / 180)
ARM_ACCEL = 120 * (M_PI / 180)


# -------------------- 参数同步清单 --------------------
_CONFIG_NAMES = (
    "ROBOT_IP",
    "ROBOT_PORT",
    "AGV_IP",
    "AGV_PORT",
    "AGV_SPEED_FORWARD",
    "AGV_SPEED_STOP",
    "AGV_RUN_DISTANCE",
    "TOTAL_DURATION",
    "AGV_STOP_TIMEOUT",
    "AGV_PICK_SETTLE_DELAY",
    "YOLO_MODEL_PATH",
    "YOLO_DETECT_CONF",
    "PICK_CONFIDENCE_THRESHOLD",
    "HOME_JOINTS",
    "APPROACH_Y_OFFSET",
    "LIFT_Z_OFFSET",
    "PICKUP_X_OFFSET",
    "PICKUP_Y_OFFSET",
    "PICK_ANGLE_ORIENTATION_INDEX",
    "PICK_ANGLE_SIGN",
    "MAX_PICK_ANGLE_DEG",
    "GRIPPER_CLOSE_IO",
    "GRIPPER_OPEN_IO",
    "GRIPPER_ACTION_DELAY",
    "PICK_POINT_ARRIVAL_DELAY",
    "SCISSORS_ENABLED",
    "ARM_SPEED",
    "ARM_ACCEL",
    "place_positions",
)


# -------------------- 运行状态别名 --------------------
running = _core.running
has_tomato = _core.has_tomato
picking_done = _core.picking_done
robot_rpc_client = _core.robot_rpc_client
ui_callback = _core.ui_callback


def _sync_config() -> None:
    # 如果 UI 或外部脚本只改了 control.py，这里会在进入核心逻辑前同步到 control_core。
    config = {name: globals()[name] for name in _CONFIG_NAMES}
    if place_positions and isinstance(place_positions[0], (list, tuple)):
        config["place_positions"] = [list(position) for position in place_positions]
    else:
        config["place_positions"] = [list(place_positions)]
    config["HOME_JOINTS"] = list(HOME_JOINTS)
    _core.configure(config)


def set_ui_callback(callback):
    global ui_callback
    ui_callback = callback
    _core.set_ui_callback(callback)


def exampleState(robot_name: str):
    return _core.exampleState(robot_name)


def get_robot_end_effector_pose(robot_name: str):
    return _core.get_robot_end_effector_pose(robot_name)


def exampleInverseK(robot_name: str, pose: Sequence[float], reference_q: Sequence[float]):
    return _core.exampleInverseK(robot_name, pose, reference_q)


def exampleStartup():
    _sync_config()
    return _core.exampleStartup()


def quaternion_to_rotation_matrix(q: Sequence[float]):
    return _core.quaternion_to_rotation_matrix(q)


def euler_to_rotation_matrix(roll: float, pitch: float, yaw: float):
    return _core.euler_to_rotation_matrix(roll, pitch, yaw)


def get_robot_end_matrix(robot_name: str):
    return _core.get_robot_end_matrix(robot_name)


def camera_to_base(point_cam: Sequence[float], robot_name: str):
    return _core.camera_to_base(point_cam, robot_name)


def check_joint_position(robot_name: str, target_joints: Sequence[float]):
    return _core.check_joint_position(robot_name, target_joints)


def check_tcp_pose(robot_name: str, target_pose: Sequence[float]):
    return _core.check_tcp_pose(robot_name, target_pose)


def waitArrival(impl: Any, target_joints=None, target_pose=None):
    return _core.waitArrival(impl, target_joints=target_joints, target_pose=target_pose)


def control_tool_io(robot_name: str, io_index: int, state: bool):
    return _core.control_tool_io(robot_name, io_index, state)


def get_next_placement():
    _sync_config()
    return _core.get_next_placement()


def get_placement_index():
    return _core.get_placement_index()


def create_placement_manager():
    _sync_config()
    return _core.create_placement_manager()


def return_to_home(robot_name: str):
    _sync_config()
    return _core.return_to_home(robot_name)


def control_robot(robot_name: str, pose: Sequence[float], angle_rad: float):
    _sync_config()
    return _core.control_robot(robot_name, pose, angle_rad)


def init_camera():
    _sync_config()
    return _core.init_camera()


def init_tomato_detector(model_path=None):
    _sync_config()
    return _core.init_tomato_detector(model_path)


def filter_ripe_tomatoes(results, confidence_threshold=None):
    _sync_config()
    return _core.filter_ripe_tomatoes(results, confidence_threshold)


def draw_tomato_tilt_line(color_image, x1, y1, x2, y2, pixel_x):
    return _core.draw_tomato_tilt_line(color_image, x1, y1, x2, y2, pixel_x)


def fit_red_line(crop_image, x1, y1):
    return _core.fit_red_line(crop_image, x1, y1)


def vision_detection_thread(pipeline, align, depth_intrinsics, model, robot_name: str):
    _sync_config()
    return _core.vision_detection_thread(pipeline, align, depth_intrinsics, model, robot_name)


def agv_control_thread(agv_client):
    _sync_config()
    return _core.agv_control_thread(agv_client)


def main():
    # control.py 本身只做兼容和配置转发，真正业务流程在 control_core.main()。
    _sync_config()
    return _core.main()


def signal_handler(sig: int, frame: Any):
    _sync_config()
    return _core.signal_handler(sig, frame)


if __name__ == "__main__":
    signal.signal(signal.SIGINT, signal_handler)
    signal.signal(signal.SIGTERM, signal_handler)
    main()