sound_server.c

本文介绍了一个基于UDP协议的音视频实时传输系统,该系统通过创建接收和发送套接字实现双向通信,利用ioctl和open系统调用配置声音设备进行音频数据的采集和回放,展示了如何在Linux环境下实现音视频的实时传输与回放。

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

#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <sys/socket.h>
#include <errno.h>
#include <stdlib.h>
#include <arpa/inet.h>

#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/soundcard.h>

#define LENGTH 1    /* 存储秒数 */
#define RATE 8000   /* 采样频率 */
#define SIZE 8      /* 量化位数 */
#define CHANNELS 1 /* 声道数目 */

// 用于保存数字音频数据的内存缓冲区  8000bytes
//unsigned char sound_buf[1024];
#define    sound_play_buf    sound_buf
unsigned char sound_buf[LENGTH*RATE*SIZE*CHANNELS/400];
//unsigned char sound_play_buf[LENGTH*RATE*SIZE*CHANNELS/16];


int get_sound(void);

int main(int argc, char **argv)
{
    struct sockaddr_in recv_addr,send_addr;
    struct sockaddr_in client_addr;
    int recv_sock,send_sock;
    socklen_t addr_len;
    int len,i;
    int state, sound_fd;
    //char buff[128],recvs[128];
    char *buff;

    switch(argc){
        case    1:
                argv[1]=0;
        case    2:
                argv[2]=0;
                break;
        default:
                break;
    };        
    
    /* 创建 socket , 关键在于这个 SOCK_DGRAM */
    if ((recv_sock = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
        perror("socket");
        exit(errno);
    } else
        printf("create socket.\n\r");
    
    if ((send_sock = socket(AF_INET, SOCK_DGRAM, 0)) == -1) {
        perror("socket_s");
        exit(errno);
    } else
        printf("create s_socket.\n\r");

    memset(&recv_addr, 0, sizeof(struct sockaddr_in));
    /* 设置地址和端口信息 */
    recv_addr.sin_family = AF_INET;
    #if 1
    recv_addr.sin_port = htons(7840);
    recv_addr.sin_addr.s_addr = INADDR_ANY;
    #else
    if (argv[1])
        recv_addr.sin_port = htons(atoi(argv[1]));
    else
        recv_addr.sin_port = htons(7838);
    if (argv[2])
        recv_addr.sin_addr.s_addr = inet_addr(argv[2]);
    else
        recv_addr.sin_addr.s_addr = INADDR_ANY;
    #endif
    /* 绑定地址和端口信息 */
    if ((bind(recv_sock, (struct sockaddr *) &recv_addr, sizeof(recv_addr))) == -1) {
        perror("bind_r");
        exit(errno);
    } else
        printf("bind address to receive socket.\n\r");

    //recv_addr.sin_port =ntohs();
    memset(&send_addr, 0, sizeof(struct sockaddr_in));
    send_addr.sin_port = htons(7850);
    send_addr.sin_addr.s_addr = INADDR_ANY;
    if ((bind(send_sock, (struct sockaddr *) &send_addr, sizeof(send_addr))) == -1) {
        perror("bind_s");
        exit(errno);
    } else
        printf("bind address to sending socket.\n\r");

    /* 循环接收数据 */
//ssize_t  sendto(int  s,  const  void *buf, size_t len, int flags, const struct sockaddr *to, socklen_t tolen);
//ssize_t  recvfrom(int s, void *buf, size_t len, int flags, struct sockaddr *from, socklen_t *fromlen);
    sound_fd=get_sound();
    while (1) {
        //for(i=0;i<25;i++)
        {
            addr_len = sizeof(client_addr);
            len = recvfrom(recv_sock, sound_buf, sizeof(sound_buf) - 1, 0, (struct sockaddr *) &client_addr, &addr_len);
            if (len < 0) {
                perror("recvfrom");
                exit(errno);
            }
            printf("收到来自%s:%d的消息\n",inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port));
            //memcpy(&sound_play_buf[i*160],sound_buf,160);

            buff="The server has played";
            client_addr.sin_port = htons(7860);
            printf("服务器发送到%s:%d的消息\n:%s\n",inet_ntoa(client_addr.sin_addr), ntohs(client_addr.sin_port),buff);
            sendto(send_sock,buff,strlen(buff),0,(struct sockaddr *) &client_addr, addr_len);
        }
        printf("Client said:\n");
        //sound_fd=get_sound();
        state = write(sound_fd, sound_play_buf, sizeof(sound_play_buf)); /* 回放 */
        if (state != sizeof(sound_play_buf))
            perror("wrote wrong number of bytes");
        #if 0
        /* 在继续录音前等待回放结束 */
        state = ioctl(sound_fd, SOUND_PCM_SYNC, 0); 
        if (state == -1)
            perror("SOUND_PCM_SYNC ioctl failed");
        //close(sound_fd);
        #endif
    }

    close(recv_sock);
    close(send_sock);
    return 0;
}

