www.vegetable_Birds.com-- Problem A

本文介绍了一种算法,用于判断跑步比赛中选手的位置和排名是否合理。通过选手位置和排名信息,该算法能够验证排名列表是否可能正确。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

                             Problem A

                                                 Time Limit: 2000/1000 MS (Java/Others)     Memory Limit: 32768/32768 K (Java/Others)
                                                                    Total Submission(s): 1088    Accepted Submission(s): 244
Problem Description
HDU hosts sporting meeting every year. One of the most exciting events is the 10000M-running.During the match many students are running on the track. So, how about the rank list now?

As we know, in a running , we rank the player according to the length everyone has passed . So if one player run 400M(one lap) farther than another player , it looks like they are running at the same position on the track , but the rank of the former is much better than the latter. Now given everyone’s position on the track , and one rank list , can you tell me whether the rank list is possible.
 
Input
The first line of input gives the number of cases, T (at most 110). the first line of each case has two intergers, n,m. (1 <= n <= 100,1 <= m <= 40000,n <= m),represents there’re n players in the m-meter running.
then n lines describe every player.

Each line have two intergers , Xi , Ri .Representing the i-th player is running at xi[0 , 399] meter in his recent lap, and ranks Ri in the ranklist .And the data make sure that no pair of the students have the same Xi or Ri. And the start point is at 0 in their first lap.
 
Output
If the rank list is possible, output “YES” ,output “NO” otherwise.
 
SampleInput
2
3 400
100 1
49 2
28 3
3 800
100 1
150 2
154 3
 
SampleOutput
YES
NO

 

 

 

 

 

 

 

题意:

跑圈 套圈之后合不合法~

第一行为人数和一圈的米数

接下来的几行为以起点为基准相对于起点的位置(米数) 和各个人的名次

题解:先拿名次进行排序(从第一名往后第二名这样依次向后排序) 然后如果前一个名次的人的米数大于第二个的米数 则表示合法 继续向后查找 如果小于前一个人的米数 则后一个人的套圈数加1

最后看最后一个人的套圈数是否大于总可套圈数

代码:

#include <iostream>
#include <algorithm>
using namespace std;
class runner
{
	public:
		int meter;
		int rank;
	
};

bool cmp(runner n1,runner n2)
{
	return n1.rank<n2.rank;
}
int taoquanshu;

int main()
{
	int testcase;
	cin>>testcase;
	
	while(testcase--)
	{
		runner pack[115];
		int maxroute,yu;
		int athlete,distance;
		cin>>athlete>>distance;
		maxroute=(distance/400)-1;
		yu=distance%400;
		
		for(int i=0;i<athlete;i++)
		{
			cin>>pack[i].meter>>pack[i].rank;
			taoquanshu=0;
		}
		
		sort(pack,pack+athlete,cmp);
		for(int j=0;j<athlete-1;j++)
		{
			if(pack[j].meter>pack[j+1].meter)
			{
				continue;
			}
			else
			{
			   taoquanshu++;
			}
		}
		
			if((taoquanshu*400+pack[athlete-1].meter)>distance)
			{
				cout<<"NO"<<endl;
			}
			else
			{
				cout<<"YES"<<endl;
			}

		
		}
			return 0;	
}


 

 

 

 

 

 

 

 

 

#!/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_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.63196, 2.63176, 0.0, 0.0, 0.0, -0.350634, 0.936513), # 点3: 搜索点1 self.create_pose(1.353, 4.08081, 0.0, 0.0, 0.0, 0.35119, 0.936304), # 点4: 搜索点2 self.create_pose(0.445499, 3.68565, 0.0, 0.0, 0.0, 0.900844, 0.434142), # 点5: 搜索点3 # 任务2终点 self.create_pose(1.09864, 3.57304, 0.0, 0.0, 0.0, 0.0148318, 0.99989), # 点6: 任务2的终点 # 任务3相关航点 self.create_pose(2.69836, 3.77956, 0.0, 0.0, 0.0, 0.745125, 0.666924), # 点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.36605, 3.90711, 0.0, 0.0, 0.0, 0.698666, 0.715448), # 点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 = 50 # 目标类别映射 self.test1_categories = { 1: ['apple', 'watermelon', 'banana'], 2: ['potato', 'tomato', 'chili'], 3: ['cake', 'coco', 'milk'] } # 任务3相关变量 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.simulation_timeout_timer = None # 特殊任务超时定时器 #价格计算相关 self.goods_1 = None self.goods_2 = None 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终点,执行特殊任务...") 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: 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: 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 audio_file and os.path.exists(audio_file): threading.Thread(target=self.play_audio_thread, args=(audio_file,)).start() rospy.loginfo("任务3完成,等待新指令...") 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 rospy.loginfo_once('当前已搜索{}次, 目标搜索次数为{}'.format(self.search_acount_current,self.search_acount_target)) if self.search_acount_current >= self.search_acount_target: self.search_acount_state = False self.search_acount_current = 0 self.handle_search_timeout() #self.cancel_current_search() twist = Twist() twist.angular.z = 0.02 self.cmd_vel_pub.publish(twist) 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) rospy.sleep(1) elif self.waypoint_index == self.WAYPOINT_SEARCH2: rospy.loginfo("前往搜索点3") self.search_acount_state = True self.navigate_to_waypoint(self.WAYPOINT_SEARCH3) rospy.sleep(1) else: rospy.logwarn("所有搜索点均未找到目标,前往任务终点") self.search_acount_state = False self.search_acount_current = 0 self.navigate_to_waypoint(self.WAYPOINT_TASK2_END) rospy.sleep(1) ##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.005 * lateral_error if abs(lateral_error) > 50 else 0.0 # 纵向控制(前进/停止) if target_width < 100: twist.linear.x = 0.3 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_SIMULATION # 发布控制命令 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 < 80: twist.linear.x = 0.3 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消息 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("设置特殊任务超时定时器(120秒)") self.simulation_timeout_timer = rospy.Timer(rospy.Duration(120), 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 # 取消超时定时器 if self.simulation_timeout_timer: self.simulation_timeout_timer.shutdown() self.simulation_timeout_timer = None # 根据str2的内容播放相应的音频 self.handle_simulation_result() def handle_simulation_result(self): """处理模拟结果并导航到路灯1(点7)""" if not self.simulation_result_received or not self.simulation_result1: return 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)) # 导航到路灯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 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 在完成任务三后发送一个line话题,话题的内容为'start',之后便等待接受到名为line_result的话题,当接收到改话题的消息为'over'时便执行Final_voice函数,执行完等待3s后便结束整个文件
最新发布
08-04
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值