更新加入料带目标检测，判断料带到位，以及控制滚筒逻辑

detect_bagor35bag/README.md

@@ -0,0 +1,111 @@
+# RKNN 料袋（bag / bag35）检测与滚筒控制逻辑
+
+本工程基于 **RKNN 模型** 对流水线上的料袋进行检测与分类（`bag` / `bag35`），  
+并根据检测结果与位置关系判断料袋状态（未到位 / 到位 / 掉出滚筒），  
+最终执行对应的 **滚筒控制逻辑** 或用于 **纯判断测试**。
+
+---
+
+## 一、目录结构
+
+```
+detect_bagor35bag/
+├── bag3568.rknn
+├── detect_bag.py
+├── main_bag_judgment.py
+├── test_bag_onlyjudgment_withou-motor-contral.py
+├── test_image/
+└── README.md
+```
+
+---
+
+## 二、功能说明
+
+### 料袋检测
+- RKNN 推理
+- 支持 `bag` / `bag35` 目标检测
+- 输出 `cls / conf / min_x` 50kg料包为bag，35kg为bag35；conf是置信度；min_x是判断料包底部距离现在传感器物理位置的距离，未到位是负数，到位后是正数距离
+
+### 状态判断
+
+| 状态 | 条件 |
+|----|----|
+| 没有料袋 | min_x is None |
+| 料袋未到位 | min_x < THRESHOLD_X |
+| 料袋到位 | THRESHOLD_X ≤ min_x ≤ THRESHOLD_maxX |
+| 料包掉出滚筒 | min_x > THRESHOLD_maxX |
+
+```python
+THRESHOLD_X = 537       # 到位阈值
+THRESHOLD_maxX = 1430  # 掉出滚筒阈值
+```
+
+### 滚筒控制规则
+
+- 未检测 / 未到位 → 不动作
+- 掉出滚筒 → 停机报警
+- 到位：
+  - bag   → 立即停止滚筒
+  - bag35 → 延时2s → 反转2s → 停止
+
+---
+
+## 三、依赖安装（已安装）
+
+```bash
+pip install opencv-python numpy rknnlite
+```
+
+---
+
+## 四、使用方式
+
+### 主程序（含电机控制）
+
+```bash
+python main_bag_judgment.py
+```
+
+### 仅判断测试（无电机）
+
+```bash
+python test_bag_onlyjudgment_withou-motor-contral.py
+```
+
+---
+
+## 五、核心接口
+
+### detect_bag
+
+```python
+cls, conf, min_x = detect_bag(img)  #不可视化图像
+cls, conf, min_x, vis_img = detect_bag(img, return_vis=True) #可视化图像
+```
+
+### bag_judgment
+
+```python
+status_bool, status_text, conf, min_x, vis_img = bag_judgment(img) #不可视化图像+滚筒控制
+```
+
+---
+
+## 六、状态文本规范
+
+```
+没有料袋
+料袋未到位
+料袋到位
+料包掉出滚筒
+```
+
+---
+
+## 七、说明
+
+- 检测与控制逻辑解耦
+- 易于扩展新料袋类型
+- 支持现场与离线测试
+

detect_bagor35bag/detect_bag.py

