215 lines
8.0 KiB
Python
215 lines
8.0 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
|
||
import rclpy
|
||
from rclpy.node import Node
|
||
from protocol.msg import HeadTofPayload, RearTofPayload, SingleTofPayload
|
||
from std_msgs.msg import String
|
||
from std_srvs.srv import Trigger
|
||
import time
|
||
import numpy as np
|
||
import csv
|
||
import os
|
||
from datetime import datetime
|
||
|
||
class TofInteractive(Node):
|
||
def __init__(self):
|
||
super().__init__('tof_interactive_node')
|
||
self.get_logger().info("TOF交互节点启动...")
|
||
|
||
# 当前状态：'loading' 配货中, 'unloading' 卸货中, 'transporting' 运输中
|
||
self.current_state = 'loading' # 初始状态设为配货状态
|
||
|
||
# 触摸检测参数
|
||
self.obstacle_distance = 0.3 # 检测障碍物的距离阈值 (米)
|
||
self.obstacle_percentage = 0.8 # 检测到障碍物的点比例阈值
|
||
self.sensitivity_s = 1.0 # 检测灵敏度 (秒)
|
||
self.last_detection_time = time.time() - self.sensitivity_s
|
||
|
||
# 存储TOF数据
|
||
self.head_tof_data = None
|
||
self.rear_tof_data = None
|
||
|
||
# 订阅头部和尾部TOF数据
|
||
self.head_sub = self.create_subscription(
|
||
HeadTofPayload,
|
||
'head_tof_payload',
|
||
self.head_callback,
|
||
10)
|
||
|
||
self.rear_sub = self.create_subscription(
|
||
RearTofPayload,
|
||
'rear_tof_payload',
|
||
self.rear_callback,
|
||
10)
|
||
|
||
# 发布状态消息
|
||
self.status_pub = self.create_publisher(String, 'interactive_status', 10)
|
||
|
||
# 创建完成配货/卸货的服务
|
||
self.complete_loading_srv = self.create_service(
|
||
Trigger,
|
||
'complete_loading',
|
||
self.complete_loading_callback)
|
||
|
||
self.complete_unloading_srv = self.create_service(
|
||
Trigger,
|
||
'complete_unloading',
|
||
self.complete_unloading_callback)
|
||
|
||
# 定时器用于状态检测
|
||
self.timer = self.create_timer(0.1, self.detection_loop)
|
||
|
||
self.get_logger().info(f"初始状态: {self.current_state}")
|
||
self.publish_status()
|
||
|
||
def head_callback(self, msg):
|
||
"""头部TOF数据回调"""
|
||
self.get_logger().info("head")
|
||
self.head_tof_data = msg
|
||
# self.get_logger().info(f"头部TOF数据接收: L={len(msg.left_head.data)}, R={len(msg.right_head.data)}")
|
||
|
||
def rear_callback(self, msg):
|
||
"""尾部TOF数据回调"""
|
||
self.get_logger().info("rear")
|
||
self.rear_tof_data = msg
|
||
# self.get_logger().info(f"尾部TOF数据接收: L={len(msg.left_rear.data)}, R={len(msg.right_rear.data)}")
|
||
|
||
def detect_obstacle(self, single_tof):
|
||
"""检测单个TOF传感器是否有障碍物"""
|
||
self.get_logger().info("obstacle")
|
||
if not single_tof.data_available or len(single_tof.data) == 0:
|
||
return False
|
||
|
||
obstacle_point = 0
|
||
for ranging in single_tof.data:
|
||
if 0.15 <= ranging <= 0.66 and ranging < self.obstacle_distance:
|
||
obstacle_point += 1
|
||
|
||
obstacle_percentage = obstacle_point / len(single_tof.data)
|
||
return obstacle_percentage >= self.obstacle_percentage
|
||
|
||
def detection_loop(self):
|
||
"""检测循环 - 检查触摸事件"""
|
||
self.get_logger().info("loop！")
|
||
current_time = time.time()
|
||
if current_time - self.last_detection_time < self.sensitivity_s:
|
||
return
|
||
self.get_logger().info("lllll！")
|
||
if self.head_tof_data is None:
|
||
return
|
||
self.get_logger().info("ppppp！")
|
||
# 检测左右头部TOF
|
||
left_detected = self.detect_obstacle(self.head_tof_data.left_head)
|
||
right_detected = self.detect_obstacle(self.head_tof_data.right_head)
|
||
|
||
# 检测到触摸事件
|
||
if left_detected or right_detected:
|
||
self.last_detection_time = current_time
|
||
self.get_logger().info("检测到触摸下巴事件！")
|
||
|
||
# 记录TOF数据
|
||
self.record_tof_data()
|
||
|
||
# 根据当前状态处理
|
||
if self.current_state == 'loading':
|
||
self.get_logger().info("配货完成！")
|
||
self.current_state = 'transporting'
|
||
self.play_sound("配货完成，开始运输")
|
||
|
||
elif self.current_state == 'unloading':
|
||
self.get_logger().info("卸货完成！")
|
||
self.current_state = 'loading' # 回到配货状态
|
||
self.play_sound("卸货完成，准备下一次配货")
|
||
|
||
self.publish_status()
|
||
|
||
def record_tof_data(self):
|
||
"""记录当前的TOF数据"""
|
||
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
|
||
filename = f"tof_data_{timestamp}.csv"
|
||
|
||
with open(filename, 'w', newline='') as file:
|
||
writer = csv.writer(file)
|
||
writer.writerow(['sensor_position', 'index', 'distance', 'intensity'])
|
||
|
||
# 记录头部TOF数据
|
||
if self.head_tof_data:
|
||
self.write_single_tof(writer, 'left_head', self.head_tof_data.left_head)
|
||
self.write_single_tof(writer, 'right_head', self.head_tof_data.right_head)
|
||
|
||
# 记录尾部TOF数据
|
||
if self.rear_tof_data:
|
||
self.write_single_tof(writer, 'left_rear', self.rear_tof_data.left_rear)
|
||
self.write_single_tof(writer, 'right_rear', self.rear_tof_data.right_rear)
|
||
|
||
self.get_logger().info(f"TOF数据已保存到文件: {filename}")
|
||
|
||
def write_single_tof(self, writer, position_name, single_tof):
|
||
"""将单个TOF传感器的数据写入CSV"""
|
||
if not single_tof.data_available:
|
||
return
|
||
|
||
# 如果intensity数据不可用，创建一个相同长度的零列表
|
||
if len(single_tof.intensity) != len(single_tof.data):
|
||
intensity_list = [0.0] * len(single_tof.data)
|
||
else:
|
||
intensity_list = single_tof.intensity
|
||
|
||
for i, (distance, intensity) in enumerate(zip(single_tof.data, intensity_list)):
|
||
writer.writerow([position_name, i, distance, intensity])
|
||
|
||
def play_sound(self, text):
|
||
"""播放提示音 (模拟)"""
|
||
self.get_logger().info(f"播放声音: {text}")
|
||
# 实际实现中应调用机器狗的音频播放接口
|
||
# 例如: self.audio_pub.publish(text)
|
||
|
||
def publish_status(self):
|
||
"""发布当前状态"""
|
||
msg = String()
|
||
msg.data = self.current_state
|
||
self.status_pub.publish(msg)
|
||
self.get_logger().info(f"状态更新: {self.current_state}")
|
||
|
||
def complete_loading_callback(self, request, response):
|
||
"""完成配货服务回调"""
|
||
if self.current_state == 'loading':
|
||
self.get_logger().info("通过服务调用完成配货")
|
||
self.current_state = 'transporting'
|
||
self.publish_status()
|
||
response.success = True
|
||
response.message = "配货完成，开始运输"
|
||
else:
|
||
response.success = False
|
||
response.message = f"当前状态 {self.current_state} 不允许配货"
|
||
return response
|
||
|
||
def complete_unloading_callback(self, request, response):
|
||
"""完成卸货服务回调"""
|
||
if self.current_state == 'unloading':
|
||
self.get_logger().info("通过服务调用完成卸货")
|
||
self.current_state = 'loading'
|
||
self.publish_status()
|
||
response.success = True
|
||
response.message = "卸货完成，准备下一次配货"
|
||
else:
|
||
response.success = False
|
||
response.message = f"当前状态 {self.current_state} 不允许卸货"
|
||
return response
|
||
|
||
|
||
def main(args=None):
|
||
rclpy.init(args=args)
|
||
node = TofInteractive()
|
||
try:
|
||
rclpy.spin(node)
|
||
except KeyboardInterrupt:
|
||
pass
|
||
finally:
|
||
node.destroy_node()
|
||
rclpy.shutdown()
|
||
|
||
|
||
if __name__ == '__main__':
|
||
main() |