# ❌ 错误: 太普通，搜不到
target_position = 1.0

# ✅ 正确: 独特的魔术数字
MAGIC_POSITION = 0xDEADBEEF / 1e7  # 3.7355592

# ✅ 正确: 特殊字符串
TRACER_FRAME = "TRACER_BASE_LINK_001"

# ✅ 正确: 特殊时间戳
TRACER_TIMESTAMP = 123456789.123456

# planning_tracer.py
import rclpy
from rclpy.node import Node
from nav2_simple_commander.robot_navigator import BasicNavigator
from geometry_msgs.msg import PoseStamped

# 示踪剂1: 魔术数字 (易于搜索)
TRACER_X = 0xDEADBEEF / 1e7  # ≈ 3.7355
TRACER_Y = 0xCAFEBABE / 1e7  # ≈ 3.4029

# 示踪剂2: 特殊字符串
TRACER_FRAME = "TRACER_MAP_001"

class PlanningTracer(Node):
    def __init__(self):
        super().__init__('planning_tracer')
        self.navigator = BasicNavigator()

        # 订阅规划路径 (关卡1: 规划器输出)
        self.create_subscription(
            PoseStamped,
            '/goal_pose',
            self.goal_callback,
            10
        )

        # 订阅全局路径 (关卡2: 全局规划器)
        self.create_subscription(
            Path,
            '/plan',
            self.plan_callback,
            10
        )

    def goal_callback(self, msg):
        self.get_logger().info(f'[TRACER] Received goal: '
                                f'x={msg.pose.position.x:.4f}, '
                                f'y={msg.pose.position.y:.4f}')

        # 埋入示踪剂作为目标
        goal = PoseStamped()
        goal.header.frame_id = TRACER_FRAME
        goal.pose.position.x = TRACER_X
        goal.pose.position.y = TRACER_Y

        self.get_logger().info(f'[TRACER] Going to magic position: '
                                f'{TRACER_X:.6f}, {TRACER_Y:.6f}')

        # 开始规划
        self.navigator.goToPose(goal)

    def plan_callback(self, msg):
        # 搜索示踪剂: 检查路径是否包含目标点
        poses = msg.poses
        self.get_logger().info(f'[TRACER] Path has {len(poses)} waypoints')

        # 检查最后一个点
        last = poses[-1]
        self.get_logger().info(f'[TRACER] Final waypoint: '
                                f'x={last.pose.position.x:.6f}, '
                                f'y={last.pose.position.y:.6f}')

        # 验证: 是否接近示踪剂目标？
        dist = ((last.pose.position.x - TRACER_X)**2 +
                (last.pose.position.y - TRACER_Y)**2)**0.5
        self.get_logger().info(f'[TRACER] Distance to target: {dist:.6f}m')

# 终端1: 启动机器人
ros2 launch your_robot_bringup minimal.launch.py

# 终端2: 启动规划器
ros2 launch nav2_bringup navigation_launch.py

# 终端3: 运行示踪程序
python3 planning_tracer.py

# 终端4: 监控话题
ros2 topic echo /plan --field poses
ros2 topic echo /local_plan --field poses

# 搜索示踪剂
ros2 topic echo /plan | grep -i "TRACER"

# 检查关卡
ros2 topic echo /cmd_vel  # 有输出吗？
ros2 topic echo /plan     # 有路径吗？
ros2 node list            # 规划器在运行吗？

# 诊断:
- /plan 无数据 → 规划器问题 (检查代价地图)
- /plan 有数据, /cmd_vel 无数据 → 控制器问题

# 可视化代价地图
ros2 run nav2_map_server map_server -params costmap

# 诊断:
- 路径绕远路 → 代价地图有膨胀层
- 路径穿过障碍 → 代价地图没更新

# control_tracer.py
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

# 示踪剂: 特殊速度值
TRACER_LINEAR_X = 0x12345678 / 1e9  # ≈ 0.305 m/s
TRACER_ANGULAR_Z = 0x87654321 / 1e9  # ≈ -0.527 rad/s

class ControlTracer(Node):
    def __init__(self):
        super().__init__('control_tracer')

        # 订阅控制指令 (关卡: 控制器输出)
        self.create_subscription(
            Twist,
            '/cmd_vel',
            self.cmd_callback,
            10
        )

        # 发布控制指令 (注入示踪剂)
        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)

        # 定时器: 每2秒注入示踪剂
        self.timer = self.create_timer(2.0, self.inject_tracer)

    def cmd_callback(self, msg):
        # 搜索示踪剂: 检查控制指令
        vx = msg.linear.x
        wz = msg.angular.z

        self.get_logger().info(f'[TRACER] cmd_vel: vx={vx:.6f}, wz={wz:.6f}')

        # 检查是否接近示踪剂值
        if abs(vx - TRACER_LINEAR_X) < 0.001:
            self.get_logger().info(f'[TRACER] *** FOUND TRACER VELOCITY! ***')

    def inject_tracer(self):
        # 注入示踪剂到控制流
        msg = Twist()
        msg.linear.x = TRACER_LINEAR_X
        msg.angular.z = TRACER_ANGULAR_Z

        self.get_logger().info(f'[TRACER] Injecting: vx={TRACER_LINEAR_X:.6f}, '
                                f'wz={TRACER_ANGULAR_Z:.6f}')
        self.cmd_pub.publish(msg)