@@ -0,0 +1,202 @@
+import os
+import cv2
+import numpy as np
+from rknnlite.api import RKNNLite
+
+# ====================== 配置 ======================
+MODEL_PATH = "bag3568.rknn"
+IMG_SIZE = (640, 640)
+
+OBJ_THRESH = 0.25
+NMS_THRESH = 0.45
+
+CLASS_NAME = ["bag", "bag35"]
+
+# ====================== 工具函数 ======================
+def softmax(x, axis=-1):
+    x = x - np.max(x, axis=axis, keepdims=True)
+    exp_x = np.exp(x)
+    return exp_x / np.sum(exp_x, axis=axis, keepdims=True)
+
+def letterbox_resize(image, size, bg_color=114):
+    target_w, target_h = size
+    h, w = image.shape[:2]
+    scale = min(target_w / w, target_h / h)
+
+    new_w, new_h = int(w * scale), int(h * scale)
+    resized = cv2.resize(image, (new_w, new_h))
+
+    canvas = np.full((target_h, target_w, 3), bg_color, dtype=np.uint8)
+    dx = (target_w - new_w) // 2
+    dy = (target_h - new_h) // 2
+    canvas[dy:dy + new_h, dx:dx + new_w] = resized
+
+    return canvas, scale, dx, dy
+
+# ====================== DFL 解码 ======================
+def dfl_decode(reg):
+    reg = reg.reshape(4, -1)
+    prob = softmax(reg, axis=1)
+    acc = np.arange(reg.shape[1])
+    return np.sum(prob * acc, axis=1)
+
+# ====================== NMS ======================
+def nms(boxes, scores, thresh):
+    boxes = np.array(boxes)
+    scores = np.array(scores)
+
+    x1, y1, x2, y2 = boxes.T
+    areas = (x2 - x1) * (y2 - y1)
+    order = scores.argsort()[::-1]
+
+    keep = []
+    while order.size > 0:
+        i = order[0]
+        keep.append(i)
+
+        xx1 = np.maximum(x1[i], x1[order[1:]])
+        yy1 = np.maximum(y1[i], y1[order[1:]])
+        xx2 = np.minimum(x2[i], x2[order[1:]])
+        yy2 = np.minimum(y2[i], y2[order[1:]])
+
+        inter = np.maximum(0, xx2 - xx1) * np.maximum(0, yy2 - yy1)
+        iou = inter / (areas[i] + areas[order[1:]] - inter)
+
+        order = order[1:][iou <= thresh]
+
+    return keep
+
+# ====================== 后处理 ======================
+def post_process(outputs, scale, dx, dy):
+    boxes_all, scores_all, classes_all = [], [], []
+
+    strides = [8, 16, 32]
+
+    for i, stride in enumerate(strides):
+        reg = outputs[i * 3 + 0][0]
+        cls = outputs[i * 3 + 1][0]
+        obj = outputs[i * 3 + 2][0]
+
+        num_classes, H, W = cls.shape
+
+        for h in range(H):
+            for w in range(W):
+                class_prob = cls[:, h, w]
+                cls_id = int(np.argmax(class_prob))
+                cls_score = class_prob[cls_id]
+
+                obj_score = obj[0, h, w]
+                score = cls_score * obj_score
+
+                if score < OBJ_THRESH:
+                    continue
+
+                l, t, r, b = dfl_decode(reg[:, h, w])
+
+                cx = (w + 0.5) * stride
+                cy = (h + 0.5) * stride
+
+                x1 = cx - l * stride
+                y1 = cy - t * stride
+                x2 = cx + r * stride
+                y2 = cy + b * stride
+
+                boxes_all.append([x1, y1, x2, y2])
+                scores_all.append(score)
+                classes_all.append(cls_id)
+
+    if not boxes_all:
+        return None, None, None
+
+    keep = nms(boxes_all, scores_all, NMS_THRESH)
+
+    boxes = np.array(boxes_all)[keep]
+    scores = np.array(scores_all)[keep]
+    classes = np.array(classes_all)[keep]
+
+    boxes[:, [0, 2]] = (boxes[:, [0, 2]] - dx) / scale
+    boxes[:, [1, 3]] = (boxes[:, [1, 3]] - dy) / scale
+
+    return boxes, classes, scores
+
+# ====================== RKNN 初始化（全局一次） ======================
+_rknn = RKNNLite()
+_rknn.load_rknn(MODEL_PATH)
+_rknn.init_runtime()
+
+# ====================== 统一接口函数 ======================
+def detect_bag(img, return_vis=False):
+    """
+    Args:
+        img (np.ndarray): BGR 原图
+        return_vis (bool)
+
+    Returns:
+        cls (str | None)
+        conf (float | None)
+        min_x (int | None)
+        vis_img (np.ndarray)  # optional
+    """
+
+    img_r, scale, dx, dy = letterbox_resize(img, IMG_SIZE)
+    outputs = _rknn.inference([np.expand_dims(img_r, 0)])
+
+    boxes, cls_ids, scores = post_process(outputs, scale, dx, dy)
+
+    if boxes is None or len(scores) == 0:
+        if return_vis:
+            return None, None, None, img.copy()
+        return None, None, None
+
+    best_idx = int(np.argmax(scores))
+
+    conf = float(scores[best_idx])
+    cls_id = int(cls_ids[best_idx])
+    cls = CLASS_NAME[cls_id]
+
+    x1, y1, x2, y2 = boxes[best_idx].astype(int)
+    min_x = int(x1)
+
+    if return_vis:
+        vis = img.copy()
+        cv2.rectangle(vis, (x1, y1), (x2, y2), (0, 255, 0), 2)
+        cv2.putText(
+            vis,
+            f"{cls}:{conf:.3f}",
+            (x1, max(y1 - 5, 0)),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.6,
+            (0, 255, 0),
+            2
+        )
+        return cls, conf, min_x, vis
+
+    return cls, conf, min_x
+
+
+# ====================== 测试 ======================
+# ====================== 测试 ======================
+if __name__ == "__main__":
+    IMG_PATH = "./test_image/4.jpg"
+    OUTPUT_DIR = "./result"
+    os.makedirs(OUTPUT_DIR, exist_ok=True)
+
+    img = cv2.imread(IMG_PATH)
+    if img is None:
+        raise FileNotFoundError(IMG_PATH)
+
+    cls, conf, min_x, vis = detect_bag(img, return_vis=True)
+
+    if cls is None:
+        print("未检测到目标")
+    else:
+        print(f"类别: {cls}")
+        print(f"置信度: {conf:.4f}")
+        print(f"最左 x: {min_x}")
+
+        if vis is not None:
+            save_path = os.path.join(OUTPUT_DIR, "vis_result.jpg")
+            cv2.imwrite(save_path, vis)
+            print("可视化结果已保存:", save_path)
+
+

