14.File Searching——文件搜索

本文介绍了一个简单的文件搜索工具的设计思路及实现代码。该工具能够基于提供的文件名模式(包括通配符'*')来搜索匹配的文件。文章详细解释了如何处理通配符并给出了完整的C语言实现。

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

这道题属于字符匹配问题,同时也是寻找子链的一种思想,首先看完样例之后的第一个想法是对于"*"号的处理,应该如何下手。对于字符,最好的处理方法是for循环,同时,我们可以采用结构体储存的方法将开头和结尾分别储存起来,然后采用对比的方式,寻找符合的答案。

首先,介绍一种专门对比子链的函数:

函数名: strstr

函数原型:

1

extern char *strstr(char *str1, const char *str2);

语法:

1

strstr(str1,str2)

str1: 被查找目标 

str2: 要查找对象

返回值:若str2是str1的子串,则返回str2在str1的首次出现的地址;如果str2不是str1的子串,则返回NULL。

来看一下题目:

Description

Have you ever used file searching tools provided by an operating system? For example, in DOS, if you type "dir *.exe", the OS will list all executable files with extension "exe" in the current directory. These days, you are so mad with the crappy operating system you are using and you decide to write an OS of your own. Of course, you want to implement file searching functionality in your OS.

Input

The input contains several test cases. Consecutive test cases are separated by a blank line. Each test case begins with an integer N (1 <= N < =100), the number of files in the current directory. Then N lines follow, each line has one string consisting of lowercase letters ('a'..'z') and the dot ('.') only, which is the name of a file. Then there is an integer M (1 <= M <= 20), the number of queries. M lines follow, each has one query string consisting of lowercase letters, the dot and the star ('*') character only. Note that the star character is the "universal matching character" which is used to represent zero or numbers of characters that are uncertain. In the beginning, you just want to write a simple version of file searching, so every string contains no more than 64 characters and there is one and only one star character in the query string. Process to the End Of File (EOF).

Output

For each test case, generate one line for the results of each query. Separate file names in the result by a comma (',') and a blank (' ') character. The file names in the result of one query should be listed according to the order they appear in the input. If there is no matching file, output "FILE NOT FOUND" (without the quotation) instead. Separate two consecutive test cases with a blank line, but Do NOT output an extra blank line after the last one.

Sample Input

4
command.com
msdos.sys
io.sys
config.sys
2
com*.com
*.sys

3
a.txt
b.txt
c.txt
1
*.doc

Sample Output

command.com
msdos.sys, io.sys, config.sys

FILE NOT FOUND

中文:

描述

您是否曾使用过操作系统提供的文件搜索工具?例如,在DOS中,如果键入“dir * .exe”,操作系统将列出当前目录中扩展名为“exe”的所有可执行文件。这些天,你对你正在使用的糟糕的操作系统感到非常生气,你决定编写自己的操作系统。当然,您希望在操作系统中实现文件搜索功能。

输入

输入包含几个测试用例。连续的测试用例由空行分隔。每个测试用例都以整数N(1 <= N <= 100)开头,即当前目录中的文件数。然后是N行,每行有一个字符串,由小写字母('a'..'z')和点('。')组成,这是一个文件的名称。然后有一个整数M(1 <= M <= 20),查询次数。M行跟随,每个都有一个由小写字母组成的查询字符串,仅包含点和星号('*')。请注意,星号是“通用匹配字符”,用于表示不确定的零个或多个字符。一开始,你只想写一个简单的文件搜索版本,所以每个字符串包含不超过64个字符,并且查询字符串中只有一个星形字符。处理到文件结尾(EOF)。

产量

对于每个测试用例,为每个查询的结果生成一行。通过逗号(',')和空白('')字符在结果中分隔文件名。应根据输入中显示的顺序列出一个查询结果中的文件名。如果没有匹配的文件,则输出“FILE NOT FOUND”(不带引号)。用空行分隔两个连续的测试用例,但不要在最后一个之后输出额外的空白行。

