#!/usr/bin/env python
# -*- coding: utf-8 -*-
"修改:将多个回调函数进行了融合，避免冲突的同时更加好打理: 新增最终播报模块"
import rospy
import actionlib
import os
import threading
import time
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from geometry_msgs.msg import PoseStamped, Twist
from my_control.msg import simulation, simulation_result
from std_msgs.msg import String, Bool
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from actionlib_msgs.msg import GoalStatus
from pyzbar import pyzbar
import cv2
from playsound import playsound
from darknet_ros_msgs.msg import BoundingBoxes, BoundingBox
from xf_mic_asr_offline.srv import Get_Offline_Result_srv,Get_Offline_Result_srvRequest
class AutomationNode:
# 状态定义 - 添加新状态
STATE_IDLE = "IDLE" # 空闲状态
STATE_NAVIGATING = "NAVIGATING" # 导航状态
STATE_ARRIVED = "ARRIVED" # 抵达目标点
STATE_SCANNING = "SCANNING" # 搜索二维码
STATE_PLAYING = "PLAYING" # 播放音频
STATE_SEARCHING = "SEARCHING" # 搜索目标物品
STATE_FOUND_TARGET = "FOUND_TARGET" # 找到目标物品
STATE_WAITING_FOR_TASK3 = "WAITING_FOR_TASK3" # 等待任务3指令
STATE_TRAFFIC_LIGHT_DETECTION = "TRAFFIC_LIGHT_DETECTION" # 交通灯识别状态
STATE_TASK3_NAVIGATING = "TASK3_NAVIGATING" # 任务3导航状态
STATE_WAITING_FOR_SIMULATION_RESULT = "WAITING_FOR_SIMULATION_RESULT" # 新增状态：等待模拟结果
STATE_WAITING_FOR_LINE_RESULT = "WAITING_FOR_LINE_RESULT"#等待寻线任务
STATE_SIMULATION = "GO_TO_DO_SIMULATION"
def __init__(self):
rospy.init_node('automation_node', anonymous=True)
# 状态变量
self.current_state = self.STATE_IDLE
self.target_waypoint = None
self.qr_detected = False
self.waypoint_index = 0
self.task3_started = False # 任务3是否已启动
self.sound_state_1 = 1
self.image = None
self.light = False
# ROS通信设置
self.bridge = CvBridge()
self.speech_sub = rospy.Subscriber('/mic/recognise_result', String, self.speech_callback)
self.arrival_sub = rospy.Subscriber('/arrival_status', Bool, self.arrival_callback)
self.status_pub = rospy.Publisher('/task_status', String, queue_size=10)
self.qr_result_pub = rospy.Publisher('/qrcode_result', String, queue_size=10)
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.yolo_sub = rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, self.yolo_callback)
self.image_sub =rospy.Subscriber('/usb_cam/image_raw', Image, self.image_callback)
self.simulation_start_pub = rospy.Publisher('/simulation_start', simulation, queue_size=10)
self.simulation_result_sub = rospy.Subscriber('/simulation_result', simulation_result, self.simulation_result_callback)
# 二维码参数配置
self.display = rospy.get_param('~display', False)
self.min_size = rospy.get_param('~min_size', 100)
self.process_counter = 0
self.process_every_n = 5
self.qr_simulation = None
# MoveBase动作客户端 - 等待服务器启动
self.move_base_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
rospy.loginfo("等待move_base服务器...")
self.move_base_client.wait_for_server() # 无限期等待直到服务器可用
rospy.loginfo("move_base服务器已连接")
rospy.loginfo("等待xf_asr_offline_node服务器...")
rospy.wait_for_service('/xf_asr_offline_node/get_offline_recognise_result_srv')
rospy.loginfo("xf_asr_offline_node服务器已连接")
self.speech_recognition = rospy.ServiceProxy('/xf_asr_offline_node/get_offline_recognise_result_srv',Get_Offline_Result_srv)
# 预定义航点(根据实际环境配置)
self.waypoints = [
# 任务1相关航点
self.create_pose(1.84581,0.56147, 0.0, 0.0, 0.0, 0.999966, -0.00827442), # 新增过渡点0
self.create_pose(1.00982, 0.630106, 0.0, 0.0, 0.0, 0.999848, 0.017445), # 点1: 扫描点,识别二维码
self.create_pose(0.238816, 1.0809, 0.0, 0.0, 0.0, -0.00275711, 0.999996), # 新增过渡点1
self.create_pose(1.91287, 1.5377, 0.0, 0.0, 0.0, 0.999987,0.00516634), # 新增过渡点2
self.create_pose(0.239635, 2.22449, 0.0, 0.0, 0.0,0.625221, 0.780448), # 点2: 任务1出口
# 任务2搜索航点
self.create_pose(1.55117,2.70212, 0.0, 0.0, 0.0, -0.610833, 0.791759), # 点3: 搜索点1
self.create_pose(1.30051, 4.05638, 0.0, 0.0, 0.0, 0.216052, 0.976382), # 点4: 搜索点2
self.create_pose(0.550306, 3.7162, 0.0, 0.0, 0.0, 0.815828, 0.578295), # 点5: 搜索点3
# 任务2终点
self.create_pose(1.09864, 3.57304, 0.0, 0.0, 0.0, 0.21246, 0.97717), # 点6: 任务2的终点
# 任务3相关航点
self.create_pose(2.63748, 3.98935, 0.0, 0.0, 0.0, 0.707858, 0.706355), # 点7: 路灯1
self.create_pose(2.57195, 3.44993, 0.0, 0.0, 0.0, -0.60105, 0.799212), # 点8: 路口1
self.create_pose(4.46442, 4.08407, 0.0, 0.0, 0.0, 0.643035, 0.765837), # 点9: 路灯2）
self.create_pose(4.53154, 3.52776, 0.0, 0.0, 0.0, -0.771258, 0.636523) # 点10: 路口2
]
# 航点索引常量(提高可读性) - 更新索引
self.WAYPOINT_TRANSITION0 = 0 # 新增的过渡点0
self.WAYPOINT_SCAN = 1 # 点1: 扫描点,识别二维码
self.WAYPOINT_TRANSITION1 = 2 # 新增过渡点1
self.WAYPOINT_TRANSITION2 = 3 # 新增过渡点2
self.WAYPOINT_TASK1_EXIT = 4 # 点2: 任务1出口
self.WAYPOINT_SEARCH1 = 5 # 点3: 搜索点1
self.WAYPOINT_SEARCH2 = 6 # 点4: 搜索点2
self.WAYPOINT_SEARCH3 = 7 # 点5: 搜索点3
self.WAYPOINT_TASK2_END = 8 # 点6: 任务2的终点
self.WAYPOINT_LIGHT1 = 9 # 点7: 路灯1）
self.WAYPOINT_CROSS1 = 10 # 点8: 路口1）
self.WAYPOINT_LIGHT2 = 11 # 点9: 路灯2）
self.WAYPOINT_CROSS2 = 12 # 点10: 路口2）
# 音频文件映射 - 添加新音频
self.audio_files = {
"Fruit": "/home/ucar/ucar_ws/src/mp3/Fruit.mp3",
"Vegetable": "/home/ucar/ucar_ws/src/mp3/Vegetable.mp3",
"Dessert": "/home/ucar/ucar_ws/src/mp3/Dessert.mp3",
"apple": "/home/ucar/ucar_ws/src/mp3/apple.mp3",
"watermelon": "/home/ucar/ucar_ws/src/mp3/watermelon.mp3",
"banana": "/home/ucar/ucar_ws/src/mp3/banana.mp3",
"potato": "/home/ucar/ucar_ws/src/mp3/potato.mp3",
"tomato": "/home/ucar/ucar_ws/src/mp3/tomato.mp3",
"chili": "/home/ucar/ucar_ws/src/mp3/chili.mp3",
"cake": "/home/ucar/ucar_ws/src/mp3/cake.mp3",
"coco": "/home/ucar/ucar_ws/src/mp3/coco.mp3",
"milk": "/home/ucar/ucar_ws/src/mp3/milk.mp3",
"load_1": "/home/ucar/ucar_ws/src/mp3/load_1.mp3",
"load_2": "/home/ucar/ucar_ws/src/mp3/load_2.mp3",
"A": "/home/ucar/ucar_ws/src/mp3/simulation_A.mp3",
"B": "/home/ucar/ucar_ws/src/mp3/simulation_B.mp3",
"C": "/home/ucar/ucar_ws/src/mp3/simulation_C.mp3"
}
# 目标搜索相关变量
self.qr_result = 0
self.target_object = None
self.search_timeout = 5.0 # 每个搜索点的超时时间(秒)
self.search_start_time = None
self.search_timer = None
self.search_timer_state = True
self.search_acount_state = True
self.search_acount_current = 0
self.search_acount_target = 80
self.search_target = False
# 目标类别映射
self.test1_categories = {
1: ['apple', 'watermelon', 'banana'],
2: ['potato', 'tomato', 'chili'],
3: ['cake', 'coco', 'milk']
}
# 任务3相关变量
self.light_timout1 =True
self.light_timout2 = True
self.traffic_light_detected = False
self.traffic_light_result = None # 'red' 或 'green'
#仿真任务相关
self.simulation_result_received = False
self.simulation_result1 = None
self.simulation_result2 = None
self.msg_send = True
self.simulation_timeout_timer = None # 特殊任务超时定时器
#价格计算相关
self.goods_1 = None
self.goods_2 = None
#寻线相关任务
self.line_pub = rospy.Publisher('/line_start', String, queue_size=10)
self.line_result_sub = rospy.Subscriber('/line_result', String, self.line_result_callback)
self.line_sent = True
rospy.loginfo("自动化节点已启动,等待语音指令...")
rospy.on_shutdown(self.shutdown)
def create_pose(self, x, y, z, qx, qy, qz, qw):
"""创建导航目标位姿"""
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation.x = qx
pose.pose.orientation.y = qy
pose.pose.orientation.z = qz
pose.pose.orientation.w = qw
return pose
def navigate_to_waypoint(self, waypoint_index):
"""导航到指定航点"""
if waypoint_index >= len(self.waypoints):
rospy.logwarn("无效的航点索引: %d", waypoint_index)
return
self.waypoint_index = waypoint_index
waypoint = self.waypoints[waypoint_index]
rospy.loginfo("导航到航点 %d", waypoint_index + 1)
# 根据任务设置状态
if self.task3_started:
self.current_state = self.STATE_TASK3_NAVIGATING
else:
self.current_state = self.STATE_NAVIGATING
self.status_pub.publish("NAVIGATING_TO_TARGET")
# 使用move_base动作客户端发送目标
goal = MoveBaseGoal()
goal.target_pose = waypoint
self.move_base_client.send_goal(goal)
# 设置定时器检查导航状态
rospy.Timer(rospy.Duration(0.3), self.check_navigation_status, oneshot=False)
def check_navigation_status(self, event):
"""检查导航状态(使用move_base动作)"""
state = self.move_base_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo_once("成功到达航点 {}".format(self.waypoint_index + 1))
self.handle_arrival()
# 停止定时器
event.new_message = None # 停止定时器
elif state in [GoalStatus.ABORTED, GoalStatus.PREEMPTED]:
rospy.logwarn("导航失败,尝试重新规划...")
self.navigate_to_waypoint(self.waypoint_index)
# 停止当前定时器
event.new_message = None
def handle_arrival(self):
"""处理到达航点后的逻辑"""
self.current_state = self.STATE_ARRIVED
self.status_pub.publish("ARRIVED_AT_TARGET")
# 发布到达状态
arrival_pub = rospy.Publisher('/arrival_status', Bool, queue_size=10)
arrival_pub.publish(True)
def speech_callback(self, msg):
"""处理语音识别结果"""
rospy.loginfo("收到语音指令: {}".format(msg.data))
# 处理初始指令
if "ok" in msg.data.lower() and self.current_state == self.STATE_IDLE:
rospy.loginfo("收到语音指令,开始导航到航点1")
self.navigate_to_waypoint(self.WAYPOINT_TRANSITION0)
elif "fail" in msg.data.lower() and self.current_state == self.STATE_IDLE:
# 创建服务请求
req = Get_Offline_Result_srvRequest()
# 根据实际srv定义设置请求字段
req.offline_recognise_start = 1 #设置1为确认启用
req.confidence_threshold = 40 #置信度
req.time_per_order = 3 #启动持续时间
self.speech_recognition(req)
def arrival_callback(self, msg):
"""处理到达状态"""
if not msg.data or self.current_state != self.STATE_ARRIVED:
return
rospy.loginfo_once("处理航点 {} 的到达逻辑".format(self.waypoint_index+1))
# 新增过渡点0 - 直接导航到下一个点（二维码扫描点）
if self.waypoint_index == self.WAYPOINT_TRANSITION0:
rospy.loginfo("到达过渡点0, 继续导航到二维码扫描点")
self.navigate_to_waypoint(self.WAYPOINT_SCAN)
# 任务1: 二维码扫描点
elif self.waypoint_index == self.WAYPOINT_SCAN:
rospy.loginfo_once("启动二维码识别...")
self.current_state = self.STATE_SCANNING
self.status_pub.publish("SCANNING")
self.image_sub = rospy.Subscriber('/usb_cam/image_raw', Image, self.image_callback)
# 过渡点1
elif self.waypoint_index == self.WAYPOINT_TRANSITION1:
rospy.loginfo("继续导航到过渡点2")
self.navigate_to_waypoint(self.WAYPOINT_TRANSITION2)
# 过渡点2
elif self.waypoint_index == self.WAYPOINT_TRANSITION2:
rospy.loginfo("继续导航到任务1出口")
self.navigate_to_waypoint(self.WAYPOINT_TASK1_EXIT)
# 任务1出口
elif self.waypoint_index == self.WAYPOINT_TASK1_EXIT:
rospy.loginfo_once("到达任务1出口,开始任务2")
#self.navigate_to_waypoint(self.WAYPOINT_SEARCH1)
self.navigate_to_waypoint(self.WAYPOINT_SEARCH1)
# 任务2搜索点
elif self.waypoint_index in [self.WAYPOINT_SEARCH1, self.WAYPOINT_SEARCH2, self.WAYPOINT_SEARCH3]:
rospy.loginfo_once("在航点 {} 开始搜索目标...".format(self.waypoint_index+1))
self.start_searching()
# 任务2终点
elif self.waypoint_index == self.WAYPOINT_TASK2_END:
rospy.loginfo_once("到达任务2终点,执行特殊任务...")
rospy.sleep(3)
self.execute_special_task()
# 任务3: 路灯1
elif self.waypoint_index == self.WAYPOINT_LIGHT1:
rospy.loginfo("到达路灯1,开始识别交通灯...")
self.current_state = self.STATE_TRAFFIC_LIGHT_DETECTION
self.status_pub.publish("DETECTING_TRAFFIC_LIGHT")
self.traffic_light_detected = False
self.traffic_light_result = None
# 设置超时定时器
if self.search_timer and self.light_timout1:
self.light_timout1 = False
self.search_timer.shutdown()
self.search_timer = rospy.Timer(rospy.Duration(self.search_timeout),
self.handle_traffic_light_timeout,
oneshot=True)
# 任务3: 路灯2
elif self.waypoint_index == self.WAYPOINT_LIGHT2:
rospy.loginfo("到达路灯2,开始识别交通灯...")
self.current_state = self.STATE_TRAFFIC_LIGHT_DETECTION
self.status_pub.publish("DETECTING_TRAFFIC_LIGHT")
self.traffic_light_detected = False
self.traffic_light_result = None
# 设置超时定时器
if self.search_timer and self.light_timout2:
self.light_timout2 = False
self.search_timer.shutdown()
self.search_timer = rospy.Timer(rospy.Duration(self.search_timeout),
self.handle_traffic_light_timeout,
oneshot=True)
# 任务3: 路口1或路口2
elif self.waypoint_index in [self.WAYPOINT_CROSS1, self.WAYPOINT_CROSS2]:
rospy.loginfo("到达路口{},准备进行下一步任务...".format(self.waypoint_index-7))
self.status_pub.publish("TASK3_COMPLETED")
self.current_state = self.STATE_IDLE
self.task3_started = False
# 根据到达的路口播放不同的音频
if self.waypoint_index == self.WAYPOINT_CROSS1:
rospy.loginfo("在路口1播放load_1音频")
audio_file = self.audio_files.get("load_1")
else:
rospy.loginfo("在路口2播放load_2音频")
audio_file = self.audio_files.get("load_2")
if self.sound_state_1 == 1:
self.sound_state_1 = 0
if audio_file and os.path.exists(audio_file):
threading.Thread(target=self.play_audio_thread, args=(audio_file,)).start()
# 发送line话题
if self.line_sent:
rospy.loginfo("发送line话题: start")
self.line_pub.publish("start")
self.line_sent = False
self.current_state = self.STATE_WAITING_FOR_LINE_RESULT
def start_searching(self):
"""启动目标搜索"""
if self.current_state == self.STATE_FOUND_TARGET:
return
rospy.loginfo_once("在航点 {} 开始搜索".format(self.waypoint_index+1))
self.current_state = self.STATE_SEARCHING
self.status_pub.publish("SEARCHING_TARGET")
if self.search_acount_state == True:
self.search_acount_current += 1
twist = Twist()
twist.angular.z = 0.2
self.cmd_vel_pub.publish(twist)
if self.search_acount_current % 10 == 0:
rospy.loginfo('当前已搜索{}次, 目标搜索次数为{}'.format(self.search_acount_current,self.search_acount_target))
if self.search_acount_current == self.search_acount_target or self.search_acount_current > self.search_acount_target:
self.handle_search_timeout()
def handle_search_timeout(self):
"""处理搜索超时"""
if self.current_state != self.STATE_SEARCHING:
return
# 检查是否已找到目标
if self.current_state == self.STATE_FOUND_TARGET:
rospy.loginfo("搜索超时但已找到目标，忽略超时")
return
rospy.logwarn("航点 {} 搜索超时".format(self.waypoint_index+1))
self.search_acount_state = False
self.search_acount_current = 0
if self.waypoint_index == self.WAYPOINT_SEARCH1:
rospy.loginfo("前往搜索点2 ")
self.search_acount_state = True
self.navigate_to_waypoint(self.WAYPOINT_SEARCH2)
elif self.waypoint_index == self.WAYPOINT_SEARCH2:
rospy.loginfo("前往搜索点3")
self.search_acount_state = True
self.navigate_to_waypoint(self.WAYPOINT_SEARCH3)
else:
rospy.logwarn("所有搜索点均未找到目标,前往任务终点")
self.search_acount_state = False
self.search_acount_current = 0
self.navigate_to_waypoint(self.WAYPOINT_TASK2_END)
##if next_index <= max_search_index:
# rospy.loginfo("前往下一个搜索点: {}".format(next_index+1))
# self.navigate_to_waypoint(next_index)
#else:
# rospy.logwarn("所有搜索点均未找到目标,前往任务终点")
# self.navigate_to_waypoint(self.WAYPOINT_TASK2_END)
def cancel_current_search(self):
if self.search_timer is not None:
try:
self.search_acount_state = False
rospy.loginfo_once("搜索定时器已取消")
except Exception as e:
rospy.logwarn("取消定时器时出错: {}".format(e))
finally:
self.search_acount_current = 0
def handle_traffic_light_timeout(self, event):
"""处理交通灯识别超时"""
if self.current_state == self.STATE_TRAFFIC_LIGHT_DETECTION:
rospy.logwarn("交通灯识别超时,执行默认操作")
# 如果在路灯1,默认前往路口1
if self.waypoint_index == self.WAYPOINT_LIGHT1:
rospy.loginfo("默认前往路口1(点9)")
self.navigate_to_waypoint(self.WAYPOINT_CROSS1)
# 如果在路灯2,默认前往路口2
elif self.waypoint_index == self.WAYPOINT_LIGHT2:
rospy.loginfo("默认前往路口2(点11)")
self.navigate_to_waypoint(self.WAYPOINT_CROSS2)
def image_callback(self, data):
"""统一的图像处理回调函数"""
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
# 统一翻转图像
cv_image = cv2.flip(cv_image, 1)
self.image = cv_image
except CvBridgeError as e:
rospy.logerr("图像转换错误: {}".format(e))
return
# 状态分发处理
if self.current_state == self.STATE_SCANNING:
self._handle_qr_scanning(cv_image)
self.process_counter +=1
elif self.current_state == self.STATE_FOUND_TARGET:
self._handle_target_tracking(cv_image)
elif self.current_state == self.STATE_TRAFFIC_LIGHT_DETECTION:
# 交通灯检测仍通过YOLO回调处理，这里只需显示图像
if self.display:
cv2.imshow("Traffic Light Detection", cv_image)
cv2.waitKey(1)
def _handle_qr_scanning(self, cv_image):
"""处理二维码扫描"""
if self.process_counter % self.process_every_n != 0:
return # 跳过处理
decoded_objects = pyzbar.decode(cv_image)
results = []
for obj in decoded_objects:
data = obj.data.decode('utf-8')
results.append(data)
if results:
result_msg = ", ".join(results)
rospy.loginfo("识别到二维码: {}".format(result_msg))
self.qr_result_pub.publish(result_msg)
self.qr_detected = True
self.current_state = self.STATE_PLAYING
self.status_pub.publish("PLAYING_AUDIO")
# 取消图像订阅（任务完成后）
#if self.image_sub:
# self.image_sub.unregister()
# self.image_sub = None
self.play_audio(result_msg)
rospy.sleep(3)
self.navigate_to_waypoint(self.WAYPOINT_TRANSITION1)
# 可选：显示图像
if self.display:
for obj in decoded_objects:
points = obj.polygon
if len(points) > 4:
hull = cv2.convexHull(np.array([point for point in points], dtype=np.float32))
hull = list(map(tuple, np.squeeze(hull)))
else:
hull = points
n = len(hull)
for j in range(n):
cv2.line(cv_image, hull[j], hull[(j+1) % n], (0, 255, 0), 3)
cv2.imshow("QR Scanning", cv_image)
cv2.waitKey(1)
def _handle_target_tracking(self, cv_image):
"""处理目标跟踪"""
if not self.target_object:
return
self.search_acount_state = False
self.search_acount_current = 0
# 计算目标位置
target_center_x = (self.xmin + self.xmax) // 2
target_width = self.xmax - self.xmin
frame_center_x = cv_image.shape[1] // 2
# 视觉控制逻辑
twist = Twist()
# 横向控制（转向）
lateral_error = target_center_x - frame_center_x
twist.angular.z = 0.01 * lateral_error if abs(lateral_error) > 30 else 0.0
# 纵向控制（前进/停止）
if target_width < 120:
twist.linear.x = 0.25
else:
# 到达目标面前
twist.linear.x = 0.0
twist.angular.z = 0.0
rospy.loginfo("已到达目标 {} 面前!".format(self.target_object))
self.status_pub.publish("ARRIVED_AT_TARGET")
self.goods_1 = self.target_object
rospy.loginfo_once("货物1为{}".format(self.goods_1))
# 播放目标音频
audio_file = self.audio_files.get(self.target_object)
#self.play_audio(audio_file)
if audio_file and os.path.exists(audio_file):
threading.Thread(target=playsound, args=(audio_file,)).start()
rospy.sleep(2)
# 导航到任务2终点
rospy.loginfo("前往任务2终点...")
self.navigate_to_waypoint(self.WAYPOINT_TASK2_END)
# 重置目标状态
self.target_object = None
self.current_state = self.STATE_WAITING_FOR_SIMULATION_RESULT
# 发布控制命令
self.cmd_vel_pub.publish(twist)
# 可选：显示跟踪框
if self.display:
cv2.rectangle(cv_image, (self.xmin, self.ymin), (self.xmax, self.ymax), (0, 255, 0), 2)
cv2.circle(cv_image, (target_center_x, (self.ymin + self.ymax)//2), 5, (0, 0, 255), -1)
cv2.imshow("Target Tracking", cv_image)
cv2.waitKey(1)
def play_audio(self, qr_code):
"""播放音频文件"""
rospy.loginfo("播放音频: {}".format(qr_code))
# 查找对应的音频文件
audio_file = None
for key in self.audio_files:
if key in qr_code:
audio_file = self.audio_files[key]
break
if audio_file and os.path.exists(audio_file):
# 播放音频（异步）
threading.Thread(target=self.play_audio_thread, args=(audio_file,)).start()
# 设置二维码结果
if "Fruit" in qr_code:
self.qr_result = 1
self.qr_simulation = "fruit"
elif "Vegetable" in qr_code:
self.qr_result = 2
self.qr_simulation = "vegetable"
elif "Dessert" in qr_code:
self.qr_result = 3
self.qr_simulation = "dessert"
else:
rospy.logwarn("未找到匹配的音频文件: {}".format(qr_code) )
def play_audio_thread(self, audio_file):
"""线程中播放音频"""
try:
playsound(audio_file)
rospy.loginfo("音频播放完成")
except Exception as e:
rospy.logerr("音频播放错误: {}".format(e))
def yolo_callback(self, msg):
"""处理YOLO检测结果"""
# 目标搜索状态
if self.current_state == self.STATE_SEARCHING or self.current_state == self.STATE_FOUND_TARGET:
test1_targets = self.test1_categories.get(self.qr_result, [])
for bbox in msg.bounding_boxes:
if bbox.Class in test1_targets:
rospy.loginfo_once("发现目标: {}".format(bbox.Class))
self.target_object = bbox.Class
self.xmin = bbox.xmin
self.ymin = bbox.ymin
self.xmax = bbox.xmax
self.ymax = bbox.ymax
# 取消搜索定时器
#self.cancel_current_search()
self.search_acount_state = False
self.search_acount_current = 0
# 更新状态
self.current_state = self.STATE_FOUND_TARGET
self.status_pub.publish("TARGET_FOUND")
# 订阅图像用于目标跟踪
#if not self.image_sub:
# self.image_sub = rospy.Subscriber('/usb_cam/image_raw', Image, self.yolo_image_callback)
break
# 交通灯识别状态
elif self.current_state == self.STATE_TRAFFIC_LIGHT_DETECTION :
# 检查交通灯类别
for bbox in msg.bounding_boxes:
if bbox.Class in ['red', 'green']:
rospy.loginfo("识别到交通灯: {}".format(bbox.Class) )
self.traffic_light_result = bbox.Class
self.traffic_light_detected = True
if bbox.Class == 'green':
self.xmin = bbox.xmin
self.ymin = bbox.ymin
self.xmax = bbox.xmax
self.ymax = bbox.ymax
target_center_x = (self.xmin + self.xmax) // 2
target_width = self.xmax - self.xmin
frame_center_x = self.image.shape[1] // 2
# 视觉控制逻辑
twist = Twist()
# 横向控制（转向）
lateral_error = target_center_x - frame_center_x
twist.angular.z = 0.005 * lateral_error if abs(lateral_error) > 50 else 0.0
# 纵向控制（前进/停止）
if target_width < 140:
twist.linear.x = 0.5
else:
# 到达目标面前
twist.linear.x = 0.0
twist.angular.z = 0.0
self.light = True
# 取消超时定时器
if self.search_timer:
self.search_timer.shutdown()
self.search_timer = None
# 根据识别结果决定下一步
if self.light == True:
self.handle_traffic_light_result()
elif bbox.Class == 'red' :
self.handle_traffic_light_result()
break
def handle_traffic_light_result(self):
"""根据交通灯识别结果决定导航路径并播放音频"""
if self.waypoint_index == self.WAYPOINT_LIGHT1: # 路灯1
if self.traffic_light_result == 'green':
rospy.loginfo("路灯1为绿灯,播放load_1音频并前往路口1")
# 播放音频
if self.sound_state_1 == 1:
self.sound_state_1 = 0
audio_file = self.audio_files.get("load_1")
if audio_file and os.path.exists(audio_file):
threading.Thread(target=self.play_audio_thread, args=(audio_file,)).start()
else:
rospy.logwarn("load_1音频文件不存在!")
# 导航到路口1
self.navigate_to_waypoint(self.WAYPOINT_CROSS1)
else: # 红灯
rospy.loginfo("路灯1为红灯,前往路灯2")
self.navigate_to_waypoint(self.WAYPOINT_LIGHT2)
elif self.waypoint_index == self.WAYPOINT_LIGHT2: # 路灯2
if self.traffic_light_result == 'green':
rospy.loginfo("路灯2为绿灯,播放load_2音频并前往路口2")
# 播放音频
if self.sound_state_1 == 1:
self.sound_state_1 = 0
audio_file = self.audio_files.get("load_1")
if audio_file and os.path.exists(audio_file):
threading.Thread(target=self.play_audio_thread, args=(audio_file,)).start()
else:
rospy.logwarn("load_1音频文件不存在!")
# 导航到路口2
self.navigate_to_waypoint(self.WAYPOINT_CROSS2)
else: # 红灯
rospy.loginfo("路灯2为红灯,默认前往路口2")
self.navigate_to_waypoint(self.WAYPOINT_CROSS2)
def execute_special_task(self):
"""执行任务二终点的特殊任务"""
rospy.loginfo_once("执行特殊任务: 发布simulation_start话题")
# 创建并发布simulation_start消息
if self.msg_send == True:
self.msg_send = False
sim_msg = simulation()
sim_msg.start = "start"
sim_msg.myclass_send = str(self.qr_simulation) # 二维码结果
self.simulation_start_pub.publish(sim_msg)
# 进入等待结果状态
self.current_state = self.STATE_WAITING_FOR_SIMULATION_RESULT
self.status_pub.publish("WAITING_FOR_SIMULATION_RESULT")
# 重置结果接收标志
self.simulation_result_received = False
# 设置超时定时器(2分50秒 = 170秒)
rospy.loginfo_once("设置特殊任务超时定时器(170秒)")
self.simulation_timeout_timer = rospy.Timer(rospy.Duration(170), self.handle_simulation_timeout, oneshot=True)
rospy.loginfo_once("已发布simulation_start,等待simulation_result...")
def simulation_result_callback(self, msg):
"""处理simulation_result消息"""
if self.current_state != self.STATE_WAITING_FOR_SIMULATION_RESULT:
return
rospy.loginfo("收到simulation_result: stop={}, room={}, myclass={}".format(msg.stop,msg.room,msg.myclass))
# 检查消息格式
if msg.stop == 'stop':
self.simulation_result1 = msg.room
self.simulation_result2 = msg.myclass
self.simulation_result_received = True
self.goods_2 = self.simulation_result2
# 根据str2的内容播放相应的音频
self.handle_simulation_result()
def handle_simulation_result(self):
"""处理模拟结果并导航到路灯1(点7)"""
if not self.simulation_result_received or not self.simulation_result1:
return
# 取消超时定时器
if self.simulation_timeout_timer:
self.simulation_timeout_timer.shutdown()
self.simulation_timeout_timer = None
rospy.loginfo("处理模拟结果: {}".format(self.simulation_result1))
# 检查结果是否有效(A, B, C)
if self.simulation_result1 in ["A", "B", "C"]:
# 播放相应的音频
audio_file = self.audio_files.get(self.simulation_result1)
if audio_file and os.path.exists(audio_file):
rospy.loginfo("播放结果音频: {}".format(self.simulation_result1) )
threading.Thread(target=self.play_audio_thread, args=(audio_file,)).start()
else:
rospy.logwarn("找不到结果音频文件:{}".format(self.simulation_result1))
rospy.sleep(2)
# 导航到路灯1(点7)开始任务3
rospy.loginfo("特殊任务完成,导航到路灯1开始任务3")
self.task3_started = True # 标记任务3已开始
self.navigate_to_waypoint(self.WAYPOINT_LIGHT1) # 直接导航到路灯1
def handle_simulation_timeout(self, event):
"""处理特殊任务超时"""
if self.current_state == self.STATE_WAITING_FOR_SIMULATION_RESULT:
rospy.logwarn("特殊任务超时(170秒),未收到simulation_result,将直接前往路灯1")
# 取消超时定时器
if self.simulation_timeout_timer:
self.simulation_timeout_timer.shutdown()
self.simulation_timeout_timer = None
# 导航到路灯1(点7)
rospy.loginfo("导航到路灯1")
self.task3_started = True # 标记任务3已开始
self.navigate_to_waypoint(self.WAYPOINT_LIGHT1)
def line_result_callback(self, msg):
"""处理line_result消息"""
if self.current_state != self.STATE_WAITING_FOR_LINE_RESULT:
return
if msg.data == 'over':
rospy.loginfo("收到line_result: over, 执行Final_voice")
self.Final_voice()
rospy.loginfo("等待3秒后关闭节点...")
rospy.sleep(3)
rospy.signal_shutdown("任务完成，关闭节点")
def Final_voice(self):
if self.qr_result == 1:
if self.goods_1 == 'apple':
if self.goods_2 == 'apple':
playsound("/home/ucar/ucar_ws/src/mp3/2apple.mp3")
elif self.goods_2 == 'banana':
playsound("/home/ucar/ucar_ws/src/mp3/apple_banana.mp3")
elif self.goods_2 == 'watermelon':
playsound("/home/ucar/ucar_ws/src/mp3/apple_watermelon.mp3")
elif self.goods_1 == 'banana':
if self.goods_2 == 'banana':
playsound("/home/ucar/ucar_ws/src/mp3/2banana.mp3")
elif self.goods_2 == 'apple':
playsound("/home/ucar/ucar_ws/src/mp3/apple_banana.mp3")
elif self.goods_2 == 'watermelon':
playsound("/home/ucar/ucar_ws/src/mp3/banana_watermelon.mp3")
elif self.goods_1 == 'watermelon':
if self.goods_2 == 'watermelon':
playsound("/home/ucar/ucar_ws/src/mp3/2watermelon.mp3")
elif self.goods_2 == 'apple':
playsound("/home/ucar/ucar_ws/src/mp3/apple_watermelon.mp3")
elif self.goods_2 == 'banana':
playsound("/home/ucar/ucar_ws/src/mp3/banana_watermelon.mp3")
if self.qr_result == 2:
if self.goods_1 == 'cake':
if self.goods_2 == 'cake':
playsound("/home/ucar/ucar_ws/src/mp3/2cake.mp3")
elif self.goods_2 == 'coco':
playsound("/home/ucar/ucar_ws/src/mp3/cake_coco.mp3")
elif self.goods_2 == 'milk':
playsound("/home/ucar/ucar_ws/src/mp3/milk_cake.mp3")
elif self.goods_1 == 'coco':
if self.goods_2 == 'coco':
playsound("/home/ucar/ucar_ws/src/mp3/2coco.mp3")
elif self.goods_2 == 'cake':
playsound("/home/ucar/ucar_ws/src/mp3/cake_coco.mp3")
elif self.goods_2 == 'milk':
playsound("/home/ucar/ucar_ws/src/mp3/milk_coco.mp3")
elif self.goods_1 == 'milk':
if self.goods_2 == 'milk':
playsound("/home/ucar/ucar_ws/src/mp3/2milk.mp3")
elif self.goods_2 == 'coco':
playsound("/home/ucar/ucar_ws/src/mp3/milk_coco.mp3")
elif self.goods_2 == 'cake':
playsound("/home/ucar/ucar_ws/src/mp3/milk_cake.mp3")
if self.qr_result == 3:
if self.goods_1 == 'potato':
if self.goods_2 == 'potato':
playsound("/home/ucar/ucar_ws/src/mp3/2potato.mp3")
elif self.goods_2 == 'chili':
playsound("/home/ucar/ucar_ws/src/mp3/chili_potato.mp3")
elif self.goods_2 == 'tomato':
playsound("/home/ucar/ucar_ws/src/mp3/tomato_potato.mp3")
elif self.goods_1 == 'chili':
if self.goods_2 == 'chili':
playsound("/home/ucar/ucar_ws/src/mp3/2chili.mp3")
elif self.goods_2 == 'potato':
playsound("/home/ucar/ucar_ws/src/mp3/chili_potato.mp3")
elif self.goods_2 == 'tomato':
playsound("/home/ucar/ucar_ws/src/mp3/chili_tomato.mp3")
elif self.goods_1 == 'tomato':
if self.goods_2 == 'tomato':
playsound("/home/ucar/ucar_ws/src/mp3/2tomato.mp3")
elif self.goods_2 == 'chili':
playsound("/home/ucar/ucar_ws/src/mp3/chili_tomato.mp3")
elif self.goods_2 == 'potato':
playsound("/home/ucar/ucar_ws/src/mp3/tomato_potato.mp3")
def shutdown(self):
"""节点关闭时的清理工作"""
rospy.loginfo("关闭节点...")
if self.image_sub:
self.image_sub.unregister()
if self.search_timer:
self.search_timer.shutdown()
if self.simulation_timeout_timer:
self.simulation_timeout_timer.shutdown()
self.cmd_vel_pub.publish(Twist()) # 停止机器人运动
if __name__ == '__main__':
try:
node = AutomationNode()
rospy.spin()
except rospy.ROSInterruptException:
pass
我想在识别交通灯的时候也实现目标跟随
扫一扫
1074
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