int get_sound(void)
{
    int fd; /* 声音设备的文件描述符 */
    int arg; /* 用于ioctl调用的参数 */
    int status;   /* 系统调用的返回值 */

    /* 打开声音设备 */
    fd = open("/dev/dsp", O_RDWR);
    if (fd < 0) {
        perror("open of /dev/dsp failed");
        exit(1);
    }
 
    /* 设置采样时的量化位数 */
    arg = SIZE;
    status = ioctl(fd, SOUND_PCM_WRITE_BITS, &arg);
    if (status == -1)
        perror("SOUND_PCM_WRITE_BITS ioctl failed");
    if (arg != SIZE)
        perror("unable to set sample size");

    /* 设置采样时的声道数目 */
    arg = CHANNELS; 
    status = ioctl(fd, SOUND_PCM_WRITE_CHANNELS, &arg);
    if (status == -1)
        perror("SOUND_PCM_WRITE_CHANNELS ioctl failed");
    if (arg != CHANNELS)
        perror("unable to set number of channels");

    /* 设置采样时的采样频率 */
    arg = RATE;
    status = ioctl(fd, SOUND_PCM_WRITE_RATE, &arg);
    if (status == -1)
        perror("SOUND_PCM_WRITE_WRITE ioctl failed");
    
    return(fd);
}
 