来看一下代码:

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
struct B
{
    char first[110];
    char last[110];
} c[110];
int main()
{
    int n,i,k,m,s,j;
    char a[110][110],b[110][110];
    while(scanf("%d",&m)!=EOF)
    {
        for(i=0; i<=m-1; i++)
            scanf("%s",a[i]);
        scanf("%d",&n);
        for(i=0; i<=n-1; i++)
        {
            s=0;
            scanf("%s",b[i]);
            for(j=0; b[i][j]!='*'; j++)
                c[i].first[j]=b[i][j];
            j++;
            for(k=0; b[i][j]!='\0'; k++,j++)
                c[i].last[k]=b[i][j];
            if(strlen(c[i].first)+strlen(c[i].first)<strlen(a[i]))
                for(k=0; k<=m-1; k++)
                {
                    if(strstr(a[k],c[i].first)&&strstr(a[k],c[i].last))
                    {
                        if(s!=0)
                            printf(", ");
                        printf("%s",a[k]);
                        s++;
                    }
                }
            if(s!=0)
                printf("\n");
        }
        if(s==0)
            printf("FILE NOT FOUND\n");
        printf("\n");
    }
    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 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: 状态定义 - 添加新状态 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” # 新增状态:等待模拟结果 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是否已启动 # 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.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.simulation_start_pub = rospy.Publisher(‘/simulation_start’, simulation, queue_size=10) self.simulation_result_sub = rospy.Subscriber(‘/simulation_result’, simulation, 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 # 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服务器已连接”) # 预定义航点(根据实际环境配置) self.waypoints = [ # 任务1相关航点 self.create_pose(0.86228, 0.643816, 0.0, 0.0, 0.0, 0.999994, -0.00352223), # 点1: 扫描点,识别二维码 self.create_pose(0.239314, 1.17115, 0.0, 0.0, 0.0, -0.00275711, 0.999996), # 点1.1: 点1到点2的过渡点 self.create_pose(1.91489, 1.73335, 0.0, 0.0, 0.0, 0.999987, 0.00516634), # 点1.2: 点1到点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.1181, 3.55528, 0.0, 0.0, 0.0, 0.0148318, 0.99989), # 点6: 任务2的终点 # 任务3相关航点 #self.create_pose(2.28661, 4.04943, 0.0, 0.0, 0.0, 0.0, 1), # 点7: 门 self.create_pose(2.69836, 3.77956, 0.0, 0.0, 0.0, 0.745125, 0.666924), # 点8: 路灯1 self.create_pose(2.56047, 3.24088, 0.0, 0.0, 0.0, -0.60105, 0.799212), # 点9: 路口1 self.create_pose(4.36605, 2.0, 0.0, 0.0, 0.0, 0.0, 1.0), # 点10: 路灯2 self.create_pose(3.0, 3.90711, 0.0, 0.0, 0.0, 0.698666, 0.715448) # 点11: 路口2 ] # 航点索引常量(提高可读性) self.WAYPOINT_SCAN = 0 self.WAYPOINT_TASK1_EXIT = 1 self.WAYPOINT_SEARCH1 = 2 self.WAYPOINT_SEARCH2 = 3 self.WAYPOINT_SEARCH3 = 4 self.WAYPOINT_TASK2_END = 5 self.WAYPOINT_DOOR = 6 self.WAYPOINT_LIGHT1 = 7 self.WAYPOINT_CROSS1 = 8 self.WAYPOINT_LIGHT2 = 9 self.WAYPOINT_CROSS2 = 10 # 音频文件映射 - 添加新音频 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/result_A.mp3”, “B”: “/home/ucar/ucar_ws/src/mp3/result_B.mp3”, “C”: “/home/ucar/ucar_ws/src/mp3/result_C.mp3” } # 目标搜索相关变量 self.qr_result = 0 self.target_object = None self.search_timeout = 15.0 # 每个搜索点的超时时间(秒) self.search_start_time = None self.search_timer = None # 目标类别映射 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_result = None self.simulation_timeout_timer = 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(1), 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(“成功到达航点 %d”, 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(f"收到语音指令: {msg.data}“) # 处理初始指令 if “ok” in msg.data.lower() and self.current_state == self.STATE_IDLE: rospy.loginfo(“收到语音指令,开始导航到航点1”) self.navigate_to_waypoint(self.WAYPOINT_SCAN) # 处理任务3启动指令 elif “start task3” in msg.data.lower() and self.current_state == self.STATE_WAITING_FOR_TASK3: rospy.loginfo(“收到任务3启动指令,开始导航到门(点7)”) self.task3_started = True self.navigate_to_waypoint(self.WAYPOINT_DOOR) def arrival_callback(self, msg): “”“处理到达状态””" if not msg.data or self.current_state != self.STATE_ARRIVED: return rospy.loginfo(f"处理航点 {self.waypoint_index+1} 的到达逻辑") # 任务1: 二维码扫描点 if self.waypoint_index == self.WAYPOINT_SCAN: rospy.loginfo(“启动二维码识别…”) 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_TASK1_EXIT: rospy.loginfo(“到达任务1出口,开始任务2”) self.start_searching() # 任务2搜索点 elif self.waypoint_index in [self.WAYPOINT_SEARCH1, self.WAYPOINT_SEARCH2, self.WAYPOINT_SEARCH3]: rospy.loginfo(f"在航点 {self.waypoint_index+1} 开始搜索目标…“) self.start_searching() # 任务2终点 elif self.waypoint_index == self.WAYPOINT_TASK2_END: rospy.loginfo(“到达任务2终点,执行特殊任务…”) self.execute_special_task() # 任务3: 门 elif self.waypoint_index == self.WAYPOINT_DOOR: rospy.loginfo(“到达门,前往路灯1(点8)”) self.navigate_to_waypoint(self.WAYPOINT_LIGHT1) # 任务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) # 订阅图像用于目标检测 if not self.image_sub: self.image_sub = rospy.Subscriber(‘/usb_cam/image_raw’, Image, self.yolo_image_callback) # 任务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(f"到达路口{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): “”“启动目标搜索”“” rospy.loginfo(“开始搜索目标…”) self.current_state = self.STATE_SEARCHING self.status_pub.publish(“SEARCHING_TARGET”) self.search_start_time = rospy.Time.now() # 设置超时定时器 if self.search_timer: self.search_timer.shutdown() self.search_timer = rospy.Timer(rospy.Duration(self.search_timeout), self.handle_search_timeout, oneshot=True) # 订阅图像用于目标检测 if not self.image_sub: self.image_sub = rospy.Subscriber(‘/usb_cam/image_raw’, Image, self.yolo_image_callback) def handle_search_timeout(self, event): “”“处理搜索超时”“” if self.current_state == self.STATE_SEARCHING: rospy.logwarn(“在航点 %d 未找到目标,前往下一个航点”, self.waypoint_index + 1) # 计算下一个航点索引 next_index = self.waypoint_index + 1 # 如果还有搜索航点,前往下一个 if next_index <= self.WAYPOINT_SEARCH3: # 航点索引2-4是搜索点 self.navigate_to_waypoint(next_index) else: # 没有更多搜索点,前往任务2终点 rospy.logwarn(“所有搜索点均未找到目标,前往任务2终点”) self.navigate_to_waypoint(self.WAYPOINT_TASK2_END) 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): “”“处理二维码扫描图像”“” if self.current_state != self.STATE_SCANNING: return try: cv_image = self.bridge.imgmsg_to_cv2(data, “bgr8”) except CvBridgeError as e: rospy.logerr(“图像转换错误: %s”, e) return # 翻转图像,否则难以识别(亲测) cv_image = cv2.flip(cv_image, 1) # 检测二维码 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(“识别到二维码: %s”, 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) # 导航到任务1出口 self.navigate_to_waypoint(self.WAYPOINT_TASK1_EXIT) def play_audio(self, qr_code): “”“播放音频文件””” rospy.loginfo(“播放音频: %s”, 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 elif “Vegetable” in qr_code: self.qr_result = 2 elif “Dessert” in qr_code: self.qr_result = 3 else: rospy.logwarn(“未找到匹配的音频文件: %s”, qr_code) def play_audio_thread(self, audio_file): “”“线程中播放音频”“” try: playsound(audio_file) rospy.loginfo(“音频播放完成”) except Exception as e: rospy.logerr(“音频播放错误: %s”, str(e)) def yolo_callback(self, msg): “”“处理YOLO检测结果”“” # 目标搜索状态 if self.current_state == self.STATE_SEARCHING: # 检查当前类别的有效目标 test1_targets = self.test1_categories.get(self.qr_result, []) for bbox in msg.bounding_boxes: if bbox.Class in test1_targets: rospy.loginfo(“发现目标: %s”, bbox.Class) self.target_object = bbox.Class self.xmin = bbox.xmin self.ymin = bbox.ymin self.xmax = bbox.xmax self.ymax = bbox.ymax # 更新状态 self.current_state = self.STATE_FOUND_TARGET self.status_pub.publish(“TARGET_FOUND”) # 取消超时定时器 if self.search_timer: self.search_timer.shutdown() self.search_timer = None # 如果尚未订阅图像,则订阅 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(“识别到交通灯: %s”, bbox.Class) self.traffic_light_result = bbox.Class self.traffic_light_detected = True # 取消超时定时器 if self.search_timer: self.search_timer.shutdown() self.search_timer = None # 根据识别结果决定下一步 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(点9)”) # 播放音频 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(点10)”) 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(点11)”) # 播放音频 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() else: rospy.logwarn(“load_2音频文件不存在!”) # 导航到路口2 self.navigate_to_waypoint(self.WAYPOINT_CROSS2) else: # 红灯 rospy.loginfo(“路灯2为红灯,默认前往路口2(点11)”) self.navigate_to_waypoint(self.WAYPOINT_CROSS2) def yolo_image_callback(self, msg): “”“处理目标跟踪图像”“” # 目标跟踪状态 if self.target_object and self.current_state == self.STATE_FOUND_TARGET: try: cv_image = self.bridge.imgmsg_to_cv2(msg, “bgr8”) except CvBridgeError as e: rospy.logerr(“图像转换错误: %s”, e) return # 翻转图像,否则难以识别(亲测) cv_image = cv2.flip(cv_image, 1) # 计算目标位置 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: twist.angular.z = 0.0006 * lateral_error else: twist.angular.z = 0.0 # 纵向控制(前进/停止) if target_width < 200: twist.linear.x = 0.3 else: # 到达目标面前 twist.linear.x = 0.0 twist.angular.z = 0.0 rospy.loginfo(“已到达目标 %s 面前!”, self.target_object) self.status_pub.publish(“ARRIVED_AT_TARGET”) # 播放目标音频 audio_file = self.audio_files.get(self.target_object) if audio_file and os.path.exists(audio_file): threading.Thread(target=playsound, args=(audio_file,)).start() # 停止当前运动 self.cmd_vel_pub.publish(twist) # 导航到任务2终点 rospy.loginfo(“前往任务2终点…”) self.navigate_to_waypoint(self.WAYPOINT_TASK2_END) # 重置目标状态 self.target_object = None # 取消图像订阅 if self.image_sub: self.image_sub.unregister() self.image_sub = None # 发布控制命令 self.cmd_vel_pub.publish(twist) def execute_special_task(self): “”“执行任务二终点的特殊任务”“” rospy.loginfo(“执行特殊任务: 发布simulation_start话题”) # 创建并发布simulation_start消息 sim_msg = simulation() sim_msg.str1 = “start” sim_msg.str2 = str(self.qr_result) # 使用二维码结果作为str2 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 self.simulation_result = None # 设置超时定时器(2分50秒 = 170秒) rospy.loginfo(“设置特殊任务超时定时器(170秒)”) if self.simulation_timeout_timer: self.simulation_timeout_timer.shutdown() self.simulation_timeout_timer = rospy.Timer(rospy.Duration(170), self.handle_simulation_timeout, oneshot=True) rospy.loginfo(“已发布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(f"收到simulation_result: str1={msg.str1}, str2={msg.str2}“) # 检查消息格式 if msg.str1 == “stop”: self.simulation_result = msg.str2 self.simulation_result_received = True # 取消超时定时器 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): “”“处理模拟结果并导航到点7(门)””" if not self.simulation_result_received or not self.simulation_result: return rospy.loginfo(“处理模拟结果: %s”, self.simulation_result) # 检查结果是否有效(A, B, C) if self.simulation_result in [“A”, “B”, “C”]: # 播放相应的音频 audio_file = self.audio_files.get(self.simulation_result) if audio_file and os.path.exists(audio_file): rospy.loginfo(“播放结果音频: %s”, self.simulation_result) threading.Thread(target=self.play_audio_thread, args=(audio_file,)).start() else: rospy.logwarn(“找不到结果音频文件: %s”, self.simulation_result) # 导航到点7(门) rospy.loginfo(“特殊任务完成,导航到点7(门)”) self.task3_started = True # 标记任务3已开始 self.navigate_to_waypoint(self.WAYPOINT_DOOR) else: rospy.logwarn(“无效的模拟结果: %s”, self.simulation_result) # 默认导航到点7(门) self.task3_started = True self.navigate_to_waypoint(self.WAYPOINT_DOOR) def handle_simulation_timeout(self, event): “”“处理特殊任务超时”“” if self.current_state == self.STATE_WAITING_FOR_SIMULATION_RESULT: rospy.logwarn(“特殊任务超时(170秒),未收到simulation_result,将直接前往点7(门)”) # 取消超时定时器 if self.simulation_timeout_timer: self.simulation_timeout_timer.shutdown() self.simulation_timeout_timer = None # 导航到点7(门) rospy.loginfo(“导航到点7(门)”) self.task3_started = True # 标记任务3已开始 self.navigate_to_waypoint(self.WAYPOINT_DOOR) 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 在点1和点2之间加上两个过渡点,同时去除原本的点7门,在到达任务二的终点接收到特殊任务的结束标志后就直接前往点8路灯1处执行任务3
最新发布
07-30
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值