# 运行控制示踪
python3 control_tracer.py

# 监控
ros2 topic echo /cmd_vel

# 绘制控制曲线
rqt_plot /cmd_vel/linear:x /cmd_vel/angular:z

// pwm_tracer.ino
// 示踪剂: 特殊PWM值
const uint16_t TRACER_PWM = 0xCAFE;  // 51914

void setup() {
  Serial.begin(115200);
  pinMode(9, OUTPUT);  // PWM pin

  // 注入示踪剂
  Serial.println("[TRACER] Injecting PWM: 0xCAFE");
  analogWrite(9, TRACER_PWM);
}

void loop() {
  // 读取实际PWM
  uint16_t actual_pwm = analogRead(9);

  Serial.print("[TRACER] Expected: ");
  Serial.print(TRACER_PWM);
  Serial.print(" Actual: ");
  Serial.println(actual_pwm);

  delay(100);
}

# 上传到Arduino
arduino-cli upload -p /dev/ttyUSB0 pwm_tracer.ino

# 串口监控
arduino-cli monitor -p /dev/ttyUSB0 -c baudrate=115200

# 逻辑分析仪
# 接通道到Pin 9，捕获PWM波形

# ekf_tracer.py
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu, JointState
from nav_msgs.msg import Odometry

# 示踪剂: 特殊加速度
TRACER_ACCEL = 9.80665  # 重力加速度

class EKFTracer(Node):
    def __init__(self):
        super().__init__('ekf_tracer')

        # 订阅原始传感器
        self.imu_sub = self.create_subscription(Imu, '/imu', self.imu_callback, 10)
        self.enc_sub = self.create_subscription(JointState, '/encoders', self.enc_callback, 10)

        # 订阅状态估计 (关卡: EKF输出)
        self.odom_sub = self.create_subscription(Odometry, '/odom', self.odom_callback, 10)

    def imu_callback(self, msg):
        accel_z = msg.linear_acceleration.z

        # 搜索示踪剂: 检查重力
        if abs(accel_z - TRACER_ACCEL) < 0.01:
            self.get_logger().info('[TRACER] *** IMU DETECTED GRAVITY! ***')
            self.get_logger().info(f'[TRACER] IMU raw: ax={accel_z:.4f}')

    def odom_callback(self, msg):
        # EKF融合后的估计
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y

        self.get_logger().info(f'[TRACER] EKF estimate: x={x:.4f}, y={y:.4f}')

# 运行EKF示踪
python3 ekf_tracer.py

# 监控
ros2 topic echo /imu --field linear_acceleration
ros2 topic echo /odom --field pose

# 检查TF树
ros2 run tf2_ros tf2_echo odom base_link

# break_robot.py
# 测试边界: 控制延迟

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

class BreakRobot(Node):
    def __init__(self):
        super().__init__('break_robot')
        self.pub = self.create_publisher(Twist, '/cmd_vel', 10)

        # 破坏1: 发送超大速度
        self.timer1 = self.create_timer(1.0, self.break_speed)

        # 破坏2: 高频发送 (压垮控制器)
        self.timer2 = self.create_timer(0.001, self.break_frequency)

    def break_speed(self):
        # 示踪剂: 超速
        msg = Twist()
        msg.linear.x = 999.0  # 远超电机能力
        self.pub.publish(msg)
        self.get_logger().info('[TRACER] Speed test: 999 m/s')

    def break_frequency(self):
        # 示踪剂: 高频
        msg = Twist()
        msg.linear.x = 0.1
        self.pub.publish(msg)
        # 预期: 控制器buffer溢出

# 运行破坏测试
python3 break_robot.py

# 监控系统状态
ros2 topic hz /cmd_vel          # 检查频率
ros2 topic echo /diagnostics    # 检查错误

# 可视化
rviz2

# 查看路径
ros2 topic echo /plan

# 查看代价地图
ros2 service call /get_costmap nav2_msgs/srv/GetCostmap

# 修改目标
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "{pose: {pose: {position: {x: 1.0, y: 0.0}}}}"

# 手动控制
ros2 topic pub /cmd_vel geometry_msgs/Twist "{linear: {x: 0.5}}"

# 查看控制器状态
ros2 topic echo /controller_state

# 调整PID参数
ros2 param set /controller Kp 1.5

# Arduino串口
arduino-cli monitor -p /dev/ttyUSB0

# I2C扫描
i2cdetect -y 1

# GPIO状态
cat /sys/class/gpio/gpio12/value

# IMU原始数据
ros2 topic echo /imu/raw

# TF树
ros2 run tf2_tools view_frames

# 传感器频率
ros2 topic hz /imu
ros2 topic hz /encoders