#!/usr/bin/env python3 # -*- coding: utf-8 -*- import rclpy from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor from rclpy.callback_groups import MutuallyExclusiveCallbackGroup # 导入ROS2消息类型 from protocol.msg import HeadTofPayload, MotionStatus, AudioPlayExtend from protocol.srv import MotionResultCmd from std_srvs.srv import SetBool # 假设导入传感器API模块 # 注意：实际使用时需要根据真实的API模块路径进行导入 from tof_sensor import TofSensor # 假设的TOF传感器API类 class DeliveryInteraction(Node): def __init__(self): super().__init__('delivery_interaction_node') # 配置参数 self.obstacle_distance = 0.3 # 检测障碍物的距离阈值(米) self.obstacle_percentage = 0.8 # 检测到障碍物的比例阈值(80%) self.sensitivity_s = 1.0 # 检测灵敏度(秒) # 状态变量 self.current_area = 0 # 0:无区域 1:配货区 2:卸货区 self.head_left_detected = False self.head_right_detected = False self.motion_id = 0 self.old_motion_id = 0 self.sensor_initialized = False # 初始化TOF传感器 self.tof_sensor = TofSensor() self.init_sensor() # 创建回调组 tof_cb_group = MutuallyExclusiveCallbackGroup() motion_cb_group = MutuallyExclusiveCallbackGroup() service_cb_group = MutuallyExclusiveCallbackGroup() # 订阅TOF数据 self.tof_sub = self.create_subscription( HeadTofPayload, 'head_tof_payload', self.tof_callback, 10, callback_group=tof_cb_group ) # 订阅运动状态 self.motion_sub = self.create_subscription( MotionStatus, 'motion_status', self.motion_callback, 10, callback_group=motion_cb_group ) # 创建区域设置服务 self.set_area_srv = self.create_service( SetBool, 'set_delivery_area', self.set_area_callback, callback_group=service_cb_group ) # 创建运动服务客户端 self.motion_client = self.create_client( MotionResultCmd, 'motion_result_cmd' ) # 创建语音发布者 self.speech_pub = self.create_publisher( AudioPlayExtend, 'speech_play_extend', 10 ) # 创建定时器检查交互条件 self.timer = self.create_timer( 0.5, # 每0.5秒检查一次 self.check_interaction ) self.get_logger().info("配货交互节点已启动，等待区域设置...") def init_sensor(self): """初始化并打开TOF传感器""" try: # 初始化传感器，simulator=False表示使用实际硬件 self.tof_sensor.Init(simulator=False) self.get_logger().info("TOF传感器初始化成功") # 打开传感器 if self.tof_sensor.Open(): self.sensor_initialized = True self.get_logger().info("TOF传感器打开成功") # 启动传感器 if self.tof_sensor.Start(): self.get_logger().info("TOF传感器启动成功") else: self.get_logger().error("TOF传感器启动失败") else: self.get_logger().error("TOF传感器打开失败") except Exception as e: self.get_logger().error(f"TOF传感器初始化过程出错: {str(e)}") def tof_callback(self, msg): """处理TOF传感器数据""" # 只有传感器初始化成功才处理数据 if not self.sensor_initialized: return # 检测左侧传感器 left_obstacle_count = sum(1 for d in msg.left_head.data if d < self.obstacle_distance) left_percentage = left_obstacle_count / len(msg.left_head.data) if msg.left_head.data else 0 self.head_left_detected = left_percentage >= self.obstacle_percentage # 检测右侧传感器 right_obstacle_count = sum(1 for d in msg.right_head.data if d < self.obstacle_distance) right_percentage = right_obstacle_count / len(msg.right_head.data) if msg.right_head.data else 0 self.head_right_detected = right_percentage >= self.obstacle_percentage def motion_callback(self, msg): """更新运动状态""" if self.motion_id != msg.motion_id: self.old_motion_id = self.motion_id self.motion_id = msg.motion_id def set_area_callback(self, request, response): """设置当前区域的服务回调""" # 检查传感器是否已初始化 if not self.sensor_initialized: response.success = False response.message = "TOF传感器未初始化，无法设置区域" self.get_logger().warn(response.message) return response # request.data: True=配货区, False=卸货区 self.current_area = 1 if request.data else 2 area_name = "配货库位" if request.data else "卸货库位" self.get_logger().info(f"已进入{area_name}，等待交互指示...") response.success = True response.message = f"区域设置为{area_name}" return response def check_interaction(self): """检查交互条件并触发相应行为""" # 传感器未初始化则不进行检查 if not self.sensor_initialized: return # 只在配货区或卸货区且坐下状态下检测交互 if self.current_area == 0 or self.motion_id != 214: return # 检测头部两侧同时触摸 if self.head_left_detected and self.head_right_detected: if self.current_area == 1: # 配货区 self.get_logger().info("配货完成指示触发") self.trigger_delivery_complete() self.current_area = 0 # 重置区域 elif self.current_area == 2: # 卸货区 self.get_logger().info("卸货完成指示触发") self.trigger_unloading_complete() self.current_area = 0 # 重置区域 def trigger_delivery_complete(self): """配货完成处理""" # 播放语音提示 self.play_audio(5000) # 假设5000是"配货完成"的语音ID # 执行动作 self.execute_motion(212) # 相对姿势动作 # 等待动作完成 self.get_logger().info("配货已完成，开始运输...") def trigger_unloading_complete(self): """卸货完成处理""" # 播放语音提示 self.play_audio(5001) # 假设5001是"卸货完成"的语音ID # 执行动作 self.execute_motion(150) # 摇尾巴动作 # 等待动作完成 self.get_logger().info("卸货已完成，准备新的配货...") def play_audio(self, play_id): """播放语音""" msg = AudioPlayExtend() msg.module_name = "delivery_interaction" msg.is_online = False msg.speech.play_id = play_id self.speech_pub.publish(msg) def execute_motion(self, motion_id): """执行指定动作""" if not self.motion_client.wait_for_service(timeout_sec=1.0): self.get_logger().warn("运动服务不可用") return request = MotionResultCmd.Request() request.motion_id = motion_id request.duration = 1000 # 持续时间(毫秒) future = self.motion_client.call_async(request) rclpy.spin_until_future_complete(self, future, timeout_sec=2.0) if future.done(): response = future.result() if response.result: self.get_logger().info(f"动作{motion_id}执行成功") else: self.get_logger().error(f"动作{motion_id}执行失败") else: self.get_logger().error("运动服务调用超时") def destroy_node(self): """节点销毁时关闭传感器""" if self.sensor_initialized: self.tof_sensor.Stop() self.tof_sensor.Close() self.get_logger().info("TOF传感器已关闭") super().destroy_node() def main(args=None): rclpy.init(args=args) # 创建节点 node = DeliveryInteraction() # 使用多线程执行器 executor = MultiThreadedExecutor() executor.add_node(node) try: executor.spin() except KeyboardInterrupt: pass finally: node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()