feat(craic_localization): add localization, teach-in, and navigation nodes

Four ROS2 nodes:
- chassis_odometry: reads chassis MCU serial (32B packet), publishes /odom + TF odom->base_footprint
- teach_points: records map-frame waypoints from AMCL TF, saves to YAML with interactive CLI
- show_points: publishes visualization_msgs/MarkerArray to /taught_points for RViz display of waypoints with ARROW+TEXT markers
- navigate_to_point: holonomic P-controller that reads YAML waypoints and drives Mecanum chassis via UDP XYW commands, with /goto topic interface

Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-openagent)

Co-authored-by: Sisyphus <clio-agent@sisyphuslabs.ai>

ros2/src/craic_localization/craic_localization/chassis_odometry.py

@@ -0,0 +1,272 @@
+#!/usr/bin/env python3
+"""底盘轮式里程计节点 (chassis_odometry)
+
+直连底盘 MCU (lzucar 板) 串口 `/dev/ttylzucar`，**只读**解析 32 字节里程计数据包，
+发布 nav_msgs/Odometry (/odom) 与 TF (odom -> base_footprint)。
+
+数据包协议（移植自 lzu_robot/src/cgbot/cgbot/seriallzucar.py，已上真机验证的同款底盘）：
+  - 帧尾标志 3 字节: 'L', 'Z', 'U'
+  - 整包 32 字节，以 'LZU' 结尾对齐
+  - packet[0]      = 校验和 = sum(packet[1:30]) % 256
+  - packet[1:29]   = <fffffff> = x, y, yaw, posFR, posFL, posBL, posBR
+                     (x/y 单位 mm，需 /1000 转米; yaw 单位弧度)
+  - packet[29]     = 模式字节, 'D' 表示航位推算有效
+  - packet[29:32]  = 帧尾 'L','Z','U'（对齐用）
+
+本节点不发送任何命令；既有底盘速度控制 (UDP->ESP32->UART) 不受影响，互不抢占串口。
+若后续需把底盘命令也迁到直连串口，应在此节点内合并发送逻辑（参考 seriallzucar 的 pack_floats_send）。
+"""
+
+import math
+import struct
+import time
+from threading import Lock, Thread
+
+import rclpy
+import serial
+from geometry_msgs.msg import Quaternion, TransformStamped
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from tf2_ros import TransformBroadcaster
+
+PACKET_SIZE = 32
+MODE_DEAD_RECKON = ord('D')
+
+
+def yaw_to_quaternion(yaw: float) -> Quaternion:
+    """绕 Z 轴偏航角 -> 四元数。"""
+    q = Quaternion()
+    q.x = 0.0
+    q.y = 0.0
+    q.z = math.sin(yaw / 2.0)
+    q.w = math.cos(yaw / 2.0)
+    return q
+
+
+class ChassisOdometry(Node):
+    def __init__(self) -> None:
+        super().__init__('chassis_odometry')
+
+        # ---- 参数 ----
+        self.port = self.declare_parameter('port', '/dev/ttylzucar').value
+        self.baudrate = int(self.declare_parameter('baudrate', 115200).value)
+        self.odom_frame_id = self.declare_parameter('odom_frame_id', 'odom').value
+        self.base_frame_id = self.declare_parameter('base_frame_id', 'base_footprint').value
+        self.odom_topic = self.declare_parameter('odom_topic', 'odom').value
+        self.publish_tf = bool(self.declare_parameter('publish_tf', True).value)
+        # 仅当模式字节为 'D'(航位推算有效) 时才发布；置 False 可在调试期放行所有包
+        self.require_mode_d = bool(self.declare_parameter('require_mode_d', True).value)
+        # 串口打开后下发的使能命令，让底盘板持续上报里程计；空字符串=不发。
+        # lzu_robot/seriallzucar.open() 与 move_try/serial_bridge 都在打开后发 "UP0LZU"，
+        # 缺它则底盘上电后只发几帧就停，odom TF 变陈旧、gmapping 丢弃所有 scan。
+        self.enable_cmd = str(self.declare_parameter('enable_cmd', 'UP0LZU').value)
+        # >0 时按该周期(秒)重发使能命令做保活；0=仅打开时发一次(与参考实现一致)
+        self.enable_cmd_period = float(self.declare_parameter('enable_cmd_period', 0.0).value)
+
+        # ---- 发布器 ----
+        self.odom_pub = self.create_publisher(Odometry, self.odom_topic, 10)
+        self.tf_broadcaster = TransformBroadcaster(self) if self.publish_tf else None
+
+        # ---- 串口接收环形缓冲 ----
+        self.BUFFER_SIZE = 1024
+        self.buffer = bytearray(self.BUFFER_SIZE)
+        self.write_index = 0
+        self.buffer_lock = Lock()
+
+        # ---- 上一帧状态（用于差分求速度）----
+        self.prev_x = None
+        self.prev_y = None
+        self.prev_yaw = None
+        self.prev_t = None
+
+        # ---- 串口 + 接收线程 ----
+        self.ser = None
+        self._running = True
+        self._reader = Thread(target=self._receiver_thread, daemon=True)
+        self._reader.start()
+
+        # 可选：周期性重发使能命令做保活
+        if self.enable_cmd and self.enable_cmd_period > 0.0:
+            self.create_timer(self.enable_cmd_period, self._send_enable_cmd)
+
+        self.get_logger().info(
+            f'chassis_odometry 启动: port={self.port}@{self.baudrate} '
+            f'odom_frame={self.odom_frame_id} base_frame={self.base_frame_id} '
+            f'publish_tf={self.publish_tf}'
+        )
+
+    # ------------------------------------------------------------------ #
+    # 串口接收 / 重连
+    # ------------------------------------------------------------------ #
+    def _open_serial(self) -> bool:
+        try:
+            self.ser = serial.Serial(self.port, self.baudrate, timeout=1)
+            self.get_logger().info(f'串口已连接 {self.port}')
+            self._send_enable_cmd()
+            return True
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warning(f'串口打开失败 {self.port}: {exc}')
+            self.ser = None
+            return False
+
+    def _send_enable_cmd(self) -> None:
+        """下发里程计使能命令(默认 UP0LZU)；空命令或串口未开则跳过。"""
+        if not self.enable_cmd or self.ser is None or not self.ser.is_open:
+            return
+        try:
+            self.ser.write(self.enable_cmd.encode('utf-8'))
+            self.ser.flush()
+            self.get_logger().info(f'已下发里程计使能命令: {self.enable_cmd}')
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warning(f'使能命令发送失败: {exc}')
+
+    def _receiver_thread(self) -> None:
+        while self._running:
+            try:
+                if self.ser is None or not self.ser.is_open:
+                    if not self._open_serial():
+                        time.sleep(1.0)
+                    continue
+                waiting = self.ser.in_waiting
+                if waiting > 0:
+                    self._write_to_buffer(self.ser.read(waiting))
+                else:
+                    time.sleep(0.005)
+            except serial.SerialException as exc:
+                self.get_logger().warning(f'串口中断，准备重连: {exc}')
+                self._close_serial()
+                time.sleep(1.0)
+            except Exception as exc:  # noqa: BLE001
+                self.get_logger().error(f'接收线程异常: {exc}')
+                self._close_serial()
+                time.sleep(1.0)
+
+    def _close_serial(self) -> None:
+        if self.ser is not None:
+            try:
+                self.ser.close()
+            except Exception:  # noqa: BLE001
+                pass
+        self.ser = None
+
+    # ------------------------------------------------------------------ #
+    # 帧解析（逐字节移植自 seriallzucar，保持已验证行为）
+    # ------------------------------------------------------------------ #
+    def _read_byte(self, index: int) -> int:
+        return self.buffer[(index + self.BUFFER_SIZE) % self.BUFFER_SIZE]
+
+    def _read_from_buffer(self, start_index: int, size: int) -> bytearray:
+        data = bytearray()
+        for i in range(size):
+            data.append(self.buffer[(start_index + i) % self.BUFFER_SIZE])
+        return data
+
+    def _write_to_buffer(self, data: bytes) -> None:
+        with self.buffer_lock:
+            for byte in data:
+                self.buffer[self.write_index] = byte
+                # 检测帧尾 'L','Z','U'
+                if (byte == ord('U')
+                        and self._read_byte(self.write_index - 1) == ord('Z')
+                        and self._read_byte(self.write_index - 2) == ord('L')):
+                    start_index = (self.write_index - PACKET_SIZE + self.BUFFER_SIZE) % self.BUFFER_SIZE
+                    packet = self._read_from_buffer(start_index, PACKET_SIZE)
+                    self._process_packet(packet)
+                self.write_index = (self.write_index + 1) % self.BUFFER_SIZE
+
+    def _process_packet(self, packet: bytearray) -> None:
+        # 校验和
+        if (sum(packet[1:30]) % 256) != packet[0]:
+            return
+        try:
+            x, y, yaw, _fr, _fl, _bl, _br = struct.unpack('<fffffff', packet[1:29])
+            mode = packet[29]
+        except struct.error:
+            return
+
+        if self.require_mode_d and mode != MODE_DEAD_RECKON:
+            return
+
+        x /= 1000.0  # mm -> m
+        y /= 1000.0
+        self._publish(x, y, yaw)
+
+    # ------------------------------------------------------------------ #
+    # 发布 Odometry + TF
+    # ------------------------------------------------------------------ #
+    def _publish(self, x: float, y: float, yaw: float) -> None:
+        now = self.get_clock().now()
+        stamp = now.to_msg()
+        quat = yaw_to_quaternion(yaw)
+
+        # 差分求速度（body 系）；首帧或 dt 过小时置零
+        vx = vy = wz = 0.0
+        t = now.nanoseconds * 1e-9
+        if self.prev_t is not None:
+            dt = t - self.prev_t
+            if dt > 1e-3:
+                dx = x - self.prev_x
+                dy = y - self.prev_y
+                dyaw = math.atan2(math.sin(yaw - self.prev_yaw),
+                                  math.cos(yaw - self.prev_yaw))
+                # 世界系位移旋到 body 系
+                vx = (dx * math.cos(yaw) + dy * math.sin(yaw)) / dt
+                vy = (-dx * math.sin(yaw) + dy * math.cos(yaw)) / dt
+                wz = dyaw / dt
+        self.prev_x, self.prev_y, self.prev_yaw, self.prev_t = x, y, yaw, t
+
+        odom = Odometry()
+        odom.header.stamp = stamp
+        odom.header.frame_id = self.odom_frame_id
+        odom.child_frame_id = self.base_frame_id
+        odom.pose.pose.position.x = x
+        odom.pose.pose.position.y = y
+        odom.pose.pose.position.z = 0.0
+        odom.pose.pose.orientation = quat
+        odom.twist.twist.linear.x = vx
+        odom.twist.twist.linear.y = vy
+        odom.twist.twist.angular.z = wz
+        # 平面机器人协方差: x,y,yaw 给小值, z/roll/pitch 不可观给大值
+        odom.pose.covariance[0] = 0.01    # x
+        odom.pose.covariance[7] = 0.01    # y
+        odom.pose.covariance[14] = 1e6    # z
+        odom.pose.covariance[21] = 1e6    # roll
+        odom.pose.covariance[28] = 1e6    # pitch
+        odom.pose.covariance[35] = 0.02   # yaw
+        odom.twist.covariance[0] = 0.01
+        odom.twist.covariance[7] = 0.01
+        odom.twist.covariance[35] = 0.02
+        self.odom_pub.publish(odom)
+
+        if self.tf_broadcaster is not None:
+            tf = TransformStamped()
+            tf.header.stamp = stamp
+            tf.header.frame_id = self.odom_frame_id
+            tf.child_frame_id = self.base_frame_id
+            tf.transform.translation.x = x
+            tf.transform.translation.y = y
+            tf.transform.translation.z = 0.0
+            tf.transform.rotation = quat
+            self.tf_broadcaster.sendTransform(tf)
+
+    def destroy_node(self) -> bool:
+        self._running = False
+        self._close_serial()
+        return super().destroy_node()
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = ChassisOdometry()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