detect_bagor35bag/main_bag_judgment.py

@@ -0,0 +1,128 @@
+import cv2
+import time
+from detect_bag import detect_bag
+#这个要注意放在Feeding同一目录下是这样调用EMV的
+from EMV.EMV import RelayController
+
+THRESHOLD_X = 537       # 到位阈值
+THRESHOLD_maxX = 1430  # 掉出滚筒阈值
+
+relay_controller = RelayController()
+
+# ==================================================
+# 不同料包的滚筒控制逻辑
+# ==================================================
+def handle_bag_motor(cls, status_bool, status_text):
+    """
+    滚筒控制总逻辑：
+    - 没检测到料包 → 不发信号
+    - 未到位 → 不发信号
+    - 掉出滚筒 → 报警（不再操作滚筒）
+    - 到位：
+        bag   → 立刻停止滚筒
+        bag35 → 持续正转1.5s反转1.5秒 → 停止
+    """
+
+    # 没检测到料包
+    if cls is None:
+        return
+
+    # 掉出滚筒（最高优先级）
+    if status_text == "料包掉出滚筒":
+        print("料包掉出滚筒 → 报警 / 停机")
+        relay_controller.close(conveyor2=True)
+        relay_controller.close(conveyor2_reverse=True)
+        return
+
+    # 未到位 → 什么都不做
+    if status_bool is not True:
+        return
+
+    # ================== 到位 + 分类 ==================
+    if cls == "bag":
+        print("[bag] 到位 → 立刻停止滚筒")
+        relay_controller.close(conveyor2=True)
+
+    elif cls == "bag35":
+        print("[bag35] 到位 → 持续正转滚筒1.5秒 后，反转滚筒 1.5 秒 到原位置→ 停止滚筒")
+        time.sleep(1.5)
+        relay_controller.open(conveyor2_reverse=True)
+        time.sleep(1.5)
+        relay_controller.close(conveyor2_reverse=True)
+
+    else:
+        # 预留扩展
+        return
+
+
+# ==================================================
+# 料袋状态判断
+# ==================================================
+def bag_judgment(img, return_conf=True, return_vis=False):
+    """
+    判断图片中的料袋状态
+    """
+    cls = None
+    conf = None
+    min_x = None
+    vis_img = None
+
+    # ================== 唯一检测调用 ==================
+    if return_vis:
+        cls, conf, min_x, vis_img = detect_bag(img, return_vis=True)
+    else:
+        cls, conf, min_x = detect_bag(img, return_vis=False)
+
+    # ================== 状态判断 ==================
+    if min_x is None:
+        status_bool = None
+        status_text = "没有料袋"
+
+    elif min_x > THRESHOLD_maxX:
+        status_bool = False
+        status_text = "料包掉出滚筒"
+
+    elif THRESHOLD_X <= min_x <= THRESHOLD_maxX:
+        status_bool = True
+        status_text = "料袋到位"
+
+    else:
+        status_bool = False
+        status_text = "料袋未到位"
+
+    # ================== 滚筒控制 ==================
+    handle_bag_motor(cls, status_bool, status_text)
+
+    # ================== 返回 ==================
+    if not return_conf:
+        conf = None
+    if not return_vis:
+        vis_img = None
+
+    return status_bool, status_text, conf, min_x, vis_img
+
+
+# ====================== 测试 ======================
+if __name__ == "__main__":
+    IMG_PATH = "./test_image/3.jpg"
+    img = cv2.imread(IMG_PATH)
+    if img is None:
+        raise FileNotFoundError(f"图片无法读取: {IMG_PATH}")
+    #这里面包含 handle_bag_motor滚筒控制，只要你记得后面机械臂抓完包之后要打开滚筒，Feeding里self.relay_controller.open(conveyor2=True)
+    status_bool, status_text, conf, min_x, vis_img = bag_judgment(
+        img,
+        return_conf = True,
+        return_vis = False
+    )
+
+    print(
+        f"判断结果: {status_bool}, "
+        f"中文状态: {status_text}, "
+        f"conf={conf}, min_x={min_x}"
+    )
+
+    if vis_img is not None:
+        cv2.imshow("Vis", vis_img)
+        cv2.waitKey(0)
+        cv2.destroyAllWindows()
+

