#!/usr/bin/env python
#coding=utf -8
import rospy
import actionlib
import subprocess
import os
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from geometry_msgs.msg import PoseStamped, Twist
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
class AutomationNode:
def __init__(self):
rospy.init_node('automation_node', anonymous=True)
# 状态变量
self.current_state = "IDLE" # 状态机: IDLE, NAVIGATING, ARRIVED, SCANNING, PLAYING, SEARCHING
self.target_waypoint = None
self.qr_detected = False
self.qr_start = 1
self.sound_1 = 1
self.sound_2 = 1
self.arrival_1 = 1
self.waypoint_index = 0
# 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.image_sub = None # 动态订阅
self.goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
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.display = rospy.get_param('~display', False) # 是否显示图像
self.min_size = rospy.get_param('~min_size', 100) # 最小二维码尺寸
self.process_counter = 0
self.process_every_n = 5 # 每5帧处理1帧
# MoveBase动作客户端
self.move_base_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
rospy.loginfo("等待move_base服务器...")
self.move_base_client.wait_for_server()
# 预定义航点（根据实际环境配置，这里设置8个点）
self.waypoints = {
"point1": self.create_pose(0.86228, 0.643816, 0.0, 0.0, 0.0, 0.999994, -0.00352223),
"point2": self.create_pose(0.239635, 2.22449, 0.0, 0.0, 0.0, 0.625221, 0.780448),
"point3": self.create_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), # 示例点，需根据实际修改
"point4": self.create_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
"point5": self.create_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
"point6": self.create_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
"point7": self.create_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
"point8": self.create_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
}
# 音频文件映射（根据实际路径配置）
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"
}
self.qr_result = 0
self.xmin = 0
self.ymin = 0
self.xmax = 0
self.ymax = 0
self.class_id = None
self.width = 0
self.height = 0
self.target_object = None
self.yolo_sub = rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, self.yolo_callback)
rospy.loginfo("自动化节点已启动，等待语音指令...")
def create_pose(self, x, y, z, qx, qy, qz, qw):
"""创建导航目标位姿"""
pose = PoseStamped()
pose.header.frame_id = "map"
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 speech_callback(self, msg):
"""处理语音识别结果"""
if "ok" in msg.data.lower() and self.current_state == "IDLE":
rospy.loginfo("收到语音指令,开始导航到航点1")
self.target_waypoint = "point1"
self.navigate_to_waypoint(self.waypoints["point1"])
def navigate_to_waypoint(self, waypoint):
"""发布导航目标"""
self.current_state = "NAVIGATING"
self.status_pub.publish("NAVIGATING_TO_TARGET")
# 使用MoveBase动作
goal = MoveBaseGoal()
goal.target_pose = waypoint
self.move_base_client.send_goal(goal)
# 设置完成超时检查
rospy.Timer(rospy.Duration(1), self.check_navigation_status, oneshot=False)
def check_navigation_status(self, event):
"""检查导航状态"""
if self.move_base_client.get_state() == GoalStatus.SUCCEEDED:
if self.arrival_1 == 1:
self.arrival_1 = 0
rospy.loginfo("成功到达目标点_{}".format(self.waypoint_index + 1))
self.current_state = "ARRIVED"
self.status_pub.publish("ARRIVED_AT_TARGET")
# 发布到达状态（模拟实际应用）
arrival_pub = rospy.Publisher('/arrival_status', Bool, queue_size=10)
arrival_pub.publish(True)
def arrival_callback(self, msg):
"""处理到达状态"""
if msg.data and self.current_state == "ARRIVED":
self.waypoint_index += 1
if self.waypoint_index == 1:
if self.qr_start == 1:
self.qr_start = 0
rospy.loginfo("启动二维码识别...")
self.current_state = "SCANNING"
self.status_pub.publish("SCANNING")
# 订阅摄像头话题（根据实际配置）
self.image_sub = rospy.Subscriber('/usb_cam/image_raw', Image, self.image_callback)
elif self.waypoint_index > 1:
self.current_state = "SEARCHING"
self.status_pub.publish("SEARCHING_TARGET")
def image_callback(self, data):
if self.current_state != "SCANNING":
return
if self.process_counter % self.process_every_n != 0:
return # 跳过处理
try:
# 将ROS图像消息转换为OpenCV图像
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
rospy.logerr("图像转换错误: {}".format(e))
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 = "PLAYING"
self.image_sub.unregister() # 停止图像订阅
self.play_audio(result_msg)
self.navigate_to_waypoint(self.waypoints["point2"])
def play_audio(self, qr_code):
"""播放音频文件"""
rospy.loginfo("播放音频: {}".format(qr_code))
self.status_pub.publish("PLAYING_AUDIO")
# 查找对应的音频文件
audio_file = None
for key in self.audio_files:
if key in qr_code:
audio_file = self.audio_files[key]
break
if self.sound_1 == 1:
if audio_file and os.path.exists(audio_file):
# 播放对应音频
playsound(audio_file)
rospy.loginfo("音频播放完成")
self.sound_1 = 0
self.qr_callback(String(data=qr_code))
def qr_callback(self, msg):
if msg.data == "Fruit":
rospy.loginfo("✅ 匹配成功! 收到: '%s'", msg)
self.qr_result = 1
if self.sound_1 == 1:
self.sound_1 = 0
playsound('/home/ucar/ucar_ws/src/mp3//fruit.mp3')
elif msg.data == "Vegetable":
rospy.loginfo("✅ 匹配成功! 收到: '%s'", msg)
self.qr_result = 2
if self.sound_1 == 1:
self.sound_1 = 0
playsound('/home/ucar/ucar_ws/src/mp3/vegetable.mp3')
elif msg.data == "Dessert":
rospy.loginfo("✅ 匹配成功! 收到: '%s'", msg)
self.qr_result = 3
if self.sound_1 == 1:
self.sound_1 = 0
playsound('/home/ucar/ucar_ws/src/mp3//dessert.mp3')
else:
rospy.loginfo("❌ 匹配失败! 收到: '%s' ", msg)
def yolo_callback(self, msg):
if self.current_state != "SEARCHING":
return
for bbox in msg.bounding_boxes:
self.class_id = bbox.Class
print("识别到的内容为:{}".format(self.class_id))
if self.qr_result == 1:
self.xmin = bbox.xmin
self.ymin = bbox.ymin
self.xmax = bbox.xmax
self.ymax = bbox.ymax
self.width = self.xmax - self.xmin
self.height = self.ymax - self.ymin
if self.class_id in ['apple', 'watermelon', 'banana']:
self.target_object = self.class_id
elif self.qr_result == 2:
self.xmin = bbox.xmin
self.ymin = bbox.ymin
self.xmax = bbox.xmax
self.ymax = bbox.ymax
self.width = self.xmax - self.xmin
self.height = self.ymax - self.ymin
if self.class_id in ['potato', 'tomato', 'chili']:
self.target_object = self.class_id
elif self.qr_result == 3:
self.xmin = bbox.xmin
self.ymin = bbox.ymin
self.xmax = bbox.xmax
self.ymax = bbox.ymax
self.width = self.xmax - self.xmin
self.height = self.ymax - self.ymin
if self.class_id in ['cake', 'coco', 'milk']:
self.target_object = self.class_id
if self.target_object is not None:
self.image_sub = rospy.Subscriber('/usb_cam/image_raw', Image, self.yolo_image_callback)
def yolo_image_callback(self, msg):
if self.target_object is None: # 如果没有检测到指定的目标则推出回调
return
try:
# 将ROS图像消息转换为OpenCV图像
cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError as e:
rospy.logerr("图像转换错误: {}".format(e))
return
if self.class_id == self.target_object:
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
if abs(lateral_error) > 50: # 偏差>50像素则转向
twist.angular.z = 0.0006 * lateral_error # 系数根据小车调整
else:
twist.angular.z = 0.0
# 纵向控制（前进/停止）
if target_width < 200: # 目标宽度<200像素（距离远）
twist.linear.x = 0.3
else: # 距离足够近，停止
twist.linear.x = 0.0
rospy.loginfo("已到达目标{}面前！".format(self.target_object))
#self.image_sub.unregister() # 停止图像订阅
# 播放对应音频
if self.sound_2 == 1:
self.sound_2 = 0
audio_file = self.audio_files.get(self.target_object)
if audio_file and os.path.exists(audio_file):
playsound(audio_file)
rospy.loginfo("音频播放完成")
# 可在此处切换到停止状态或其他逻辑
self.cmd_vel_pub.publish(twist)
if __name__ == '__main__':
try:
node = AutomationNode()
rospy.spin()
except rospy.ROSInterruptException:
pass
在以上代码中添加如果没有找到目标就前往下一个航点寻找的逻辑，并且如果找到目标并停在目标前面就直接前往第八个航点
本文提供了 Tomato 项目的 Git 仓库地址，包括通过 SSH 和 HTTP 协议访问的方式。这对于想要贡献代码或者下载源码进行二次开发的开发者来说是非常重要的信息。
扫一扫
740
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