ros2/src/craic_localization/craic_localization/navigate_to_point.py

@@ -0,0 +1,253 @@
+#!/usr/bin/env python3
+"""导航程序 (navigate_to_point)：自动驱动麦轮底盘到示教记录的点位。
+
+- 目标点：读取 teach_points 生成的 taught_points.yaml（map 系 x,y,yaw）。
+- 反馈：TF map->base_footprint（= AMCL 实时定位），需 localization.launch.py 在运行。
+- 控制：holonomic P 控制器，把 map 系位姿误差投影到机体系，生成 (前进, 侧移, 转向)
+        速度，按 udp_teleop 的 XYW 约定经 UDP→ESP32 下发，到达容差内停车。
+
+速度符号默认按 udp_teleop/keyboard_control 推导：
+  前进 W→chassis_y 取负, 左移 A→chassis_x 取负, 左转(CCW) Q→chassis_w 取负
+  => sign_x = sign_y = sign_w = -1。若实车方向相反，翻转对应 sign 参数即可。
+
+用法：
+  # 交互选点
+  ros2 run craic_localization navigate_to_point --ros-args \
+      -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml
+  # 一次性去某点
+  ros2 run craic_localization navigate_to_point --ros-args -p points_file:=... -p goal:=B_1
+  # 运行中用话题指定（供上层任务程序调用）
+  ros2 topic pub -1 /goto std_msgs/String "{data: 'C_1'}"
+  # 干跑（只打印命令不发，安全验证逻辑/符号）
+  ros2 run craic_localization navigate_to_point --ros-args -p points_file:=... -p dry_run:=true
+
+安全：首次务必先 dry_run，再低速单点测试，手放 Ctrl+C（退出会自动发停车）。
+"""
+
+import math
+import socket
+import threading
+
+import rclpy
+import yaml
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.time import Time
+from std_msgs.msg import String
+from tf2_ros import (
+    Buffer,
+    ConnectivityException,
+    ExtrapolationException,
+    LookupException,
+    TransformListener,
+)
+
+
+def quat_to_yaw(qx, qy, qz, qw):
+    return math.atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz))
+
+
+def norm_angle(a):
+    return math.atan2(math.sin(a), math.cos(a))
+
+
+def clamp(v, lo, hi):
+    return max(lo, min(hi, v))
+
+
+class NavigateToPoint(Node):
+    def __init__(self):
+        super().__init__('navigate_to_point')
+        p = self.declare_parameter
+        self.points_file = p('points_file', 'taught_points.yaml').value
+        self.udp_ip = p('udp_ip', '192.168.4.1').value
+        self.udp_port = int(p('udp_port', 8888).value)
+        self.map_frame = p('map_frame', 'map').value
+        self.base_frame = p('base_frame', 'base_footprint').value
+        self.control_rate = float(p('control_rate', 20.0).value)
+        self.max_linear = float(p('max_linear', 60.0).value)      # 麦轮线速度命令上限(同 keyboard ~100)
+        self.max_angular = float(p('max_angular', 30.0).value)    # 角速度命令上限(同 keyboard ~45)
+        self.kp_linear = float(p('kp_linear', 150.0).value)       # m -> 命令单位
+        self.kp_angular = float(p('kp_angular', 40.0).value)      # rad -> 命令单位
+        self.pos_tol = float(p('pos_tolerance', 0.05).value)      # 到位容差(米)
+        self.yaw_tol = float(p('yaw_tolerance', 0.05).value)      # 到位容差(弧度)
+        self.sign_x = float(p('sign_x', -1.0).value)              # 侧移符号(机体左为正)
+        self.sign_y = float(p('sign_y', -1.0).value)              # 前进符号(机体前为正)
+        self.sign_w = float(p('sign_w', -1.0).value)              # 转向符号(CCW 为正)
+        self.goal_timeout = float(p('goal_timeout', 30.0).value)
+        self.dry_run = bool(p('dry_run', False).value)
+        goal_arg = str(p('goal', '').value)
+
+        self.points = self._load_points()
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+        self._lock = threading.Lock()
+        self.goal_name = None
+        self.goal = None          # (x, y, yaw)
+        self.goal_deadline = None
+        self.one_shot = bool(goal_arg)
+        self.done = False         # one_shot 完成标志
+
+        self.create_subscription(String, 'goto', self._goto_cb, 10)
+        self.timer = self.create_timer(1.0 / self.control_rate, self._control_tick)
+
+        self.get_logger().info(
+            f'navigate_to_point 就绪 (UDP {self.udp_ip}:{self.udp_port}, dry_run={self.dry_run})')
+        if goal_arg:
+            self.set_goal(goal_arg)
+
+    # -------- 点位 / 目标 -------- #
+    def _load_points(self):
+        try:
+            with open(self.points_file, 'r', encoding='utf-8') as f:
+                data = yaml.safe_load(f) or {}
+            pts = dict(data.get('points', {}))
+            self.get_logger().info(f'载入 {len(pts)} 个点: {", ".join(pts) or "(空)"}')
+            return pts
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().error(f'读取点位文件失败 {self.points_file}: {exc}')
+            return {}
+
+    def set_goal(self, name):
+        pt = self.points.get(name)
+        if pt is None:
+            self.get_logger().warn(f'未知点位: {name}（可用: {", ".join(self.points)}）')
+            return False
+        with self._lock:
+            self.goal_name = name
+            self.goal = (float(pt['x']), float(pt['y']), float(pt.get('yaw', 0.0)))
+            self.goal_deadline = self.get_clock().now() + Duration(seconds=self.goal_timeout)
+        self.get_logger().info(
+            f'前往 {name}: x={self.goal[0]:.3f} y={self.goal[1]:.3f} yaw={self.goal[2]:.3f}')
+        return True
+
+    def _goto_cb(self, msg):
+        self.set_goal(msg.data.strip())
+
+    # -------- 反馈 / 下发 -------- #
+    def _lookup(self):
+        try:
+            t = self.tf_buffer.lookup_transform(self.map_frame, self.base_frame, Time())
+        except (LookupException, ConnectivityException, ExtrapolationException):
+            return None
+        tr = t.transform.translation
+        q = t.transform.rotation
+        return (tr.x, tr.y, quat_to_yaw(q.x, q.y, q.z, q.w))
+
+    def _send(self, cx, cy, cw):
+        cmd = f'XYW:{int(round(cx))}:{int(round(cy))}:{int(round(cw))}:XZHY\n'.encode()
+        if self.dry_run:
+            self.get_logger().info(f'[dry] {cmd.decode().strip()}')
+        else:
+            self.sock.sendto(cmd, (self.udp_ip, self.udp_port))
+
+    def _stop(self):
+        self._send(0, 0, 0)
+
+    # -------- 控制环 -------- #
+    def _control_tick(self):
+        with self._lock:
+            goal, deadline, name = self.goal, self.goal_deadline, self.goal_name
+        if goal is None:
+            return
+        if self.get_clock().now() > deadline:
+            self.get_logger().warn(f'{name} 超时未到达，停车')
+            self._finish()
+            return
+
+        pose = self._lookup()
+        if pose is None:
+            self._stop()
+            self.get_logger().warn('无定位 (map->base_footprint)，停车等待…',
+                                   throttle_duration_sec=2.0)
+            return
+
+        px, py, pyaw = pose
+        tx, ty, tyaw = goal
+        dx, dy = tx - px, ty - py
+        dist = math.hypot(dx, dy)
+        dyaw = norm_angle(tyaw - pyaw)
+
+        if dist < self.pos_tol and abs(dyaw) < self.yaw_tol:
+            self.get_logger().info(
+                f'到达 {name}（位置误差 {dist * 100:.1f}cm，朝向误差 {math.degrees(dyaw):.1f}°）')
+            self._finish()
+            return
+
+        # map 系误差投影到机体系（x 前, y 左, REP-103）
+        e_fwd = dx * math.cos(pyaw) + dy * math.sin(pyaw)
+        e_left = -dx * math.sin(pyaw) + dy * math.cos(pyaw)
+
+        v_fwd = self.kp_linear * e_fwd
+        v_left = self.kp_linear * e_left
+        # 限制平移合速度（防止 x+y 叠加超限）
+        speed = math.hypot(v_fwd, v_left)
+        if speed > self.max_linear:
+            s = self.max_linear / speed
+            v_fwd *= s
+            v_left *= s
+        # 已到位置容差内则只修朝向
+        if dist < self.pos_tol:
+            v_fwd = v_left = 0.0
+        w = clamp(self.kp_angular * dyaw, -self.max_angular, self.max_angular)
+
+        self.get_logger().info(
+            f'{name}: 距离 {dist * 100:.1f}cm, 朝向差 {math.degrees(dyaw):.1f}°',
+            throttle_duration_sec=0.5)
+
+        # 机体速度 -> XYW（侧移=chassis_x, 前进=chassis_y, 转向=chassis_w）
+        self._send(self.sign_x * v_left, self.sign_y * v_fwd, self.sign_w * w)
+
+    def _finish(self):
+        self._stop()
+        with self._lock:
+            self.goal = self.goal_name = self.goal_deadline = None
+        if self.one_shot:
+            self.done = True
+
+    def destroy_node(self):
+        try:
+            self._stop()
+        except Exception:  # noqa: BLE001
+            pass
+        return super().destroy_node()
+
+
+def interactive(node: NavigateToPoint, stop_flag: threading.Event):
+    while not stop_flag.is_set() and rclpy.ok():
+        try:
+            line = input(f'去哪个点? ({", ".join(node.points) or "无"} | q 退出) > ').strip()
+        except EOFError:
+            break
+        low = line.lower()
+        if low in ('q', 'quit', 'exit'):
+            stop_flag.set()
+            break
+        elif low in ('s', 'stop'):
+            node._finish()
+        elif line:
+            node.set_goal(line)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = NavigateToPoint()
+    stop_flag = threading.Event()
+    if not node.one_shot:
+        threading.Thread(target=interactive, args=(node, stop_flag), daemon=True).start()
+    try:
+        while rclpy.ok() and not stop_flag.is_set() and not (node.one_shot and node.done):
+            rclpy.spin_once(node, timeout_sec=0.1)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node._stop()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