detect_bagor35bag/test_bag_onlyjudgment_withou-motor-contral.py

@@ -0,0 +1,72 @@
+import cv2
+from detect_bag import detect_bag
+
+THRESHOLD_X = 537        # 到位阈值
+THRESHOLD_maxX = 1430   # 掉出滚筒阈值
+
+
+def bag_judgment(img, return_conf=True, return_vis=False):
+    """
+    判断图片中的料袋状态（测试版，不控制电机）
+    """
+    cls = None
+    conf = None
+    min_x = None
+    vis_img = None
+
+    # ================== 唯一调用 ==================
+    if return_vis:
+        cls, conf, min_x, vis_img = detect_bag(img, return_vis=True)
+    else:
+        cls, conf, min_x = detect_bag(img, return_vis=False)
+
+    # ================== 状态判断 ==================
+    if min_x is None:
+        status_bool = None
+        status_text = "没有料袋"
+
+    elif min_x > THRESHOLD_maxX:
+        status_bool = False
+        status_text = "料包掉出滚筒"
+
+    elif THRESHOLD_X <= min_x <= THRESHOLD_maxX:
+        status_bool = True
+        status_text = "料袋到位"
+
+    else:
+        status_bool = False
+        status_text = "料袋未到位"
+
+    # ================== 返回 ==================
+    if not return_conf:
+        conf = None
+    if not return_vis:
+        vis_img = None
+
+    return status_bool, status_text, conf, min_x, vis_img
+
+
+# ====================== 测试 ======================
+if __name__ == "__main__":
+    IMG_PATH = "./test_image/3.jpg"
+    img = cv2.imread(IMG_PATH)
+    if img is None:
+        raise FileNotFoundError(f"图片无法读取: {IMG_PATH}")
+
+    status_bool, status_text, conf, min_x, vis_img = bag_judgment(
+        img,
+        return_conf=True,
+        return_vis=True
+    )
+
+    print(
+        f"判断结果: {status_bool}, "
+        f"中文状态: {status_text}, "
+        f"conf={conf}, min_x={min_x}"
+    )
+
+    if vis_img is not None:
+        cv2.imshow("Vis", vis_img)
+        cv2.waitKey(0)
+        cv2.destroyAllWindows()
+