#!/usr/bin/env python # -*- coding: utf-8 -*- import rospy import math from geometry_msgs.msg import PoseStamped from nav_msgs.msg import Odometry class ExperimentMonitor: def __init__(self): rospy.init_node('experiment_monitor_node') # --- 参数配置 --- self.goal_tolerance = 0.3 # 判定到达的距离阈值 (米) self.odom_topic = "/Odometry" # 如果你的里程计话题不同请修改 self.goal_topic = "/move_base_simple/goal" # --- 状态变量 --- self.is_running = False self.start_time = None self.total_distance = 0.0 self.last_pose = None self.goal_pose = None # --- 订阅话题 --- rospy.Subscriber(self.goal_topic, PoseStamped, self.goal_callback) rospy.Subscriber(self.odom_topic, Odometry, self.odom_callback) rospy.loginfo("==== 实验监控裁判已就位 ====") rospy.loginfo("等待在 RViz 中点击目标点或运行发布脚本...") def goal_callback(self, msg): """监听到新目标，重置数据并开始计时""" self.goal_pose = msg.pose.position self.is_running = True self.start_time = rospy.get_time() self.total_distance = 0.0 self.last_pose = None rospy.loginfo("🏁 检测到新任务！目标点: (%.2f, %.2f) 开始计时...", self.goal_pose.x, self.goal_pose.y) def odom_callback(self, msg): if not self.is_running or self.goal_pose is None: return curr_pose = msg.pose.pose.position # 1. 累加实际行驶距离 (里程计路径长度) if self.last_pose is not None: dist = math.sqrt((curr_pose.x - self.last_pose.x)**2 + (curr_pose.y - self.last_pose.y)**2) self.total_distance += dist self.last_pose = curr_pose # 2. 计算当前距离目标的剩余距离 dist_to_goal = math.sqrt((curr_pose.x - self.goal_pose.x)**2 + (curr_pose.y - self.goal_pose.y)**2) # 3. 判定“冲线” if dist_to_goal < self.goal_tolerance: self.finish_experiment() def finish_experiment(self): self.is_running = False duration = rospy.get_time() - self.start_time # 防止除以零 avg_speed = self.total_distance / duration if duration > 0 else 0.0 rospy.loginfo("🎯 任务完成！(已进入容差范围)") print("\n" + "="*30) print("🚩 实验结果汇总:") print("⏱️ 消耗时间: {:.2f} s".format(duration)) print("📏 行驶里程: {:.2f} m".format(self.total_distance)) print("🚀 平均速度: {:.4f} m/s".format(avg_speed)) print("="*30 + "\n") # 提示等待下一次实验 rospy.loginfo("等待下一个目标点...") if __name__ == '__main__': try: monitor = ExperimentMonitor() rospy.spin() except rospy.ROSInterruptException: pass