ros2/src/craic_localization/craic_localization/show_points.py

@@ -0,0 +1,125 @@
+#!/usr/bin/env python3
+"""在 rviz 显示所有示教点位：读 taught_points.yaml，发布 MarkerArray。
+
+每个点画：① 箭头 = 位置 + 朝向(yaw)；② 文字 = 点名。frame_id = map。
+周期性重读文件并发布，因此示教(teach_points)时新增/删除的点会实时反映到 rviz。
+
+rviz 里加 MarkerArray 显示、话题 /taught_points（localization.rviz 已内置该显示）。
+
+用法：
+  ros2 run craic_localization show_points --ros-args \
+      -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml
+"""
+
+import math
+import os
+
+import rclpy
+import yaml
+from geometry_msgs.msg import Quaternion
+from rclpy.node import Node
+from visualization_msgs.msg import Marker, MarkerArray
+
+
+def yaw_to_quat(yaw):
+    q = Quaternion()
+    q.z = math.sin(yaw / 2.0)
+    q.w = math.cos(yaw / 2.0)
+    return q
+
+
+class ShowPoints(Node):
+    def __init__(self):
+        super().__init__('show_points')
+        self.points_file = os.path.abspath(
+            self.declare_parameter('points_file', 'taught_points.yaml').value)
+        self.frame_id = self.declare_parameter('frame_id', 'map').value
+        self.rate = float(self.declare_parameter('rate', 1.0).value)
+
+        self.pub = self.create_publisher(MarkerArray, 'taught_points', 1)
+        self.timer = self.create_timer(1.0 / self.rate, self._tick)
+        self.get_logger().info(
+            f'show_points: {self.points_file} -> /taught_points (frame={self.frame_id})')
+
+    def _load(self):
+        try:
+            with open(self.points_file, 'r', encoding='utf-8') as f:
+                data = yaml.safe_load(f) or {}
+            return dict(data.get('points', {}))
+        except FileNotFoundError:
+            return {}
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f'读取点位失败: {exc}', throttle_duration_sec=5.0)
+            return {}
+
+    def _tick(self):
+        pts = self._load()
+        now = self.get_clock().now().to_msg()
+        arr = MarkerArray()
+
+        # 先清除上一帧（与新增放同一条消息，rviz 一次性处理，无闪烁），自动反映删除
+        clear = Marker()
+        clear.header.frame_id = self.frame_id
+        clear.action = Marker.DELETEALL
+        arr.markers.append(clear)
+
+        for i, (name, p) in enumerate(pts.items()):
+            try:
+                x, y = float(p['x']), float(p['y'])
+                yaw = float(p.get('yaw', 0.0))
+            except (KeyError, TypeError, ValueError):
+                continue
+
+            # 箭头：位置 + 朝向
+            a = Marker()
+            a.header.frame_id = self.frame_id
+            a.header.stamp = now
+            a.ns = 'arrow'
+            a.id = i
+            a.type = Marker.ARROW
+            a.action = Marker.ADD
+            a.pose.position.x = x
+            a.pose.position.y = y
+            a.pose.position.z = 0.05
+            a.pose.orientation = yaw_to_quat(yaw)
+            a.scale.x = 0.25   # 杆长
+            a.scale.y = 0.04   # 杆径
+            a.scale.z = 0.04   # 头径
+            a.color.r, a.color.g, a.color.b, a.color.a = 0.1, 0.8, 1.0, 1.0  # 青色
+            arr.markers.append(a)
+
+            # 文字：点名
+            t = Marker()
+            t.header.frame_id = self.frame_id
+            t.header.stamp = now
+            t.ns = 'label'
+            t.id = i
+            t.type = Marker.TEXT_VIEW_FACING
+            t.action = Marker.ADD
+            t.pose.position.x = x
+            t.pose.position.y = y
+            t.pose.position.z = 0.22
+            t.pose.orientation.w = 1.0
+            t.scale.z = 0.12   # 字高
+            t.color.r, t.color.g, t.color.b, t.color.a = 1.0, 1.0, 1.0, 1.0
+            t.text = name
+            arr.markers.append(t)
+
+        self.pub.publish(arr)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = ShowPoints()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

