#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
ROS2 节点：识别到绿色启动后扫白色面积寻迹，并在指定时间后检测路面黑色字/块并短暂停止
新增：黑块暂停结束后，开启“红球推行（融合白线巡线）”
- 在未检测到绿色之前原地等待（不旋转、不前进）
- 识别到绿色并连续确认若干帧后进入白色区域质心巡线
- 在运行了指定时间后（默认 91.5s），若在白色道路中间检测到黑色块则停住指定时长（默认 3s）再继续运行
- 暂停结束后开启红球推行：保持白线巡线 + 根据红球位置微调方向、根据红球面积动态减速
发布：/cmd_vel（geometry_msgs/Twist）
订阅：/image_raw（sensor_msgs/Image）
处理后图像发布：/camera/process_image
"""
import time
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image as RosImage
from geometry_msgs.msg import Twist
from cv_bridge import CvBridge
import cv2
import numpy as np
class WhiteAreaFollower(Node):
def __init__(self):
super().__init__('white_area_follower')
# ===== 巡线基础参数 =====
self.declare_parameter('desired_area', 20000) # 期望白色区域面积（像素）
self.declare_parameter('max_speed', 0.85)
self.declare_parameter('min_speed', 0.15)
self.declare_parameter('angular_k', 0.012)
self.declare_parameter('roi_top_ratio', 0.55)
self.declare_parameter('min_area_detect', 600)
self.declare_parameter('show_window', False)
# ===== 绿色检测参数（HSV） =====
self.declare_parameter('green_h_low', 59)
self.declare_parameter('green_s_low', 63)
self.declare_parameter('green_v_low', 130)
self.declare_parameter('green_h_high', 97)
self.declare_parameter('green_s_high', 255)
self.declare_parameter('green_v_high', 255)
self.declare_parameter('min_green_area', 400) # 绿色最小面积（像素）
self.declare_parameter('green_confirm_frames', 4) # 连续帧确认数
# ===== 黑块/黑字暂停参数 =====
self.declare_parameter('stop_after_seconds', 78.5) # 运行多少秒后开始寻找黑块
self.declare_parameter('stop_duration', 3) # 暂停时长（秒）
self.declare_parameter('min_black_area', 500) # 黑块最小面积（像素）
self.declare_parameter('black_detect_method', 'otsu') # 'otsu' 或 'fixed_v'
self.declare_parameter('black_fixed_v', 80) # 若使用 fixed_v，则 V 阈值
self.declare_parameter('center_tolerance_ratio', 0.15) # 黑块质心需位于图像中间 ±ratio
# ===== 红球推行参数（新增） =====
# HSV 双区间（绕 0/180）
self.declare_parameter('red_h_low', 0)
self.declare_parameter('red_h_high', 10)
self.declare_parameter('red_h2_low', 160)
self.declare_parameter('red_h2_high', 179)
self.declare_parameter('red_s_low', 100)
self.declare_parameter('red_v_low', 100)
self.declare_parameter('min_red_area', 350) # 红球最小面积（像素）
self.declare_parameter('max_red_area', 20000) # 红球面积上限（用来近距离降速）
self.declare_parameter('red_angular_k', 0.0045) # 红球角速度系数（越大越“跟球”）
self.declare_parameter('red_influence', 0.3) # 红球对角速度影响权重（0~1）
self.declare_parameter('red_min_linear', 0.08) # 推球时最小线速度下限
self.declare_parameter('red_linear_scale', 0.6) # 推球线速度比例（相对 max_speed）
# ===== 读取参数 =====
self.desired_area = float(self.get_parameter('desired_area').value)
self.max_speed = float(self.get_parameter('max_speed').value)
self.min_speed = float(self.get_parameter('min_speed').value)
self.angular_k = float(self.get_parameter('angular_k').value)
self.roi_top_ratio = float(self.get_parameter('roi_top_ratio').value)
self.min_area_detect = float(self.get_parameter('min_area_detect').value)
self.show_window = bool(self.get_parameter('show_window').value)
self.green_h_low = int(self.get_parameter('green_h_low').value)
self.green_s_low = int(self.get_parameter('green_s_low').value)
self.green_v_low = int(self.get_parameter('green_v_low').value)
self.green_h_high = int(self.get_parameter('green_h_high').value)
self.green_s_high = int(self.get_parameter('green_s_high').value)
self.green_v_high = int(self.get_parameter('green_v_high').value)
self.min_green_area = int(self.get_parameter('min_green_area').value)
self.green_confirm_frames = int(self.get_parameter('green_confirm_frames').value)
self.stop_after_seconds = float(self.get_parameter('stop_after_seconds').value)
self.stop_duration = float(self.get_parameter('stop_duration').value)
self.min_black_area = int(self.get_parameter('min_black_area').value)
self.black_detect_method = str(self.get_parameter('black_detect_method').value)
self.black_fixed_v = int(self.get_parameter('black_fixed_v').value)
self.center_tolerance_ratio = float(self.get_parameter('center_tolerance_ratio').value)
# 红球参数
self.red_h_low = int(self.get_parameter('red_h_low').value)
self.red_h_high = int(self.get_parameter('red_h_high').value)
self.red_h2_low = int(self.get_parameter('red_h2_low').value)
self.red_h2_high = int(self.get_parameter('red_h2_high').value)
self.red_s_low = int(self.get_parameter('red_s_low').value)
self.red_v_low = int(self.get_parameter('red_v_low').value)
self.min_red_area = int(self.get_parameter('min_red_area').value)
self.max_red_area = int(self.get_parameter('max_red_area').value)
self.red_angular_k = float(self.get_parameter('red_angular_k').value)
self.red_influence = float(self.get_parameter('red_influence').value)
self.red_min_linear = float(self.get_parameter('red_min_linear').value)
self.red_linear_scale = float(self.get_parameter('red_linear_scale').value)
# ===== 状态机 =====
self.started = False
self._green_count = 0
self._pause_triggered = False
self._paused_until = None
self._start_time = time.monotonic()
# 黑块暂停结束后，开启红球推行融合
self._red_push_enabled = False
self.bridge = CvBridge()
self.sub = self.create_subscription(RosImage, '/image_raw', self.image_callback, 10)
self.pub_img = self.create_publisher(RosImage, '/camera/process_image', 10)
self.cmd_pub = self.create_publisher(Twist, 'cmd_vel', 10)
self.get_logger().info('white_area_follower started (waiting for green to begin)')
# ===== 绿色检测 =====
def detect_green(self, hsv, roi_slice=None):
lower = np.array([self.green_h_low, self.green_s_low, self.green_v_low])
upper = np.array([self.green_h_high, self.green_s_high, self.green_v_high])
mask = cv2.inRange(hsv, lower, upper)
if roi_slice is not None:
full = np.zeros_like(mask)
full[roi_slice] = mask[roi_slice]
mask = full
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (7, 7))
clean = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel, iterations=1)
clean = cv2.morphologyEx(clean, cv2.MORPH_CLOSE, kernel, iterations=2)
contours, _ = cv2.findContours(clean, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
max_area = max([cv2.contourArea(c) for c in contours], default=0)
return clean, max_area, contours
# ===== 红球检测（新增） =====
def detect_red_ball(self, hsv):
lower1 = np.array([self.red_h_low, self.red_s_low, self.red_v_low])
upper1 = np.array([self.red_h_high, 255, 255])
lower2 = np.array([self.red_h2_low, self.red_s_low, self.red_v_low])
upper2 = np.array([self.red_h2_high, 255, 255])
mask1 = cv2.inRange(hsv, lower1, upper1)
mask2 = cv2.inRange(hsv, lower2, upper2)
mask = cv2.bitwise_or(mask1, mask2)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, np.ones((5, 5), np.uint8))
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, np.ones((7, 7), np.uint8))
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if not contours:
return None, mask
largest = max(contours, key=cv2.contourArea)
area = cv2.contourArea(largest)
if area < self.min_red_area:
return None, mask
M = cv2.moments(largest)
if M['m00'] == 0:
return None, mask
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00'])
return (cx, cy, area), mask
def image_callback(self, msg: RosImage):
try:
frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
except Exception as e:
self.get_logger().error(f'cv_bridge convert failed: {e}')
return
now = time.monotonic()
elapsed = now - self._start_time
h, w = frame.shape[:2]
roi_top = int(h * self.roi_top_ratio)
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
# 绿色检测（整图）
green_mask, green_area, green_contours = self.detect_green(hsv)
vis = frame.copy()
# 绿色可视化
if green_contours:
largest_g = max(green_contours, key=cv2.contourArea)
area_g = cv2.contourArea(largest_g)
if area_g >= self.min_green_area:
x, y, gw, gh = cv2.boundingRect(largest_g)
cv2.rectangle(vis, (x, y), (x + gw, y + gh), (0, 255, 0), 2)
cv2.putText(vis, f'GREEN {int(area_g)}', (x, max(0, y - 6)),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
# 更新绿色确认计数
if green_area >= self.min_green_area:
self._green_count += 1
else:
self._green_count = 0
if not self.started and self._green_count >= self.green_confirm_frames:
self.started = True
self.get_logger().info(f'Green detected for {self.green_confirm_frames} frames, starting white-area following.')
twist = Twist()
found = False
largest = None # 用于黑块搜索 ROI
# ===== 暂停期：保持停止 =====
if self._paused_until is not None and now < self._paused_until:
twist.linear.x = 0.0
twist.angular.z = 0.0
remaining = self._paused_until - now
cv2.putText(vis, f'PAUSED {remaining:.1f}s', (10, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 2)
else:
# 刚结束暂停：开启红球推行
if self._paused_until is not None and now >= self._paused_until:
self._paused_until = None
self._red_push_enabled = True
self.get_logger().info('Pause finished, RED-PUSH enabled, resuming behavior.')
if self.started:
# ===== 白色检测（ROI 下半区域） =====
lower_w = np.array([0, 0, 200])
upper_w = np.array([180, 60, 255])
white_mask = cv2.inRange(hsv, lower_w, upper_w)
roi_mask = np.zeros_like(white_mask)
roi_mask[roi_top:h, :] = white_mask[roi_top:h, :]
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
clean = cv2.morphologyEx(roi_mask, cv2.MORPH_OPEN, kernel, iterations=1)
clean = cv2.morphologyEx(clean, cv2.MORPH_CLOSE, kernel, iterations=2)
contours, _ = cv2.findContours(clean, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
# 可视化白色 mask 到 ROI 区域
vis[roi_top:h, :] = cv2.addWeighted(vis[roi_top:h, :], 0.7,
cv2.cvtColor(clean, cv2.COLOR_GRAY2BGR)[roi_top:h, :],
0.9, 0)
if contours:
largest = max(contours, key=cv2.contourArea)
area = cv2.contourArea(largest)
if area >= self.min_area_detect:
M = cv2.moments(largest)
if M['m00'] != 0:
cx = int(M['m10'] / M['m00'])
cy = int(M['m01'] / M['m00']) + roi_top
cv2.drawContours(vis, [largest + np.array([0, roi_top])], -1, (0, 255, 0), 2)
cv2.circle(vis, (cx, cy), 6, (0, 0, 255), -1)
cv2.line(vis, (w // 2, h), (cx, cy), (255, 0, 0), 2)
# ====== 巡线基础控制 ======
err = cx - (w / 2)
ang_white = - float(err) * self.angular_k
ratio = min(area / self.desired_area, 2.0)
linear_base = self.min_speed + (self.max_speed - self.min_speed) * (1.0 - ratio)
linear_base = float(np.clip(linear_base, self.min_speed, self.max_speed))
twist.angular.z = ang_white
twist.linear.x = linear_base
found = True
# ====== 红球推行融合（仅在暂停后启用） ======
if self._red_push_enabled:
red_res, red_mask = self.detect_red_ball(hsv)
if red_res is not None:
rcx, rcy, rarea = red_res
# 红球可视化
cv2.circle(vis, (rcx, rcy), 10, (0, 0, 255), -1)
cv2.putText(vis, f'RED {int(rarea)}', (10, 40),
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 2)
# 角度融合：白线为主，红球辅助
err_r = rcx - (w / 2)
ang_red = - float(err_r) * self.red_angular_k
twist.angular.z = (1.0 - self.red_influence) * ang_white + self.red_influence * ang_red
# 线速度：越近越慢
near = float(np.clip(rarea / max(1.0, self.max_red_area), 0.0, 1.0))
# 线速度在 [red_min_linear, max_speed*red_linear_scale] 按距离反比缩放
lin_cap = self.max_speed * self.red_linear_scale * (1.0 - near)
lin_cap = max(lin_cap, self.red_min_linear)
twist.linear.x = min(twist.linear.x, lin_cap)
if not found:
# 启动后但未检测到白区：停止（不旋转）
twist.linear.x = 0.0
twist.angular.z = 0.0
# ===== 黑块/黑字检测：达到时间且未触发过时检测 =====
if (not self._pause_triggered) and (elapsed >= self.stop_after_seconds):
# 优先在当前白色最大区域内搜索黑块；若没有白区则在 ROI 全区搜索
if largest is not None:
bx, by, bw_box, bh_box = cv2.boundingRect(largest)
sx, sy, sw_box, sh_box = bx, by, bw_box, bh_box
else:
sx, sy, sw_box, sh_box = 0, roi_top, w, h - roi_top
pad = 8
x1 = max(0, sx - pad)
y1 = max(roi_top, sy - pad)
x2 = min(w, sx + sw_box + pad)
y2 = min(h, sy + sh_box + pad)
if x2 > x1 and y2 > y1:
crop = frame[y1:y2, x1:x2]
gray = cv2.cvtColor(crop, cv2.COLOR_BGR2GRAY)
gray = cv2.GaussianBlur(gray, (5, 5), 0)
if self.black_detect_method == 'fixed_v':
v_crop = hsv[y1:y2, x1:x2, 2]
_, black_bin = cv2.threshold(v_crop, self.black_fixed_v, 255, cv2.THRESH_BINARY_INV)
black_bin = black_bin.astype('uint8')
else:
_, th = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
black_bin = cv2.bitwise_not(th)
k2 = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
black_clean = cv2.morphologyEx(black_bin, cv2.MORPH_OPEN, k2, iterations=1)
black_clean = cv2.morphologyEx(black_clean, cv2.MORPH_CLOSE, k2, iterations=2)
b_contours, _ = cv2.findContours(black_clean, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if b_contours:
for b in b_contours:
b_area = cv2.contourArea(b)
if b_area < self.min_black_area:
continue
BM = cv2.moments(b)
if BM['m00'] == 0:
continue
bcx_local = int(BM['m10'] / BM['m00'])
bcy_local = int(BM['m01'] / BM['m00'])
bcx = x1 + bcx_local
bcy = y1 + bcy_local
center_dist = abs(bcx - (w / 2))
if center_dist <= (self.center_tolerance_ratio * w):
# 触发暂停（一次性）
self._pause_triggered = True
self._paused_until = now + self.stop_duration
twist.linear.x = 0.0
twist.angular.z = 0.0
self.get_logger().info(
f'Black region detected at center (area={int(b_area)}), pausing for {self.stop_duration}s'
)
# 可视化黑块（映射回整图坐标）
b_shifted = b + np.array([[x1, y1]])
cv2.drawContours(vis, [b_shifted], -1, (0, 0, 255), 2)
cv2.putText(vis, f'BLACK {int(b_area)} - PAUSE', (10, 70),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
break
else:
# 未启动：等待绿色触发
twist.linear.x = 0.0
twist.angular.z = 0.0
cv2.putText(vis, 'WAITING FOR GREEN...', (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2)
# ===== 发布速度 =====
try:
self.cmd_pub.publish(twist)
except Exception as e:
self.get_logger().warn(f'publish cmd_vel failed: {e}')
# ===== 输出图像（带状态与绿色掩码缩略图） =====
try:
gvis = cv2.cvtColor(green_mask, cv2.COLOR_GRAY2BGR)
small = cv2.resize(gvis, (int(w * 0.25), int(h * 0.25)))
sw, sh = small.shape[1], small.shape[0]
vis[0:sh, w - sw:w] = cv2.addWeighted(vis[0:sh, w - sw:w], 0.6, small, 0.9, 0)
status_text = (
'PAUSED' if (self._paused_until is not None and time.monotonic() < self._paused_until)
else ('STARTED' if self.started else f'WAITING (green frames: {self._green_count}/{self.green_confirm_frames})')
)
extra = ' RED-PUSH:ON' if self._red_push_enabled else ''
info_text = f'{status_text}{extra} elapsed:{int(elapsed)}s'
cv2.putText(vis, info_text, (10, h - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 0), 2)
out_msg = self.bridge.cv2_to_imgmsg(vis, encoding='bgr8')
out_msg.header.stamp = self.get_clock().now().to_msg()
out_msg.header.frame_id = msg.header.frame_id if msg.header.frame_id else 'camera'
self.pub_img.publish(out_msg)
except Exception as e:
self.get_logger().warn(f'publish process_image failed: {e}')
if self.show_window:
cv2.imshow('white_area_follower', vis)
try:
if self.started and 'clean' in locals():
cv2.imshow('white_mask', clean)
except Exception:
pass
cv2.waitKey(1)
def destroy_node(self):
try:
if self.show_window:
cv2.destroyAllWindows()
except Exception:
pass
super().destroy_node()
def main(args=None):
rclpy.init(args=args)
node = WhiteAreaFollower()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.get_logger().info('shutting down')
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
这个识别红球的各个参数代表什么，把注释写详细一些
最新发布
本文解决了一项在使用JetsonNano和TI_mmwaveIWR1443进行自主机器人开发过程中遇到的编译错误。具体错误是在catkin_make时提示follower.cpp文件中的PLUGINLIB_DECLARE_CLASS语法不正确。通过将PLUGINLIB_DECLARE_CLASS更改为PLUGINLIB_EXPORT_CLASS成功解决了该问题。
扫一扫
898
到【灌水乐园】发言