import time import pickle import numpy as np from mini_bdx_runtime.rustypot_position_hwi import HWI from mini_bdx_runtime.onnx_infer import OnnxInfer from mini_bdx_runtime.raw_imu import Imu from mini_bdx_runtime.poly_reference_motion import PolyReferenceMotion from mini_bdx_runtime.xbox_controller import XBoxController from mini_bdx_runtime.feet_contacts import FeetContacts from mini_bdx_runtime.eyes import Eyes from mini_bdx_runtime.sounds import Sounds from mini_bdx_runtime.antennas import Antennas from mini_bdx_runtime.projector import Projector from mini_bdx_runtime.rl_utils import make_action_dict, LowPassActionFilter from mini_bdx_runtime.duck_config import DuckConfig import os HOME_DIR = os.path.expanduser("~") class RLWalk: def __init__( self, onnx_model_path: str, duck_config_path: str = f"{HOME_DIR}/duck_config.json", serial_port: str = "/dev/ttyACM0", control_freq: float = 50, pid=[30, 0, 0], action_scale=0.25, commands=False, pitch_bias=0, save_obs=False, replay_obs=None, cutoff_frequency=None, ): self.duck_config = DuckConfig(config_json_path=duck_config_path) self.commands = commands self.pitch_bias = pitch_bias self.onnx_model_path = onnx_model_path self.policy = OnnxInfer(self.onnx_model_path, awd=True) self.num_dofs = 14 self.max_motor_velocity = 5.24 # rad/s # Control self.control_freq = control_freq self.pid = pid self.save_obs = save_obs if self.save_obs: self.saved_obs = [] self.replay_obs = replay_obs if self.replay_obs is not None: self.replay_obs = pickle.load(open(self.replay_obs, "rb")) self.action_filter = None if cutoff_frequency is not None: self.action_filter = LowPassActionFilter( self.control_freq, cutoff_frequency ) self.hwi = HWI(self.duck_config, serial_port) self.start() self.imu = Imu( sampling_freq=int(self.control_freq), user_pitch_bias=self.pitch_bias, upside_down=self.duck_config.imu_upside_down, ) self.feet_contacts = FeetContacts() # Scales self.action_scale = action_scale self.last_action = np.zeros(self.num_dofs) self.last_last_action = np.zeros(self.num_dofs) self.last_last_last_action = np.zeros(self.num_dofs) self.init_pos = list(self.hwi.init_pos.values()) self.motor_targets = np.array(self.init_pos.copy()) self.prev_motor_targets = np.array(self.init_pos.copy()) self.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.paused = self.duck_config.start_paused self.command_freq = 20 # hz if self.commands: self.xbox_controller = XBoxController(self.command_freq) # Reference motion, but we only really need the length of one phase # TODO self.PRM = PolyReferenceMotion("./polynomial_coefficients.pkl") self.imitation_i = 0 self.imitation_phase = np.array([0, 0]) self.phase_frequency_factor = 1.0 self.phase_frequency_factor_offset = ( self.duck_config.phase_frequency_factor_offset ) # Optional expression features if self.duck_config.eyes: self.eyes = Eyes() if self.duck_config.projector: self.projector = Projector() if self.duck_config.speaker: self.sounds = Sounds( volume=1.0, sound_directory="../mini_bdx_runtime/assets/" ) if self.duck_config.antennas: self.antennas = Antennas() def get_obs(self): imu_data = self.imu.get_data() dof_pos = self.hwi.get_present_positions( ignore=[ "left_antenna", "right_antenna", ] ) # rad dof_vel = self.hwi.get_present_velocities( ignore=[ "left_antenna", "right_antenna", ] ) # rad/s if dof_pos is None or dof_vel is None: return None if len(dof_pos) != self.num_dofs: print(f"ERROR len(dof_pos) != {self.num_dofs}") return None if len(dof_vel) != self.num_dofs: print(f"ERROR len(dof_vel) != {self.num_dofs}") return None cmds = self.last_commands feet_contacts = self.feet_contacts.get() obs = np.concatenate( [ imu_data["gyro"], imu_data["accelero"], cmds, dof_pos - self.init_pos, dof_vel * 0.05, self.last_action, self.last_last_action, self.last_last_last_action, self.motor_targets, feet_contacts, self.imitation_phase, ] ) return obs def start(self): kps = [self.pid[0]] * 14 kds = [self.pid[2]] * 14 # lower head kps kps[5:9] = [8, 8, 8, 8] self.hwi.set_kps(kps) self.hwi.set_kds(kds) self.hwi.turn_on() time.sleep(2) def get_phase_frequency_factor(self, x_velocity): max_phase_frequency = 1.2 min_phase_frequency = 1.0 # Perform linear interpolation freq = min_phase_frequency + (abs(x_velocity) / 0.15) * ( max_phase_frequency - min_phase_frequency ) return freq def run(self): i = 0 try: print("Starting") start_t = time.time() while True: left_trigger = 0 right_trigger = 0 t = time.time() if self.commands: self.last_commands, self.buttons, left_trigger, right_trigger = ( self.xbox_controller.get_last_command() ) if self.buttons.dpad_up.triggered: self.phase_frequency_factor_offset += 0.05 print( f"Phase frequency factor offset {round(self.phase_frequency_factor_offset, 3)}" ) if self.buttons.dpad_down.triggered: self.phase_frequency_factor_offset -= 0.05 print( f"Phase frequency factor offset {round(self.phase_frequency_factor_offset, 3)}" ) if self.buttons.LB.is_pressed: self.phase_frequency_factor = 1.3 else: self.phase_frequency_factor = 1.0 if self.buttons.X.triggered: if self.duck_config.projector: self.projector.switch() if self.buttons.B.triggered: if self.duck_config.speaker: self.sounds.play_random_sound() if self.duck_config.antennas: self.antennas.set_position_left(right_trigger) self.antennas.set_position_right(left_trigger) if self.buttons.A.triggered: self.paused = not self.paused if self.paused: print("PAUSE") else: print("UNPAUSE") if self.paused: time.sleep(0.1) continue obs = self.get_obs() if obs is None: continue self.imitation_i += 1 * ( self.phase_frequency_factor + self.phase_frequency_factor_offset ) self.imitation_i = self.imitation_i % self.PRM.nb_steps_in_period self.imitation_phase = np.array( [ np.cos( self.imitation_i / self.PRM.nb_steps_in_period * 2 * np.pi ), np.sin( self.imitation_i / self.PRM.nb_steps_in_period * 2 * np.pi ), ] ) if self.save_obs: self.saved_obs.append(obs) if self.replay_obs is not None: if i < len(self.replay_obs): obs = self.replay_obs[i] else: print("BREAKING ") break action = self.policy.infer(obs) self.last_last_last_action = self.last_last_action.copy() self.last_last_action = self.last_action.copy() self.last_action = action.copy() # action = np.zeros(10) self.motor_targets = self.init_pos + action * self.action_scale # self.motor_targets = np.clip( # self.motor_targets, # self.prev_motor_targets # - self.max_motor_velocity * (1 / self.control_freq), # control dt # self.prev_motor_targets # + self.max_motor_velocity * (1 / self.control_freq), # control dt # ) if self.action_filter is not None: self.action_filter.push(self.motor_targets) filtered_motor_targets = self.action_filter.get_filtered_action() if ( time.time() - start_t > 1 ): # give time to the filter to stabilize self.motor_targets = filtered_motor_targets self.prev_motor_targets = self.motor_targets.copy() head_motor_targets = self.last_commands[3:] + self.motor_targets[5:9] self.motor_targets[5:9] = head_motor_targets action_dict = make_action_dict( self.motor_targets, list(self.hwi.joints.keys()) ) self.hwi.set_position_all(action_dict) i += 1 took = time.time() - t # print("Full loop took", took, "fps : ", np.around(1 / took, 2)) if (1 / self.control_freq - took) < 0: print( "Policy control budget exceeded by", np.around(took - 1 / self.control_freq, 3), ) time.sleep(max(0, 1 / self.control_freq - took)) except KeyboardInterrupt: if self.duck_config.antennas: self.antennas.stop() if self.save_obs: pickle.dump(self.saved_obs, open("robot_saved_obs.pkl", "wb")) print("TURNING OFF") if __name__ == "__main__": import argparse parser = argparse.ArgumentParser() parser.add_argument("--onnx_model_path", type=str, required=True) parser.add_argument( "--duck_config_path", type=str, required=False, default=f"{HOME_DIR}/duck_config.json", ) parser.add_argument("-a", "--action_scale", type=float, default=0.25) parser.add_argument("-p", type=int, default=30) parser.add_argument("-i", type=int, default=0) parser.add_argument("-d", type=int, default=0) parser.add_argument("-c", "--control_freq", type=int, default=50) parser.add_argument("--pitch_bias", type=float, default=0, help="deg") parser.add_argument( "--commands", action="store_true", default=True, help="external commands, keyboard or gamepad. Launch control_server.py on host computer", ) parser.add_argument( "--save_obs", type=str, required=False, default=False, help="save the run's observations", ) parser.add_argument( "--replay_obs", type=str, required=False, default=None, help="replay the observations from a previous run (can be from the robot or from mujoco)", ) parser.add_argument("--cutoff_frequency", type=float, default=None) args = parser.parse_args() pid = [args.p, args.i, args.d] print("Done parsing args") rl_walk = RLWalk( args.onnx_model_path, duck_config_path=args.duck_config_path, action_scale=args.action_scale, pid=pid, control_freq=args.control_freq, commands=args.commands, pitch_bias=args.pitch_bias, save_obs=args.save_obs, replay_obs=args.replay_obs, cutoff_frequency=args.cutoff_frequency, ) print("Done instantiating RLWalk") rl_walk.run() 这个程序为什么手柄按A有反应,按B X Y left right LB 都没反应?
07-30
#!/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、付费专栏及课程。

余额充值