mi-task/tmp/interdemo3.py

#!/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()