ros2/src/craic_localization/craic_localization/teach_points.py

@@ -0,0 +1,221 @@
+#!/usr/bin/env python3
+"""示教程序 (teach_points)：记录机器人在 map 坐标系下的位姿，存为 YAML 供导航复用。
+
+依赖定位栈在运行(localization.launch.py 提供 AMCL)，位姿来源为 TF: map -> base_footprint
+(= AMCL 实时定位结果)。用法是另开终端用 keyboard_control 把车开到目标点，回到本程序按回车记录。
+
+预设要记录的点(可用参数 points 覆盖)：B_1..B_6, C_1, D_1, E_1, F_1。
+也可随时用 `name <名字>` 记录任意自定义点。
+
+运行：
+  # 终端1：定位栈
+  ros2 launch craic_localization localization.launch.py map:=.../craic.yaml
+  # 终端2：遥控(开车到点)
+  ros2 run udp_teleop keyboard_control --ros-args --params-file src/udp_teleop/config/params.yaml
+  # 终端3：示教(记录)，建议把结果存到包的 config 下供导航读取
+  ros2 run craic_localization teach_points --ros-args \
+      -p output_file:=$PWD/src/craic_localization/config/taught_points.yaml
+
+交互命令：
+  回车 / r      记录"当前待记录点"的位姿，并前进到下一个
+  p             只打印当前位姿，不记录
+  name <X>      把当前位姿记录为自定义名字 X(已存在则覆盖)
+  del <X>       删除已记录的点 X
+  list          列出已记录的点
+  skip / back   跳过 / 回退当前待记录点
+  save          立即保存到文件
+  h             帮助
+  q             保存并退出
+"""
+
+import math
+import os
+import threading
+
+import rclpy
+import yaml
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.time import Time
+from tf2_ros import (
+    Buffer,
+    ConnectivityException,
+    ExtrapolationException,
+    LookupException,
+    TransformListener,
+)
+
+DEFAULT_POINTS = ['B_1', 'B_2', 'B_3', 'B_4', 'B_5', 'B_6', 'C_1', 'D_1', 'E_1', 'F_1']
+
+
+def quat_to_yaw(qx, qy, qz, qw):
+    """四元数 -> 绕 Z 偏航角(弧度)。"""
+    return math.atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz))
+
+
+class TeachPoints(Node):
+    def __init__(self):
+        super().__init__('teach_points')
+        self.map_frame = self.declare_parameter('map_frame', 'map').value
+        self.base_frame = self.declare_parameter('base_frame', 'base_footprint').value
+        self.output_file = os.path.abspath(
+            self.declare_parameter('output_file', 'taught_points.yaml').value)
+        self.points = list(self.declare_parameter('points', DEFAULT_POINTS).value)
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+
+        self.recorded = {}  # name -> {x, y, yaw}
+        self._load_existing()
+
+    # -------- 位姿读取 -------- #
+    def lookup_pose(self):
+        """读取 map->base_frame 的最新变换，返回 ({x,y,yaw}, None) 或 (None, 错误信息)。"""
+        try:
+            t = self.tf_buffer.lookup_transform(
+                self.map_frame, self.base_frame, Time(),
+                timeout=Duration(seconds=1.0))
+        except (LookupException, ConnectivityException, ExtrapolationException) as exc:
+            return None, str(exc)
+        tr = t.transform.translation
+        q = t.transform.rotation
+        yaw = quat_to_yaw(q.x, q.y, q.z, q.w)
+        return {'x': round(tr.x, 4), 'y': round(tr.y, 4), 'yaw': round(yaw, 4)}, None
+
+    # -------- 文件读写 -------- #
+    def _load_existing(self):
+        if os.path.isfile(self.output_file):
+            try:
+                with open(self.output_file, 'r', encoding='utf-8') as f:
+                    data = yaml.safe_load(f) or {}
+                self.recorded = dict(data.get('points', {}))
+                if self.recorded:
+                    print(f'已载入已有记录 {len(self.recorded)} 个点：{self.output_file}')
+            except Exception as exc:  # noqa: BLE001
+                print(f'载入已有文件失败(忽略)：{exc}')
+
+    def save(self):
+        os.makedirs(os.path.dirname(self.output_file) or '.', exist_ok=True)
+        header = (
+            '# 示教记录的点位（map 坐标系；x,y 单位米，yaw 单位弧度）\n'
+            '# 由 craic_localization/teach_points 生成\n'
+        )
+        body = yaml.safe_dump({'points': self.recorded},
+                              default_flow_style=False, sort_keys=False,
+                              allow_unicode=True)
+        with open(self.output_file, 'w', encoding='utf-8') as f:
+            f.write(header + body)
+
+
+HELP = __doc__.split('交互命令：', 1)[1]
+
+
+def interactive(node: TeachPoints):
+    idx = 0
+    print('=' * 60)
+    print('  示教程序 teach_points —— 记录 map 系下位姿')
+    print(f'  输出文件: {node.output_file}')
+    print(f'  预设点: {", ".join(node.points)}')
+    print('  (输入 h 查看命令)')
+    print('=' * 60)
+
+    while rclpy.ok():
+        target = node.points[idx] if idx < len(node.points) else None
+        tag = target if target else '(预设已完，用 name <X> 记录自定义点)'
+        try:
+            line = input(f'[已记录 {len(node.recorded)}] 下一个: {tag} > ').strip()
+        except EOFError:
+            break
+        cmd = line.lower()
+
+        if cmd in ('q', 'quit', 'exit'):
+            node.save()
+            print(f'已保存到 {node.output_file}，退出。')
+            break
+
+        elif cmd in ('', 'r'):
+            if target is None:
+                print('  预设点已记录完。用 "name <X>" 记录自定义点，或 q 退出。')
+                continue
+            pose, err = node.lookup_pose()
+            if err:
+                print(f'  ✗ 读取位姿失败(定位栈在运行吗?): {err}')
+                continue
+            node.recorded[target] = pose
+            node.save()
+            print(f'  ✓ {target} = x:{pose["x"]} y:{pose["y"]} '
+                  f'yaw:{pose["yaw"]}rad ({math.degrees(pose["yaw"]):.1f}°)  [已存盘]')
+            idx += 1
+
+        elif cmd == 'p':
+            pose, err = node.lookup_pose()
+            if err:
+                print(f'  ✗ 读取位姿失败: {err}')
+            else:
+                print(f'  当前位姿 x:{pose["x"]} y:{pose["y"]} '
+                      f'yaw:{pose["yaw"]}rad ({math.degrees(pose["yaw"]):.1f}°)')
+
+        elif cmd.startswith('name '):
+            name = line[5:].strip()
+            if not name:
+                print('  用法: name <名字>')
+                continue
+            pose, err = node.lookup_pose()
+            if err:
+                print(f'  ✗ 读取位姿失败: {err}')
+                continue
+            node.recorded[name] = pose
+            node.save()
+            print(f'  ✓ {name} = x:{pose["x"]} y:{pose["y"]} '
+                  f'yaw:{pose["yaw"]}rad  [已存盘]')
+
+        elif cmd.startswith('del '):
+            name = line[4:].strip()
+            if node.recorded.pop(name, None) is not None:
+                node.save()
+                print(f'  已删除 {name}  [已存盘]')
+            else:
+                print(f'  没有名为 {name} 的记录')
+
+        elif cmd == 'list':
+            if not node.recorded:
+                print('  (空)')
+            for k, v in node.recorded.items():
+                print(f'    {k}: x:{v["x"]} y:{v["y"]} yaw:{v["yaw"]}')
+
+        elif cmd == 'skip':
+            idx += 1
+
+        elif cmd == 'back':
+            idx = max(0, idx - 1)
+
+        elif cmd == 'save':
+            node.save()
+            print(f'  已保存到 {node.output_file}')
+
+        elif cmd in ('h', 'help', '?'):
+            print(HELP)
+
+        else:
+            print('  未知命令，输入 h 查看帮助。')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = TeachPoints()
+    # 后台 spin 以填充 TF 缓冲
+    spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
+    spin_thread.start()
+    try:
+        interactive(node)
+    except KeyboardInterrupt:
+        node.save()
+        print(f'\n中断，已保存到 {node.output_file}')
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()