forked from YikaiFu-cart/acAubo
Refactor control parameters and enhance depth handling for tomato harvesting
- Updated YOLO model path to a relative path. - Introduced new parameters for depth handling, including minimum and maximum depth, fallback settings, and pixel validation thresholds. - Increased maximum pick angle to accommodate wider fruit stems. - Added cut pull assist feature with configurable options. - Removed unused resource path resolution functions from control_core.py. - Simplified resource path handling in main.py. - Updated UI colors and styles for improved aesthetics. - Removed outdated requirements.txt file.
This commit is contained in:
Brunsmeier
142
control_core.py
142
control_core.py
@ -6,7 +6,6 @@ import os
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
@ -40,13 +39,6 @@ PICK_ENDPOINT_KEYPOINT_INDEX = 1 # YOLO pose 第 1 个点为方向端点 endpoi
|
||||
PICK_KEYPOINT_BOX_MARGIN_RATIO = 0.15 # 关键点允许略超出 bbox 的比例。
|
||||
PICK_CUTPOINT_MAX_REL_Y = 0.70 # 采摘点在 bbox 内的最大相对高度，防止误取下方点。
|
||||
PICK_KEYPOINT_MIN_DISTANCE_PX = 8.0 # cutpoint 与 endpoint 的最小像素距离。
|
||||
PICK_ENDPOINT_Y_TOLERANCE_PX = 4 # endpoint 允许比 cutpoint 略高的像素容差。
|
||||
PICK_DEPTH_WINDOW_RADIUS = 5 # 首次稳健取深度的窗口半径，5 表示 11x11。
|
||||
PICK_DEPTH_FALLBACK_WINDOW_RADIUS = 10 # 首次深度不足时的扩大窗口半径，10 表示 21x21。
|
||||
PICK_DEPTH_MIN_VALID_PIXELS = 5 # 深度窗口内至少需要的有效深度点数量。
|
||||
PICK_DEPTH_MIN_M = 0.25 # 有效深度下限，单位 m。
|
||||
PICK_DEPTH_MAX_M = 0.6 # 有效深度上限，单位 m。
|
||||
PICK_DEPTH_MEDIAN_TOLERANCE_M = 0.035 # 深度中位数滤波容差，单位 m。
|
||||
|
||||
# R_tc = np.array(
|
||||
# [
|
||||
@ -106,61 +98,6 @@ ui_callback: Callable[[np.ndarray], None] = lambda frame: None
|
||||
robot_rpc_client = pyaubo_sdk.RpcClient()
|
||||
|
||||
|
||||
def get_runtime_resource_dir() -> Path:
|
||||
"""返回运行时资源目录，兼容 PyInstaller 解包目录。"""
|
||||
if hasattr(sys, "_MEIPASS"):
|
||||
return Path(sys._MEIPASS).resolve()
|
||||
return Path(__file__).resolve().parent
|
||||
|
||||
|
||||
def get_runtime_app_dir() -> Path:
|
||||
"""返回应用目录，打包后优先使用可执行文件所在目录。"""
|
||||
if getattr(sys, "frozen", False):
|
||||
return Path(sys.executable).resolve().parent
|
||||
return Path(__file__).resolve().parent
|
||||
|
||||
|
||||
def get_resource_candidates(filename: str) -> List[Path]:
|
||||
"""返回模型/图片等资源的候选路径。"""
|
||||
resource_dir = get_runtime_resource_dir()
|
||||
app_dir = get_runtime_app_dir()
|
||||
return [
|
||||
resource_dir / filename,
|
||||
resource_dir / "models" / filename,
|
||||
resource_dir / "tools" / filename,
|
||||
app_dir / filename,
|
||||
app_dir / "models" / filename,
|
||||
app_dir / "tools" / filename,
|
||||
Path.cwd() / filename,
|
||||
Path.cwd() / "models" / filename,
|
||||
Path.cwd() / "tools" / filename,
|
||||
]
|
||||
|
||||
|
||||
def resolve_resource_path(path_value: Optional[str], fallback_name: Optional[str] = None) -> Optional[Path]:
|
||||
"""解析运行时资源路径，兼容源码运行与打包运行。"""
|
||||
if path_value:
|
||||
candidate = Path(path_value).expanduser()
|
||||
if candidate.is_absolute():
|
||||
candidate = candidate.resolve()
|
||||
if candidate.exists():
|
||||
return candidate
|
||||
else:
|
||||
for relative_candidate in (
|
||||
get_runtime_app_dir() / candidate,
|
||||
get_runtime_resource_dir() / candidate,
|
||||
Path.cwd() / candidate,
|
||||
):
|
||||
resolved = relative_candidate.resolve()
|
||||
if resolved.exists():
|
||||
return resolved
|
||||
if fallback_name:
|
||||
for candidate in get_resource_candidates(fallback_name):
|
||||
if candidate.exists():
|
||||
return candidate.resolve()
|
||||
return None
|
||||
|
||||
|
||||
def configure(config: Dict[str, Any]) -> None:
|
||||
"""接收 control.py 同步过来的可调参数。"""
|
||||
_runtime_config.update(config)
|
||||
@ -804,7 +741,21 @@ class RobotArmController:
|
||||
|
||||
self.close_scissors_for_pick()
|
||||
|
||||
retreat_joints = self.move_pose(approach_pose, pickup_joints, description="采摘后返回预抓取点")
|
||||
retreat_reference_joints = pickup_joints
|
||||
if _cfg("CUT_PULL_ASSIST_ENABLED"):
|
||||
pull_pose = pickup_pose.copy()
|
||||
pull_pose[1] += _cfg("CUT_PULL_ASSIST_Y_OFFSET")
|
||||
print(
|
||||
"机械臂：执行剪切后辅助拉拽 "
|
||||
f"Y 偏移 {_cfg('CUT_PULL_ASSIST_Y_OFFSET'):.3f} m"
|
||||
)
|
||||
pull_joints = self.move_pose(pull_pose, pickup_joints, description="剪切后辅助拉拽")
|
||||
if pull_joints is None:
|
||||
self.return_home()
|
||||
return False
|
||||
retreat_reference_joints = pull_joints
|
||||
|
||||
retreat_joints = self.move_pose(approach_pose, retreat_reference_joints, description="采摘后返回预抓取点")
|
||||
if retreat_joints is None:
|
||||
self.return_home()
|
||||
return False
|
||||
@ -854,9 +805,7 @@ def init_camera() -> Tuple[Any, Any, Any]:
|
||||
|
||||
def init_tomato_detector(model_path: Optional[str] = None) -> Any:
|
||||
"""初始化 YOLO 模型。"""
|
||||
configured_path = model_path or _cfg("YOLO_MODEL_PATH") or "models/best.pt"
|
||||
resolved_path = resolve_resource_path(configured_path, fallback_name="best.pt")
|
||||
use_path = str(resolved_path) if resolved_path is not None else configured_path
|
||||
use_path = model_path or _cfg("YOLO_MODEL_PATH") or "best.pt"
|
||||
print(f"加载 YOLO 模型 | 路径: {use_path}")
|
||||
|
||||
if not os.path.exists(use_path):
|
||||
@ -986,7 +935,13 @@ def calculate_keypoint_line_angle_rad(
|
||||
if abs(dx) < 1e-6 and abs(dy) < 1e-6:
|
||||
return 0.0
|
||||
|
||||
# 果梗是一条无方向直线；endpoint 在剪切点的任一侧都应表示同一倾角。
|
||||
# 归一化后，横向或斜向果梗不会因关键点标注顺序而得到 180°旋转。
|
||||
angle_rad = math.atan2(dx, dy)
|
||||
if angle_rad > math.pi / 2:
|
||||
angle_rad -= math.pi
|
||||
elif angle_rad <= -math.pi / 2:
|
||||
angle_rad += math.pi
|
||||
max_angle_rad = math.radians(float(_cfg("MAX_PICK_ANGLE_DEG")))
|
||||
return max(-max_angle_rad, min(max_angle_rad, angle_rad))
|
||||
|
||||
@ -1131,10 +1086,6 @@ class VisionController:
|
||||
print(f"Pick candidate skipped: keypoints too close ({distance:.1f}px)")
|
||||
return False
|
||||
|
||||
if end_point[1] + PICK_ENDPOINT_Y_TOLERANCE_PX < cutpoint[1]:
|
||||
print(f"Pick candidate skipped: endpoint appears above cutpoint @ cut={cutpoint}, end={end_point}")
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def extract_candidate_keypoints(
|
||||
@ -1271,7 +1222,7 @@ class VisionController:
|
||||
for sample_y in range(y_start, y_end + 1):
|
||||
for sample_x in range(x_start, x_end + 1):
|
||||
depth = float(depth_frame.get_distance(sample_x, sample_y))
|
||||
if PICK_DEPTH_MIN_M <= depth <= PICK_DEPTH_MAX_M:
|
||||
if _cfg("PICK_DEPTH_MIN_M") <= depth <= _cfg("PICK_DEPTH_MAX_M"):
|
||||
samples.append(depth)
|
||||
return samples
|
||||
|
||||
@ -1283,25 +1234,27 @@ class VisionController:
|
||||
frame_shape: Tuple[int, int, int],
|
||||
) -> Optional[float]:
|
||||
center_depth = float(depth_frame.get_distance(pixel_x, pixel_y))
|
||||
for radius in (PICK_DEPTH_WINDOW_RADIUS, PICK_DEPTH_FALLBACK_WINDOW_RADIUS):
|
||||
for radius in (_cfg("PICK_DEPTH_WINDOW_RADIUS"), _cfg("PICK_DEPTH_FALLBACK_WINDOW_RADIUS")):
|
||||
samples = self.collect_depth_samples(depth_frame, pixel_x, pixel_y, frame_shape, radius)
|
||||
if len(samples) < PICK_DEPTH_MIN_VALID_PIXELS:
|
||||
if len(samples) < _cfg("PICK_DEPTH_MIN_VALID_PIXELS"):
|
||||
continue
|
||||
|
||||
sample_array = np.array(samples, dtype=float)
|
||||
median_depth = float(np.median(sample_array))
|
||||
stable_samples = sample_array[np.abs(sample_array - median_depth) <= PICK_DEPTH_MEDIAN_TOLERANCE_M]
|
||||
if len(stable_samples) >= PICK_DEPTH_MIN_VALID_PIXELS:
|
||||
stable_samples = sample_array[
|
||||
np.abs(sample_array - median_depth) <= _cfg("PICK_DEPTH_MEDIAN_TOLERANCE_M")
|
||||
]
|
||||
if len(stable_samples) >= _cfg("PICK_DEPTH_MIN_VALID_PIXELS"):
|
||||
depth = float(np.median(stable_samples))
|
||||
else:
|
||||
depth = median_depth
|
||||
|
||||
if not (PICK_DEPTH_MIN_M <= center_depth <= PICK_DEPTH_MAX_M):
|
||||
if not (_cfg("PICK_DEPTH_MIN_M") <= center_depth <= _cfg("PICK_DEPTH_MAX_M")):
|
||||
print(
|
||||
"采摘点中心深度无效，使用邻域稳健深度 "
|
||||
f"{depth:.3f}m @ ({pixel_x}, {pixel_y}), radius={radius}"
|
||||
)
|
||||
elif abs(center_depth - depth) > PICK_DEPTH_MEDIAN_TOLERANCE_M:
|
||||
elif abs(center_depth - depth) > _cfg("PICK_DEPTH_MEDIAN_TOLERANCE_M"):
|
||||
print(
|
||||
"采摘点中心深度跳变，使用邻域稳健深度 "
|
||||
f"{depth:.3f}m 替代 {center_depth:.3f}m @ ({pixel_x}, {pixel_y})"
|
||||
@ -1314,6 +1267,37 @@ class VisionController:
|
||||
)
|
||||
return None
|
||||
|
||||
def get_cutpoint_depth(
|
||||
self,
|
||||
depth_frame: Any,
|
||||
cutpoint: Tuple[int, int],
|
||||
end_point: Optional[Tuple[int, int]],
|
||||
frame_shape: Tuple[int, int, int],
|
||||
) -> Optional[float]:
|
||||
"""获取剪切点深度；细果梗无深度时沿原有关键点连线做有限回退。"""
|
||||
depth = self.get_robust_depth(depth_frame, cutpoint[0], cutpoint[1], frame_shape)
|
||||
if depth is not None:
|
||||
return depth
|
||||
if not _cfg("PICK_DEPTH_LINE_FALLBACK_ENABLED") or end_point is None:
|
||||
return None
|
||||
|
||||
direction = np.array(end_point, dtype=float) - np.array(cutpoint, dtype=float)
|
||||
norm = float(np.linalg.norm(direction))
|
||||
if norm < 1e-6:
|
||||
return None
|
||||
direction /= norm
|
||||
for step in range(1, int(_cfg("PICK_DEPTH_LINE_FALLBACK_STEPS")) + 1):
|
||||
offset = direction * float(_cfg("PICK_DEPTH_LINE_FALLBACK_STEP_PX")) * step
|
||||
sample_x, sample_y = self.clamp_pixel(cutpoint[0] + offset[0], cutpoint[1] + offset[1], frame_shape)
|
||||
depth = self.get_robust_depth(depth_frame, sample_x, sample_y, frame_shape)
|
||||
if depth is not None:
|
||||
print(
|
||||
"剪切点无有效深度，使用果梗方向邻近深度 "
|
||||
f"{depth:.3f}m @ ({sample_x}, {sample_y}) 估计剪切点"
|
||||
)
|
||||
return depth
|
||||
return None
|
||||
|
||||
def build_detected_tomato(
|
||||
self,
|
||||
candidate: TomatoCandidate,
|
||||
@ -1329,7 +1313,7 @@ class VisionController:
|
||||
return None
|
||||
pixel_x, pixel_y = candidate.cutpoint
|
||||
|
||||
depth = self.get_robust_depth(depth_frame, pixel_x, pixel_y, frame.shape)
|
||||
depth = self.get_cutpoint_depth(depth_frame, candidate.cutpoint, candidate.end_point, frame.shape)
|
||||
if depth is None:
|
||||
print(f"无效深度值 @ ({pixel_x}, {pixel_y})")
|
||||
return None
|
||||
|
||||
Reference in New Issue
Block a user