#!/usr/bin/env python
# coding=utf-8
import rospy
import cv2
import cv_bridge
import numpy
import os # 用于运行外部程序
import time # 用于计时功能
import subprocess # 用于管理外部进程
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Image
import math
class Follower:
def __init__(self):
# 初始化ROS相关组件
self.bridge = cv_bridge.CvBridge()
self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.image_callback)
self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
self.twist = Twist()
# 计时相关变量
self.primary_start_time = time.time() # 记录主开始时间
self.secondary_start_time = None # 记录恢复后的开始时间
self.third_start_time = None # 新增:记录renqunos1启动后的时间戳
self.elapsed_primary_time = 0.0 # 主经过时间
self.elapsed_secondary_time = 0.0 # 恢复后经过时间
self.elapsed_third_time = 0.0 # 新增:renqunos1启动后的经过时间
self.final_secondary_time = 0.0 # 记录停止时的二次时间
self.timer_stopped = False # 标记计时器是否已停止
self.secondary_phase = 0 # 二次计时阶段:0-初始阶段 1-fangos后阶段
# 状态标记变量
self.programs_executed = False # 标记初始外部程序是否已执行
self.loufangos_executed = False # 标记loufangos.py是否已执行
self.fangos_executed = False # 标记fangos.py是否已执行
self.renqunos_executed = False # 标记renqunos.py是否已执行
self.renqunos2_executed = False # 新增:标记renqunos2.py是否已执行
self.paused_primary_time = 0.0 # 记录暂停时已过去的主时间
self.navigation_restored = False # 标记是否已恢复巡线功能
# 控制参数整合
self.pid_params = {
'Kp': 0.082, 'Ki': 0.005, 'Kd': 0.05,
'output': 0.0, 'error': 0.0, 'last_error': 0.0,
'integral': 0.0, 'integral_limit': 50.0
}
self.angular_params = {
'max': 0.8, 'min': -0.5, 'rate_limit': 0.3,
'last': 0.0, 'smoothing': 0.3, 'current': 0.0
}
# 状态与检测参数
self.line_mode = "LINE_FOLLOWING"
self.last_valid_line = "NONE"
self.turn_threshold = 5.0
self.line_angle = 0.0
self.is_turning = False
self.min_line_area = 1000
self.max_line_area = 40000 # 最大轨迹面积阈值
self.line_area = 0
# 运动参数
self.speeds = {
'no_line_linear': 0.16,
'no_line_angular': 0.0,
'turning_linear': 0.16,
'normal_linear': 0.16,
'straight_linear': 0.2 # 直行模式线速度
}
# 可视化样式参数
self.colors = {
'panel': (40, 40, 40), 'text': (255, 255, 255),
'highlight': (0, 255, 255), 'border': (0, 255, 0),
'roi': (255, 165, 0), 'target_line': (0, 255, 0),
'marker': (0, 0, 255), # 红色标记点 (B, G, R)
'marker_border': (255, 255, 255),
'dashboard_bg': (30, 30, 30), 'dashboard_border': (100, 100, 100),
'speed': (0, 255, 0), 'angular': (0, 0, 255),
'tick': (200, 200, 200), 'label': (255, 255, 255),
'needle_head': (255, 255, 0)
}
self.style = {
'panel_alpha': 0.7, 'dashboard_alpha': 0.8,
'padding': 8, 'font': cv2.FONT_HERSHEY_SIMPLEX,
'font_scale': 0.6, 'watermark_scale': 0.8,
'watermark_thickness': 2,
'line_type': cv2.LINE_AA if hasattr(cv2, 'LINE_AA') else 1
}
# 存储需要管理的ROS launch进程标识
self.launch_identifiers = [
"easy_handeye publish.launch",
"apriltag_ros continuous_detection.launch",
"mirobot_moveit_config mirobot.launch",
"jxblaunch程序",
"mirobot.launch"
]
def reset_pid(self):
"""重置PID控制器参数"""
pid = self.pid_params
pid['output'] = pid['error'] = pid['last_error'] = pid['integral'] = 0.0
self.angular_params['last'] = 0.0
def smooth_angular(self, target_angular):
"""平滑角速度输出"""
ap = self.angular_params
delta = target_angular - ap['last']
# 限制角速度变化率
if delta > ap['rate_limit']:
target_angular = ap['last'] + ap['rate_limit']
elif delta < -ap['rate_limit']:
target_angular = ap['last'] - ap['rate_limit']
# 平滑处理
smoothed = (1 - ap['smoothing']) * target_angular + ap['smoothing'] * ap['last']
smoothed = max(min(smoothed, ap['max']), ap['min'])
ap['last'] = ap['current'] = smoothed
return smoothed
def calculate_line_angle(self, mask):
"""计算线条相对水平线的角度"""
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if not contours:
return 0.0
max_contour = max(contours, key=cv2.contourArea)
rect = cv2.minAreaRect(max_contour)
angle = rect[2]
# 角度标准化
if angle < -45:
angle += 90
elif angle > 45:
angle -= 90
return abs(angle)
def draw_transparent_panel(self, image, x, y, width, height):
"""绘制半透明面板"""
overlay = image.copy()
cv2.rectangle(overlay, (x, y), (x + width, y + height), self.colors['panel'], -1)
cv2.addWeighted(overlay, self.style['panel_alpha'], image, 1 - self.style['panel_alpha'], 0, image)
cv2.rectangle(image, (x, y), (x + width, y + height), self.colors['border'], 1, self.style['line_type'])
def draw_dashboard(self, image):
"""绘制带指针的仪表盘"""
h, w = image.shape[:2]
speed_center = (w//4, h - 100)
angular_center = (3*w//4, h - 100)
radius = 50
overlay = image.copy()
# 绘制速度表盘
self.draw_gauge(overlay, speed_center, radius, "SPEED", 0, 0.4,
self.twist.linear.x, self.colors['speed'], -150, -30)
# 绘制角速度表盘
self.draw_gauge(overlay, angular_center, radius, "ANGULAR", -0.8, 0.8,
self.angular_params['current'], self.colors['angular'], 30, 150)
cv2.addWeighted(overlay, self.style['dashboard_alpha'], image, 1 - self.style['dashboard_alpha'], 0, image)
def draw_gauge(self, overlay, center, radius, label, min_val, max_val, current_val, color, start_angle, end_angle):
"""绘制单个指针式仪表"""
# 绘制表盘背景
cv2.circle(overlay, center, radius, self.colors['dashboard_bg'], -1)
cv2.circle(overlay, center, radius, self.colors['dashboard_border'], 2, self.style['line_type'])
# 绘制刻度
for i in range(7): # 6+1个刻度
angle = start_angle + (end_angle - start_angle) * i / 6
rad = math.radians(angle)
tick_start = (
center[0] + (radius - 8) * math.cos(rad),
center[1] + (radius - 8) * math.sin(rad)
)
tick_end = (
center[0] + radius * math.cos(rad),
center[1] + radius * math.sin(rad)
)
cv2.line(overlay, (int(tick_start[0]), int(tick_start[1])),
(int(tick_end[0]), int(tick_end[1])),
self.colors['tick'], 2, self.style['line_type'])
# 计算指针角度
val_range = max_val - min_val
normalized = 0 if val_range == 0 else max(0, min(1, (current_val - min_val) / val_range))
pointer_angle = start_angle + (end_angle - start_angle) * normalized
pointer_rad = math.radians(pointer_angle)
# 绘制指针
pointer_end = (
center[0] + (radius - 10) * math.cos(pointer_rad),
center[1] + (radius - 10) * math.sin(pointer_rad)
)
cv2.line(overlay, center, (int(pointer_end[0]), int(pointer_end[1])),
color, 3, self.style['line_type'])
cv2.circle(overlay, (int(pointer_end[0]), int(pointer_end[1])), 5,
self.colors['needle_head'], -1, self.style['line_type'])
cv2.circle(overlay, center, 4, (255, 255, 255), -1, self.style['line_type'])
# 绘制标签和数值
cv2.putText(overlay, label, (center[0] - 30, center[1] + radius + 15),
self.style['font'], 0.5, self.colors['label'], 1, self.style['line_type'])
cv2.putText(overlay, "%.2f" % current_val,
(center[0] - 20, center[1] - radius - 10),
self.style['font'], 0.45, self.colors['label'], 1, self.style['line_type'])
def draw_watermark(self, image):
"""绘制水印"""
h, w = image.shape[:2]
text = "GiveMeFive"
text_size = cv2.getTextSize(text, self.style['font'], self.style['watermark_scale'],
self.style['watermark_thickness'])[0]
text_pos = ((w - text_size[0]) // 2, h - 20)
# 绘制带边框文本
cv2.putText(image, text, text_pos, self.style['font'], self.style['watermark_scale'],
(0, 0, 0), 3, self.style['line_type'])
cv2.putText(image, text, text_pos, self.style['font'], self.style['watermark_scale'],
self.colors['text'], self.style['watermark_thickness'], self.style['line_type'])
def draw_roi_border(self, image, left, right, top, bottom):
"""绘制ROI虚线边框"""
line_len, gap_len = 10, 5
# 顶部边框
x = left
while x < right:
cv2.line(image, (x, top), (min(x + line_len, right), top),
self.colors['roi'], 2, self.style['line_type'])
x += line_len + gap_len
# 底部边框
x = left
while x < right:
cv2.line(image, (x, bottom), (min(x + line_len, right), bottom),
self.colors['roi'], 2, self.style['line_type'])
x += line_len + gap_len
# 左侧边框
y = top
while y < bottom:
cv2.line(image, (left, y), (left, min(y + line_len, bottom)),
self.colors['roi'], 2, self.style['line_type'])
y += line_len + gap_len
# 右侧边框
y = top
while y < bottom:
cv2.line(image, (right, y), (right, min(y + line_len, bottom)),
self.colors['roi'], 2, self.style['line_type'])
y += line_len + gap_len
def kill_ros_launch(self, launch_name):
"""关闭指定的ROS launch进程"""
try:
os.system("pkill -f '{}'".format(launch_name))
rospy.loginfo("已关闭ROS launch: {}".format(launch_name))
except Exception as e:
rospy.logerr("关闭ROS launch {} 失败: {}".format(launch_name, str(e)))
def execute_initial_programs(self):
"""执行初始外部程序并关闭相关launch"""
rospy.loginfo("到达18.3秒,准备执行初始外部程序...")
# 停止机器人运动
self.twist.linear.x = 0.0
self.twist.angular.z = 0.0
self.cmd_vel_pub.publish(self.twist)
rospy.loginfo("机器人已停止")
# 运行初始外部程序
rospy.loginfo("开始运行jxbos.py...")
os.system("python /home/eaibot/rt_ws/src/shibie/jxbos.py")
rospy.loginfo("开始运行5.py...")
os.system("python /home/eaibot/rt_ws/src/shibie/jxb/5.py")
rospy.loginfo("初始外部程序执行完毕,准备关闭ROS launch并恢复巡线功能")
# 关闭所有指定的ROS launch文件
for launch in self.launch_identifiers:
self.kill_ros_launch(launch)
# 记录暂停时间并初始化二次计时器(第一阶段)
self.paused_primary_time = self.elapsed_primary_time
self.secondary_start_time = time.time()
self.secondary_phase = 0 # 标记为初始二次计时阶段
rospy.loginfo("ROS launch已关闭,巡线功能已恢复")
self.reset_pid()
def execute_loufang_program(self):
"""执行loufangos.py程序(不停车,保持巡线)"""
rospy.loginfo("二次计时达到10秒,启动loufangos1.py程序(保持巡线)...")
# 启动程序但不停止机器人运动
os.system("python /home/eaibot/rt_ws/src/shibie/loufangos1.py &") # 后台运行以不阻塞巡线
self.loufangos_executed = True
rospy.loginfo("loufangos.py程序启动完成,继续巡线...")
def execute_fang_program(self):
"""执行fangos.py程序(停车)"""
rospy.loginfo("二次计时达到13秒,停止小车并启动fangos.py程序...")
# 停止小车运动
self.twist.linear.x = 0.0
self.twist.angular.z = 0.0
self.cmd_vel_pub.publish(self.twist)
# 执行fangos.py程序
rospy.loginfo("开始运行fangos.py...")
os.system("python /home/eaibot/rt_ws/src/shibie/fangos.py")
# 程序执行完毕后关闭launch并恢复巡线
for launch in self.launch_identifiers:
self.kill_ros_launch(launch)
self.fangos_executed = True
self.navigation_restored = True
self.reset_pid()
# 重置二次计时器,进入第二阶段计时
self.timer_stopped = False # 重新启动计时器
self.secondary_phase = 1 # 标记为fangos后的二次计时阶段
self.secondary_start_time = time.time() # 重置计时起点
self.elapsed_secondary_time = 0.0 # 重置已过时间
rospy.loginfo("fangos.py程序执行完毕,已重置二次计时器并恢复巡线功能")
def execute_renqu_program(self):
"""执行renqunos.py程序(不停车,保持巡线)"""
rospy.loginfo("fangos后二次计时达到8秒,启动renqunos1.py程序(保持巡线)...")
# 不停止小车,保持当前运动状态
# 执行renqunos.py程序(后台运行不阻塞巡线)
rospy.loginfo("开始运行renqunos1.py...")
os.system("python /home/eaibot/rt_ws/src/shibie/renqunos1.py &") # 使用&后台运行
# 新增:初始化第三个时间戳
self.third_start_time = time.time()
self.elapsed_third_time = 0.0
# 程序执行完毕后标记状态
self.renqunos_executed = True
rospy.loginfo("renqunos1.py程序启动完成,继续巡线...")
def execute_renqu2_program(self):
"""新增:执行renqunos2.py程序(不停车,保持巡线)"""
rospy.loginfo("第三个时间戳达到18秒,启动renqunos2.py程序(保持巡线)...")
# 不停止小车,保持当前运动状态
# 执行renqunos2.py程序(后台运行不阻塞巡线)
rospy.loginfo("开始运行renqunos2.py...")
os.system("python /home/eaibot/rt_ws/src/shibie/renqunos2.py &") # 使用&后台运行
# 程序执行完毕后标记状态
self.renqunos2_executed = True
rospy.loginfo("renqunos2.py程序启动完成,继续巡线...")
def image_callback(self, msg):
# 计算经过的时间
if not self.programs_executed:
self.elapsed_primary_time = time.time() - self.primary_start_time
else:
if self.secondary_start_time and not self.timer_stopped:
self.elapsed_secondary_time = time.time() - self.secondary_start_time
# 新增:计算第三个时间戳(renqunos1启动后)
if self.third_start_time and not self.timer_stopped and self.renqunos_executed:
self.elapsed_third_time = time.time() - self.third_start_time
# 初始阶段:检查是否到达18.2秒执行初始程序
if not self.programs_executed and self.elapsed_primary_time >= 20.27:
self.execute_initial_programs()
self.programs_executed = True
return
# 二次计时第一阶段:检查10秒执行loufangos.py(保持巡线)
if (self.programs_executed and self.secondary_phase == 0 and
not self.loufangos_executed and not self.timer_stopped and
self.elapsed_secondary_time >= 16):
self.execute_loufang_program()
# 不返回,继续执行巡线逻辑
# 二次计时第一阶段:检查13秒执行fangos.py(停车)
if (self.programs_executed and self.secondary_phase == 0 and
self.loufangos_executed and not self.fangos_executed and
not self.timer_stopped and self.elapsed_secondary_time >= 22.8):
self.execute_fang_program()
return
# 二次计时第二阶段(fangos后):检查8秒执行renqunos1.py
if (self.programs_executed and self.secondary_phase == 1 and
self.fangos_executed and not self.renqunos_executed and
not self.timer_stopped and self.elapsed_secondary_time >= 8.2):
self.execute_renqu_program()
# 不返回,继续执行巡线逻辑
# 新增:第三个时间戳监控:检查12秒执行renqunos2.py
if (self.programs_executed and self.secondary_phase == 1 and
self.renqunos_executed and not self.renqunos2_executed and
not self.timer_stopped and self.elapsed_third_time >= 18):
self.execute_renqu2_program()
# 不返回,继续执行巡线逻辑
# 图像转换与预处理
image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
# 黑色线条检测
mask = cv2.inRange(hsv, (0, 0, 0), (85, 55, 65))
mask = cv2.medianBlur(mask, 5)
# 定义并应用ROI
h, w, d = image.shape
search = {'left': w//4-140, 'right': 3*w//4-160, 'top':0, 'bottom': h*2//3 +100}
mask[0:search['top'], :] = mask[search['bottom']:h, :] = 0
mask[:, 0:search['left']] = mask[:, search['right']:w] = 0
# 计算线条面积并绘制ROI边框
self.line_area = cv2.countNonZero(mask)
self.draw_roi_border(image, **search)
# 线条检测与控制逻辑(始终保持巡线)
if not self.timer_stopped or self.navigation_restored:
pid = self.pid_params
M = cv2.moments(mask)
if M['m00'] > 0 and self.line_area > self.min_line_area:
# 大面积轨迹处理
if self.line_area > self.max_line_area:
self.line_mode = "STRAIGHT_LINE"
self.twist.linear.x = self.speeds['straight_linear']
self.twist.angular.z = 0.0
self.angular_params['current'] = 0.0
self.last_valid_line = "WIDE"
self.is_turning = False
self.line_angle = 0.0
else:
# 常规巡线模式
self.line_mode = "LINE_FOLLOWING"
cx, cy = int(M['m10']/M['m00']), int(M['m01']/M['m00'])
# 中心点调整
cx = cx - 200
# 绘制中心标记
cv2.circle(image, (cx, cy), 8, self.colors['marker'], -1)
cv2.circle(image, (cx, cy), 9, self.colors['marker_border'], 1, self.style['line_type'])
# 绘制目标线
desired_h = h*2//3 - 225
cv2.line(image, (0, desired_h), (w, desired_h), self.colors['target_line'], 2, self.style['line_type'])
cv2.arrowedLine(image, (w-30, desired_h), (w, desired_h), self.colors['target_line'], 2, self.style['line_type'])
cv2.arrowedLine(image, (30, desired_h), (0, desired_h), self.colors['target_line'], 2, self.style['line_type'])
# 角度计算与转弯检测
self.line_angle = self.calculate_line_angle(mask)
self.is_turning = self.line_angle > self.turn_threshold
# PID控制计算
pid['last_error'] = pid['error']
pid['error'] = desired_h - cy
pid['integral'] = max(min(pid['integral'] + pid['error'], pid['integral_limit']), -pid['integral_limit'])
pid['output'] = pid['Kp']*pid['error'] + pid['Ki']*pid['integral'] + pid['Kd']*(pid['error'] - pid['last_error'])
# 设置运动参数(始终保持巡线)
self.twist.linear.x = self.speeds['turning_linear'] if self.is_turning else self.speeds['normal_linear']
self.smooth_angular(-pid['output'] / 15)
self.twist.angular.z = self.angular_params['current']
self.last_valid_line = "FOUND"
else:
# 无线模式
self.line_mode = "NO_LINE"
self.twist.linear.x = self.speeds['no_line_linear']
self.twist.angular.z = self.speeds['no_line_angular']
self.angular_params['current'] = self.speeds['no_line_angular']
self.last_valid_line = "LOST" if self.line_area <= self.min_line_area else "SMALL"
self.is_turning = False
self.line_angle = 0.0
else:
# 计时停止且未恢复巡线时保持停止状态
self.twist.linear.x = 0.0
self.twist.angular.z = 0.0
self.angular_params['current'] = 0.0
self.line_mode = "STOPPED"
self.last_valid_line = "STOPPED"
# 时间显示
cv2.putText(image, "Primary Time: %.1fs" % self.elapsed_primary_time, (18, 130), self.style['font'], self.style['font_scale'],
self.colors['text'], 1, self.style['line_type'])
if self.programs_executed:
secondary_time = self.final_secondary_time if self.timer_stopped else self.elapsed_secondary_time
# 显示当前二次计时阶段
phase_text = " (Phase {})".format(self.secondary_phase) if self.secondary_phase > 0 else ""
time_text = "Secondary Time: %.1fs%s" % (secondary_time, phase_text)
color = self.colors['highlight'] if not self.timer_stopped else (0, 165, 255)
cv2.putText(image, time_text, (18, 150), self.style['font'], self.style['font_scale'],
color, 1, self.style['line_type'])
# 新增:显示第三个时间戳
if self.renqunos_executed:
third_text = "Third Time: %.1fs" % self.elapsed_third_time
cv2.putText(image, third_text, (18, 170), self.style['font'], self.style['font_scale'],
(255, 100, 100), 1, self.style['line_type'])
# 绘制信息面板
panel_height = 220 if self.programs_executed else 140 # 增加高度以显示更多状态
panel = {'w': 220, 'h': panel_height, 'offset': 20}
# 系统状态面板
self.draw_transparent_panel(image, 10, 10, panel['w'], panel['h'])
cv2.putText(image, "SYSTEM STATUS", (18, 30), self.style['font'], self.style['font_scale'],
self.colors['highlight'], 1, self.style['line_type'])
cv2.putText(image, "Mode: %s" % self.line_mode, (18, 50), self.style['font'], self.style['font_scale'],
self.colors['text'], 1, self.style['line_type'])
cv2.putText(image, "Line: %s" % self.last_valid_line, (18, 70), self.style['font'], self.style['font_scale'],
self.colors['text'], 1, self.style['line_type'])
cv2.putText(image, "Linear: %.2f m/s" % self.twist.linear.x, (18, 90), self.style['font'], self.style['font_scale'],
self.colors['text'], 1, self.style['line_type'])
cv2.putText(image, "Angular: %.3f rad/s" % self.angular_params['current'], (18, 110), self.style['font'], self.style['font_scale'],
self.colors['text'], 1, self.style['line_type'])
# 二次阶段状态显示
if self.programs_executed:
cv2.putText(image, "Loufangos: %s" % ("DONE" if self.loufangos_executed else "PENDING"),
(18, 130), self.style['font'], self.style['font_scale'],
self.colors['highlight'] if self.loufangos_executed else self.colors['text'], 1, self.style['line_type'])
cv2.putText(image, "Fangos: %s" % ("DONE" if self.fangos_executed else "PENDING"),
(18, 150), self.style['font'], self.style['font_scale'],
self.colors['highlight'] if self.fangos_executed else self.colors['text'], 1, self.style['line_type'])
# 显示renqunos执行状态
cv2.putText(image, "Renqunos1: %s" % ("DONE" if self.renqunos_executed else "PENDING"),
(18, 170), self.style['font'], self.style['font_scale'],
self.colors['highlight'] if self.renqunos_executed else self.colors['text'], 1, self.style['line_type'])
# 新增:显示renqunos2执行状态
cv2.putText(image, "Renqunos2: %s" % ("DONE" if self.renqunos2_executed else "PENDING"),
(18, 190), self.style['font'], self.style['font_scale'],
self.colors['highlight'] if self.renqunos2_executed else self.colors['text'], 1, self.style['line_type'])
cv2.putText(image, "Navigation: %s" % ("RESTORED" if self.navigation_restored else "ACTIVE"),
(18, 210), self.style['font'], self.style['font_scale'],
self.colors['highlight'] if self.navigation_restored else self.colors['text'], 1, self.style['line_type'])
# 线条分析面板
self.draw_transparent_panel(image, w - panel['w'] - 10, 10, panel['w'], panel['h'])
cv2.putText(image, "LINE ANALYSIS", (w - panel['w'] + 8, 30), self.style['font'], self.style['font_scale'],
self.colors['highlight'], 1, self.style['line_type'])
cv2.putText(image, "Angle: %.1f deg" % self.line_angle, (w - panel['w'] + 8, 50), self.style['font'], self.style['font_scale'],
self.colors['text'], 1, self.style['line_type'])
cv2.putText(image, "Turning: %s" % ("YES" if self.is_turning else "NO"), (w - panel['w'] + 8, 70), self.style['font'], self.style['font_scale'],
self.colors['text'], 1, self.style['line_type'])
cv2.putText(image, "Area: %d/%d px" % (self.line_area, self.min_line_area), (w - panel['w'] + 8, 90), self.style['font'], self.style['font_scale'],
self.colors['text'], 1, self.style['line_type'])
cv2.putText(image, "Max Area: %d" % self.max_line_area, (w - panel['w'] + 8, 110), self.style['font'], self.style['font_scale'],
self.colors['text'], 1, self.style['line_type'])
# 程序执行状态
status_text = "Post-Program: Active" if self.programs_executed else "Pre-Program: Active"
cv2.putText(image, status_text, (w - panel['w'] + 8, 130), self.style['font'], self.style['font_scale'],
self.colors['highlight'] if self.programs_executed else self.colors['text'], 1, self.style['line_type'])
# 计时器状态
timer_text = "Timer: STOPPED" if self.timer_stopped else "Timer: RUNNING"
cv2.putText(image, timer_text, (w - panel['w'] + 8, 150), self.style['font'], self.style['font_scale'],
(0, 165, 255) if self.timer_stopped else self.colors['text'], 1, self.style['line_type'])
# 显示当前二次计时阶段
if self.programs_executed:
phase_status = "Phase {} Active".format(self.secondary_phase) if self.secondary_phase > 0 else "Initial Phase"
cv2.putText(image, "Secondary: {}".format(phase_status), (w - panel['w'] + 8, 170), self.style['font'], self.style['font_scale'],
self.colors['highlight'] if self.secondary_phase > 0 else self.colors['text'], 1, self.style['line_type'])
# 新增:显示第三个计时器状态
if self.renqunos_executed:
third_status = "Third Timer: Active" if not self.renqunos2_executed else "Third Timer: Completed"
cv2.putText(image, "{}".format(third_status), (w - panel['w'] + 8, 190), self.style['font'], self.style['font_scale'],
(255, 100, 100) if not self.renqunos2_executed else self.colors['highlight'], 1, self.style['line_type'])
# 绘制仪表盘和水印
self.draw_dashboard(image)
self.draw_watermark(image)
# 发布指令与显示图像
self.cmd_vel_pub.publish(self.twist)
cv2.imshow("Camera View", image)
cv2.imshow("Mask View", mask)
cv2.waitKey(3)
if __name__ == "__main__":
rospy.init_node("opencv_line_follower")
follower = Follower()
rospy.spin()
上面是第一个程序叫做xunxian20.py,用来巡线。(python2.7)
修改上下两个红绿灯识别部分不太完善的代码。然后给我完整的修复过的,按照各自版本的代码。
修改方式为,当xunxian20.py的第三个时间戳结束后启动第四个时间戳当第四个时间戳到达8秒后。打开一个关于摄像头的launch用来发布图像话题,具体语句为"roslaunch usb_cam usb_cam-test1.launch",如果此launch已经启动则跳过,当此launch启动完毕后。启动位于“/home/eaibot/rt_ws/src/shibie/light”路径下且在虚拟环境名为yolov5的虚拟环境中的lightdetect.py代码。lightdetect.py会订阅上面启动的launch发布的图像,以固定频率订阅话题,话题名为'/usb_cam1/image_raw',并且以固定频率检测这些订阅的图片。第一种情况当第一次检测的结果是green时退出lightdetect.py程序,然后退出"roslaunch usb_cam usb_cam-test1.launch"启动的launch。这一过程从启动launch到关闭launch都是一直保持巡线的,小车不会停下。第二种情况,当第一次检测的结果是yellow和red时阻塞xunxian20.py里用“roslaunch xpkg_bringup bringup_basic_ctrl.launch”发布的
'/cmd_vel'话题,也就是说依旧保持巡线但是小车静止不动,然后再次订阅并检测图片,直到检测出green后退出lightdetect,py程序,然后关闭"roslaunch usb_cam usb_cam-test1.launch"启动的launch然后停止对'/cmd_vel'话题的阻塞。也就是说整个过程小车一直保持巡线唯一一种停下的情况是检测出red和yellow时阻塞“/cmd_vel"的接收使小车暂时停止。
下面是第二个程序叫做lightdetect.py,用来对红绿灯做出检测和判断。(python3)
#!/usr/bin/python
# -*- coding: utf-8 -*-
import argparse
import os
import platform
import sys
from pathlib import Path
import numpy as np
import yaml
import torch
import cv2
import csv
from ultralytics.utils.plotting import Annotator, colors
# ===== 处理pathlib跨平台路径问题 =====
import pathlib
plt = platform.system()
if plt != 'Windows':
pathlib.WindowsPath = pathlib.PosixPath
# 再次强化路径兼容性
if platform.system() != "Windows":
from pathlib import PosixPath
Path.WindowsPath = PosixPath
# ROS相关导入
import rospy
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
# 自定义CvBridge解决兼容性问题
class CvBridge:
def imgmsg_to_cv2(self, img_msg, encoding):
if encoding == "bgr8":
if img_msg.encoding not in ["bgr8", "rgb8"]:
raise Exception(f"图像编码不匹配: 期望bgr8或rgb8, 实际{img_msg.encoding}")
dtype = np.uint8
image = np.frombuffer(img_msg.data, dtype=dtype).reshape(
img_msg.height, img_msg.width, 3
)
if img_msg.encoding == "rgb8":
image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
return image
else:
raise Exception(f"不支持的编码格式: {encoding}")
def cv2_to_imgmsg(self, cv_image, encoding="bgr8"):
img_msg = Image()
img_msg.height = cv_image.shape[0]
img_msg.width = cv_image.shape[1]
img_msg.encoding = encoding
img_msg.is_bigendian = 0
img_msg.data = cv_image.tobytes()
img_msg.step = img_msg.width * 3
return img_msg
# 添加YOLOv5根目录到系统路径
current_script_path = os.path.abspath(__file__)
current_dir = os.path.dirname(current_script_path)
yolov5_root = os.path.dirname(current_dir)
sys.path.insert(0, yolov5_root)
# 打印路径信息用于调试
print(f"当前脚本路径: {current_script_path}")
print(f"YOLOv5根目录: {yolov5_root}")
print(f"当前工作目录: {os.getcwd()}")
print("系统路径:")
for p in sys.path:
print(f"- {p}")
# 导入YOLOv5模块
from models.common import DetectMultiBackend
from utils.general import (
check_file, check_img_size, check_imshow, non_max_suppression,
scale_boxes, xyxy2xywh, increment_path
)
from utils.torch_utils import select_device
# ROS相关全局变量
bridge = CvBridge()
cmd_vel_pub = None
image_received = False # 全局变量
latest_image = None # 全局变量
# 交通灯颜色与指示映射(确保与模型类别名称一致)
TRAFFIC_LIGHT_INDICATION = {
'red': '停车',
'yellow': '减速',
'green': '通行'
}
# 路径处理工具函数
def convert_windows_path_to_posix(obj):
"""递归将WindowsPath对象转换为PosixPath对象"""
if isinstance(obj, dict):
return {k: convert_windows_path_to_posix(v) for k, v in obj.items()}
elif isinstance(obj, list):
return [convert_windows_path_to_posix(v) for v in obj]
elif isinstance(obj, Path):
return Path(str(obj))
else:
return obj
# 模型加载函数
def load_model_safely(weights_path, data_path, device):
"""安全加载模型,处理跨平台路径问题"""
print(f"尝试安全加载模型: {weights_path}")
try:
model = DetectMultiBackend(weights_path, device=device, data=data_path)
stride = model.stride
names = model.names
print(f"成功安全加载模型,步长: {stride}, 类别: {names}") # 打印模型实际类别名称
return model
except Exception as e:
print(f"安全加载模型失败: {e}")
try:
checkpoint = torch.load(weights_path, map_location=device)
checkpoint = convert_windows_path_to_posix(checkpoint)
if isinstance(checkpoint, dict) and 'model' in checkpoint:
model = checkpoint['model'].float().fuse().eval()
else:
model = checkpoint.float().fuse().eval()
with open(data_path, 'r') as f:
data_dict = yaml.safe_load(f)
if 'names' in data_dict:
model.names = data_dict['names']
print(f"模型类别名称: {model.names}") # 打印模型实际类别名称
stride = model.stride.max() if hasattr(model, 'stride') else 32
print(f"成功使用激进方法加载模型,步长: {stride}")
return model
except Exception as e2:
print(f"激进方法加载模型失败: {e2}")
return None
# ROS图像回调函数
def image_callback(msg):
global latest_image, image_received
try:
latest_image = bridge.imgmsg_to_cv2(msg, "bgr8")
image_received = True
except Exception as e:
print(f"图像转换错误: {e}")
def run(
weights='best.pt',
data='light.yaml',
imgsz=640,
conf_thres=0.3, # 降低置信度阈值,提高黄灯检测灵敏度
iou_thres=0.45,
max_det=1000,
device='',
view_img=False,
save_txt=False,
save_crop=False,
nosave=False,
classes=None,
line_thickness=18,
):
global cmd_vel_pub, image_received, latest_image
# 初始化ROS节点
rospy.init_node('traffic_light_detector', anonymous=True)
cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
print("已创建/cmd_vel发布器")
# 订阅摄像头图像话题
rospy.Subscriber('/usb_cam1/image_raw', Image, image_callback)
print("已订阅 /usb_cam1/image_raw 话题")
# 加载模型
device = select_device(device)
weights_path = os.path.join(current_dir, weights)
data_path = os.path.join(current_dir, data)
print(f"尝试加载模型: {weights_path}")
print(f"尝试加载数据配置: {data_path}")
# 检查文件存在性
if not os.path.exists(weights_path):
print(f"错误: 权重文件不存在: {weights_path}")
print(f"当前目录内容: {os.listdir(current_dir)}")
sys.exit(1)
if not os.path.exists(data_path):
print(f"错误: 数据配置文件不存在: {data_path}")
sys.exit(1)
# 加载模型
model = load_model_safely(weights_path, data_path, device)
if model is None:
print("所有加载方式均失败,程序无法继续")
sys.exit(1)
# 获取步长
stride = getattr(model, 'stride', 32)
if isinstance(stride, torch.Tensor):
stride = stride.max().item()
imgsz = check_img_size(imgsz, s=stride)
# 创建保存目录
save_dir = increment_path(Path('runs/detect/exp'), exist_ok=False)
(save_dir / 'labels' if save_txt else save_dir).mkdir(parents=True, exist_ok=True)
# 模型预热
print("正在手动预热模型...")
im = torch.zeros(1, 3, imgsz, imgsz).to(device)
_ = model(im) if device.type != 'cpu' else None
# 等待接收第一帧图像
print("等待接收图像...")
while not image_received and not rospy.is_shutdown():
rospy.sleep(0.1)
if rospy.is_shutdown():
print("ROS节点已关闭")
return
print("开始检测交通灯...")
rate = rospy.Rate(10) # 10Hz
should_exit = False
frame_count = 0 # 用于生成唯一的图像文件名
# 连续检测计数器,避免瞬时检测导致误判
yellow_detected_count = 0
red_detected_count = 0
green_detected_count = 0
required_consecutive_detections = 2 # 需要连续2帧检测到才生效
consecutive_green_to_exit = 3 # 需要连续检测到绿灯3次才退出
while not rospy.is_shutdown() and not should_exit:
if not image_received:
rate.sleep()
continue
# 使用最新接收到的图像
im0 = latest_image.copy()
image_received = False
frame_count += 1 # 递增帧计数
# 图像预处理
im = cv2.resize(im0, (imgsz, imgsz))
im = cv2.cvtColor(im, cv2.COLOR_BGR2RGB)
im = im.transpose(2, 0, 1)
im = np.ascontiguousarray(im)
im = torch.from_numpy(im).to(device)
im = im.float() / 255.0
if len(im.shape) == 3:
im = im.unsqueeze(0)
# 模型推理
pred = model(im)
pred = non_max_suppression(pred, conf_thres, iou_thres, classes, max_det=max_det)
# 处理检测结果
detected_classes = set()
for i, det in enumerate(pred):
save_path = str(save_dir / f"frame_{frame_count}.jpg")
txt_path = str(save_dir / 'labels' / f"frame_{frame_count}")
if len(det):
# 调整边界框尺寸
det[:, :4] = scale_boxes(im.shape[2:], det[:, :4], im0.shape).round()
# 收集检测到的类别(转换为小写,增强兼容性)
detected_classes = {model.names[int(c)].lower() for c in det[:, -1].unique()}
traffic_light_info = [
f"{cls}({TRAFFIC_LIGHT_INDICATION.get(cls, '未知')})"
for cls in detected_classes
]
print(f"检测到目标: {', '.join(traffic_light_info)}")
# 绘制边界框
annotator = Annotator(im0, line_width=line_thickness)
for *xyxy, conf, cls in reversed(det):
c = int(cls)
label = f"{model.names[c]} {conf:.2f}"
annotator.box_label(xyxy, label, color=colors(c, True))
if save_txt:
xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4))).view(-1).tolist()
with open(f"{txt_path}.txt", 'a') as f:
f.write(f"{c} {' '.join(map(str, xywh))} {conf:.2f}\n")
if save_crop:
cv2.imwrite(f"{save_dir}/{model.names[c]}_frame_{frame_count}.jpg",
im0[int(xyxy[1]):int(xyxy[3]), int(xyxy[0]):int(xyxy[2])])
im0 = annotator.result()
# 显示结果
if view_img:
cv2.imshow("Traffic Light Detection", im0)
if cv2.waitKey(1) == ord('q'):
break
# 保存结果图像
if not nosave:
cv2.imwrite(save_path, im0)
# 小车控制逻辑(优化版本)
twist = Twist()
# 重置计数器(如果未检测到对应颜色)
if 'yellow' not in detected_classes:
yellow_detected_count = 0
if 'red' not in detected_classes:
red_detected_count = 0
if 'green' not in detected_classes:
green_detected_count = 0
# 连续检测计数
if 'yellow' in detected_classes:
yellow_detected_count += 1
print(f"连续检测到黄灯 {yellow_detected_count}/{required_consecutive_detections}")
if 'red' in detected_classes:
red_detected_count += 1
print(f"连续检测到红灯 {red_detected_count}/{required_consecutive_detections}")
if 'green' in detected_classes:
green_detected_count += 1
print(f"连续检测到绿灯 {green_detected_count}/{consecutive_green_to_exit}")
# 控制逻辑:连续检测到指定次数才执行动作
if red_detected_count >= required_consecutive_detections:
twist.linear.x = 0.0
twist.angular.z = 0.0
cmd_vel_pub.publish(twist)
print("检测到红灯,小车已停止")
elif yellow_detected_count >= required_consecutive_detections:
twist.linear.x = 0.0 # 黄灯也停止(或根据需求改为减速)
twist.angular.z = 0.0
cmd_vel_pub.publish(twist)
print("检测到黄灯,小车已停止")
elif green_detected_count >= consecutive_green_to_exit:
print(f"连续检测到绿灯 {consecutive_green_to_exit} 次,退出红绿灯检测")
should_exit = True
# 未检测到需停止的灯时,不发布指令(避免覆盖巡线指令)
# else:
# # 不发布任何指令,保持巡线程序的控制
# pass
rate.sleep()
# 清理资源
cv2.destroyAllWindows()
print("红绿灯检测程序已退出")
def parse_opt():
parser = argparse.ArgumentParser()
parser.add_argument('--weights', type=str, default='best.pt', help='模型路径')
parser.add_argument('--data', type=str, default='light.yaml', help='数据集配置')
parser.add_argument('--imgsz', type=int, default=640, help='推理图像尺寸')
parser.add_argument('--conf-thres', type=float, default=0.3, help='置信度阈值') # 降低阈值
parser.add_argument('--iou-thres', type=float, default=0.45, help='NMS IoU阈值')
parser.add_argument('--device', type=str, default='', help='设备')
parser.add_argument('--view-img', action='store_true', help='显示结果')
parser.add_argument('--save-txt', action='store_true', help='保存为txt')
parser.add_argument('--save-crop', action='store_true', help='保存裁剪结果')
parser.add_argument('--nosave', action='store_true', help='不保存结果')
parser.add_argument('--classes', nargs='+', type=int, help='过滤类别')
parser.add_argument('--line-thickness', type=int, default=18, help='边界框线宽')
opt = parser.parse_args()
return opt
if __name__ == "__main__":
opt = parse_opt()
run(**vars(opt))
最新发布