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