202 lines
7.4 KiB
Python
202 lines
7.4 KiB
Python
|
#!/usr/bin/env python3
|
|||
|
|
import rclpy
|
|||
|
|
from rclpy.node import Node
|
|||
|
|
from protocol.msg import HeadTofPayload, SingleTofPayload
|
|||
|
|
from std_msgs.msg import Header
|
|||
|
|
import time
|
|||
|
|
from enum import Enum
|
|||
|
|
import numpy as np
|
|||
|
|
from utils.speech_demo import speak
|
|||
|
|
# TOF传感器位置枚举 (与协议一致)
|
|||
|
|
class TofPosition(Enum):
|
|||
|
|
LEFT_HEAD = 0
|
|||
|
|
RIGHT_HEAD = 1
|
|||
|
|
LEFT_REAR = 2
|
|||
|
|
RIGHT_REAR = 3
|
|||
|
|
HEAD = 4
|
|||
|
|
REAR = 5
|
|||
|
|
|
|||
|
|
# 机器状态枚举
|
|||
|
|
class RobotState(Enum):
|
|||
|
|
PREPARING_LOADING = 1 # 准备配货状态
|
|||
|
|
TRANSPORTING = 2 # 运输中状态
|
|||
|
|
PREPARING_UNLOADING = 3 # 准备卸货状态
|
|||
|
|
|
|||
|
|
class AdvancedTofInteractive(Node):
|
|||
|
|
def __init__(self):
|
|||
|
|
super().__init__('advanced_tof_interactive')
|
|||
|
|
self.get_logger().info("高级TOF交互节点启动")
|
|||
|
|
|
|||
|
|
# 机器狗当前状态 (初始为准备配货状态)
|
|||
|
|
self.current_state = RobotState.PREPARING_LOADING
|
|||
|
|
|
|||
|
|
# TOF检测参数
|
|||
|
|
self.touch_threshold = 0.15 # 触摸检测阈值(米)
|
|||
|
|
self.touch_percentage = 0.2 # 有效触摸点比例
|
|||
|
|
self.cooldown_time = 2.0 # 触摸事件冷却时间(秒)
|
|||
|
|
self.last_detection_time = 0.0
|
|||
|
|
self.min_valid_distance = 0 # 最小有效距离(150mm)
|
|||
|
|
self.max_valid_distance = 1 # 最大有效距离(660mm)
|
|||
|
|
|
|||
|
|
# 触摸模式检测参数
|
|||
|
|
self.touch_history = {"left": False, "right": False}
|
|||
|
|
self.touch_duration_threshold = 1 # 持续触摸时间阈值(秒)
|
|||
|
|
self.touch_start_time = {"left": 0.0, "right": 0.0}
|
|||
|
|
|
|||
|
|
# 创建TOF数据订阅器
|
|||
|
|
self.head_tof_sub = self.create_subscription(
|
|||
|
|
HeadTofPayload,
|
|||
|
|
'/custom_namespace/head_tof_payload',
|
|||
|
|
self.head_tof_callback,
|
|||
|
|
10
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
# 创建状态提示发布器
|
|||
|
|
self.state_pub = self.create_publisher(
|
|||
|
|
SingleTofPayload, # 使用TOF消息作为载体
|
|||
|
|
'/custom_namespace/interactive_state',
|
|||
|
|
10
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
# 状态提示消息
|
|||
|
|
self.state_messages = {
|
|||
|
|
RobotState.PREPARING_LOADING: "准备配货：请同时触摸头部两侧完成配货",
|
|||
|
|
RobotState.PREPARING_UNLOADING: "准备卸货：请同时触摸头部两侧完成卸货",
|
|||
|
|
RobotState.TRANSPORTING: "运输中：请勿触摸"
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
def process_tof_data(self, tof_data):
|
|||
|
|
"""
|
|||
|
|
处理单个TOF传感器的8x8数据矩阵
|
|||
|
|
返回:
|
|||
|
|
valid_matrix: 有效数据点矩阵 (True/False)
|
|||
|
|
mean_distance: 平均距离
|
|||
|
|
touch_detected: 是否检测到触摸
|
|||
|
|
"""
|
|||
|
|
# 转换为numpy数组
|
|||
|
|
data_array = np.array(tof_data.data).reshape(8, 8)
|
|||
|
|
|
|||
|
|
# 创建有效数据掩码
|
|||
|
|
valid_mask = (data_array > self.min_valid_distance) & (data_array < self.max_valid_distance)
|
|||
|
|
|
|||
|
|
# 计算有效点的平均距离
|
|||
|
|
if np.any(valid_mask):
|
|||
|
|
mean_distance = np.mean(data_array[valid_mask])
|
|||
|
|
else:
|
|||
|
|
mean_distance = self.max_valid_distance
|
|||
|
|
|
|||
|
|
# 检测触摸
|
|||
|
|
touch_mask = (data_array < self.touch_threshold) & valid_mask
|
|||
|
|
touch_points = np.sum(touch_mask)
|
|||
|
|
touch_detected = (touch_points / valid_mask.size) >= self.touch_percentage
|
|||
|
|
|
|||
|
|
return valid_mask, mean_distance, touch_detected
|
|||
|
|
|
|||
|
|
def head_tof_callback(self, msg):
|
|||
|
|
"""处理头部TOF数据回调"""
|
|||
|
|
current_time = time.time()
|
|||
|
|
|
|||
|
|
# 检查冷却时间
|
|||
|
|
if current_time - self.last_detection_time < self.cooldown_time:
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
# 处理左侧TOF数据
|
|||
|
|
left_valid, left_mean, left_touch = self.process_tof_data(msg.left_head)
|
|||
|
|
|
|||
|
|
# 处理右侧TOF数据
|
|||
|
|
right_valid, right_mean, right_touch = self.process_tof_data(msg.right_head)
|
|||
|
|
|
|||
|
|
# 更新触摸历史
|
|||
|
|
self.update_touch_history('left', left_touch, current_time)
|
|||
|
|
self.update_touch_history('right', right_touch, current_time)
|
|||
|
|
|
|||
|
|
# 检测同时触摸事件
|
|||
|
|
if self.touch_history['left'] and self.touch_history['right']:
|
|||
|
|
self.get_logger().info(f"检测到同时触摸: 左={left_mean:.3f}m, 右={right_mean:.3f}m")
|
|||
|
|
self.last_detection_time = current_time
|
|||
|
|
self.handle_interaction()
|
|||
|
|
|
|||
|
|
def update_touch_history(self, side, current_touch, current_time):
|
|||
|
|
"""更新触摸历史状态"""
|
|||
|
|
if current_touch:
|
|||
|
|
if not self.touch_history[side]:
|
|||
|
|
# 触摸开始
|
|||
|
|
self.touch_start_time[side] = current_time
|
|||
|
|
self.touch_history[side] = True
|
|||
|
|
else:
|
|||
|
|
# 检测触摸持续时间是否达到阈值
|
|||
|
|
if self.touch_history[side]:
|
|||
|
|
duration = current_time - self.touch_start_time[side]
|
|||
|
|
if duration < self.touch_duration_threshold:
|
|||
|
|
self.get_logger().debug(f"{side}触摸时间不足: {duration:.2f}s")
|
|||
|
|
self.touch_history[side] = False
|
|||
|
|
|
|||
|
|
def handle_interaction(self):
|
|||
|
|
#speak("111")
|
|||
|
|
self.get_logger().info("handleinteraction")
|
|||
|
|
"""处理交互事件"""
|
|||
|
|
# 创建状态消息
|
|||
|
|
state_msg = SingleTofPayload()
|
|||
|
|
state_msg.header.stamp = self.get_clock().now().to_msg()
|
|||
|
|
state_msg.data_available = True
|
|||
|
|
|
|||
|
|
if self.current_state == RobotState.PREPARING_LOADING:
|
|||
|
|
self.get_logger().info("✅ 配货完成！开始运输...")
|
|||
|
|
state_msg.tof_position = TofPosition.HEAD.value
|
|||
|
|
state_msg.data = [1.0] # 表示配货完成
|
|||
|
|
self.current_state = RobotState.TRANSPORTING
|
|||
|
|
|
|||
|
|
elif self.current_state == RobotState.PREPARING_UNLOADING:
|
|||
|
|
self.get_logger().info("✅ 卸货完成！准备新的配货...")
|
|||
|
|
state_msg.tof_position = TofPosition.HEAD.value
|
|||
|
|
state_msg.data = [2.0] # 表示卸货完成
|
|||
|
|
self.current_state = RobotState.PREPARING_LOADING
|
|||
|
|
|
|||
|
|
else:
|
|||
|
|
self.get_logger().info("⚠️ 当前状态不接收触摸指令")
|
|||
|
|
state_msg.data_available = False
|
|||
|
|
|
|||
|
|
# 发布状态消息
|
|||
|
|
self.state_pub.publish(state_msg)
|
|||
|
|
self.play_audio_feedback()
|
|||
|
|
|
|||
|
|
def play_audio_feedback(self):
|
|||
|
|
self.get_logger().info("feedback")
|
|||
|
|
"""播放音频反馈"""
|
|||
|
|
# 实际实现需调用机器狗音频接口
|
|||
|
|
# 示例：self.audio_publisher.publish(audio_msg)
|
|||
|
|
self.get_logger().info("播放操作确认音效")
|
|||
|
|
|
|||
|
|
def publish_state_info(self):
|
|||
|
|
"""定期发布状态信息"""
|
|||
|
|
state_msg = SingleTofPayload()
|
|||
|
|
state_msg.header.stamp = self.get_clock().now().to_msg()
|
|||
|
|
state_msg.tof_position = TofPosition.HEAD.value
|
|||
|
|
state_msg.data_available = True
|
|||
|
|
|
|||
|
|
if self.current_state in self.state_messages:
|
|||
|
|
state_str = self.state_messages[self.current_state]
|
|||
|
|
self.get_logger().info(state_str)
|
|||
|
|
# 将状态消息编码到data字段
|
|||
|
|
state_msg.data = [float(self.current_state.value)]
|
|||
|
|
else:
|
|||
|
|
state_msg.data_available = False
|
|||
|
|
|
|||
|
|
self.state_pub.publish(state_msg)
|
|||
|
|
|
|||
|
|
def main(args=None):
|
|||
|
|
rclpy.init(args=args)
|
|||
|
|
node = AdvancedTofInteractive()
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
# 定期发布状态信息
|
|||
|
|
timer = node.create_timer(3.0, node.publish_state_info)
|
|||
|
|
rclpy.spin(node)
|
|||
|
|
except KeyboardInterrupt:
|
|||
|
|
pass
|
|||
|
|
finally:
|
|||
|
|
node.destroy_node()
|
|||
|
|
rclpy.shutdown()
|
|||
|
|
|
|||
|
|
if __name__ == '__main__':
|
|||
|
|
main()
|