Please enter the commit message for your changes. Lines starting

with '#' will be ignored, and an empty message aborts the commit.

On branch master
Your branch is ahead of 'origin/master' by 2 commits.
  (use "git push" to publish your local commits)

Changes to be committed:
	modified:   .gitignore
	modified:   README.md
	new file:   bashes/auto-wifi-connect.service
	new file:   bashes/auto-wifi-connect.sh
	deleted:    keyboard_control.py
	new file:   my_model/image.png
	new file:   path_follower_demo.py
	new file:   scripts/PIDtracking.py
	new file:   scripts/__pycache__/publish_sine_path.cpython-310.pyc
	new file:   scripts/publish_sine_path.py
	modified:   src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml
	modified:   src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak
	new file:   src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak2
	modified:   src/origincar_base/config/ekf.yaml
	new file:   src/origincar_base/config/ekf.yaml.bak
	modified:   src/origincar_base/launch/base_serial.launch.py
	new file:   src/origincar_base/launch/base_serial.launch.py.bak
	modified:   src/origincar_base/launch/origincar_bringup.launch.py
	new file:   src/past_control/CMakeLists.txt
	new file:   src/past_control/config/past_control.yaml
	new file:   src/past_control/include/past_control/tools.h
	new file:   src/past_control/launch/past_control.launch.py
	new file:   src/past_control/msg/Obstacle.msg
	new file:   src/past_control/msg/ObstacleArray.msg
	new file:   src/past_control/package.xml
	new file:   src/past_control/src/lane_follower_node.cpp
	new file:   src/past_control/src/obstacle_detector_node.cpp
	new file:   src/past_control/src/racing_orchestrator.cpp
	new file:   src/planner/CMakeLists.txt
	new file:   src/planner/config/planner.yaml
	new file:   src/planner/launch/planner.launch.py
	new file:   src/planner/package.xml
	new file:   src/planner/src/planner_version.cpp
	modified:   src/qr_detection/src/qr_dete_depth.cpp
	new file:   src/racing_control/CMakeLists.txt
	new file:   src/racing_control/include/racing_control/racing_control.hpp
	new file:   src/racing_control/package.xml
	new file:   src/racing_control/src/racing_control.cpp
	modified:   src/vlm_detect/setup.py
	new file:   src/vlm_detect/vlm_detect/__pycache__/__init__.cpython-310.pyc
	new file:   src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc
	new file:   src/vlm_detect/vlm_detect/test_publisher.py
	new file:   src/vlm_detect/vlm_detect/tts_node.py
	modified:   src/vlm_detect/vlm_detect/vlm_node.py
	new file:   tools/measure_turning_radius.py
	new file:   tools/set_volume.py
	new file:   tools/udp_to_cmdvel.py
	new file:   tools/windows_keyboard_control.py
	new file:   vlm_server.py
	new file:   "\350\260\203\350\257\225\350\256\260\345\275\225.Assets/1.png"
	renamed:    "\350\260\203\350\257\225\350\256\260\345\275\225.log" -> "\350\260\203\350\257\225\350\256\260\345\275\225.md"

.gitignore

@@ -10,3 +10,4 @@ log
 # VSCode database
 .vscode
 
+datas

README.md

@@ -104,4 +104,9 @@ foxglove # 启动foxbridge
 
 运行二维码检测（无缓存版）：
 `ros2 run qr_detection qr_dete_depth_node --ros-args -p use_buffer:=false`
-# 五、日志
+
+# 五、tools 工具使用
+`measure_turning_radius.py`：功能为测试实际转向角度与输入的关系。启动后，会在/turning_radius_data文件夹下生成测试数据
+`set_volume.py`：功能为设置USB声卡的音量，修改第七行或直接`python3 set_volume.py 80`即可更改为80（或任何你想要的数字）
+`windows_keyboard_control.py`：用于Windows端发送Twist控制命令（键盘遥控）
+`udp_to_cmdvel.py`：用于板端接收键盘遥控命令（与上一个文件联动）

bashes/auto-wifi-connect.service

@@ -0,0 +1,16 @@
+[Unit]
+Description=开机自动连接 WiFi（优先 openwrt_5G，回退 kskbl_2.4G）
+After=NetworkManager.service
+Wants=NetworkManager.service
+Before=network-online.target
+WantedBy=multi-user.target
+
+[Service]
+Type=oneshot
+ExecStart=/usr/local/bin/auto-wifi-connect.sh
+RemainAfterExit=yes
+StandardOutput=journal+console
+StandardError=journal+console
+
+[Install]
+WantedBy=multi-user.target

bashes/auto-wifi-connect.sh

@@ -0,0 +1,78 @@
+#!/bin/bash
+# ============================================================================
+# auto-wifi-connect.sh — 开机自动连接 WiFi
+#
+# 优先级：openwrt_5G > kskbl_2.4G
+# 扫描可用网络，优先连接 5G 热点，不可用时回退到 2.4G
+#
+# 退出码：
+#   0 — 成功连接
+#   1 — NetworkManager 未就绪（超时）
+#   2 — 两个热点均不可用
+#   3 — 连接失败
+# ============================================================================
+
+PRIMARY="openwrt_5G"
+FALLBACK="kskbl_2.4G"
+WIFI_PASSWORD="11112222"
+MAX_WAIT=30          # 最多等 NetworkManager 就绪的秒数
+SCAN_WAIT=3          # WiFi 扫描后的等待秒数
+
+log() {
+    echo "[auto-wifi] $(date '+%H:%M:%S') $*" | tee /dev/kmsg 2>/dev/null
+}
+
+# ── 等待 NetworkManager 就绪 ──
+log "等待 NetworkManager 就绪..."
+for i in $(seq 1 $MAX_WAIT); do
+    state=$(nmcli -t -f STATE general status 2>/dev/null)
+    if [ -n "$state" ]; then
+        log "NetworkManager 状态: $state"
+        break
+    fi
+    sleep 2
+done
+
+if [ -z "$state" ]; then
+    log "错误: NetworkManager 在 ${MAX_WAIT}s 内未就绪"
+    exit 1
+fi
+
+# ── 获取 WiFi 网卡名 ──
+WIFI_IFACE=$(nmcli -t -f DEVICE,TYPE device status 2>/dev/null | grep ':wifi$' | cut -d: -f1 | head -1)
+if [ -z "$WIFI_IFACE" ]; then
+    log "错误: 未找到 WiFi 网卡"
+    exit 2
+fi
+log "WiFi 网卡: $WIFI_IFACE"
+
+# ── 先检查是否已经连接到目标热点 ──
+current=$(nmcli -t -f GENERAL.CONNECTION device show "$WIFI_IFACE" 2>/dev/null | cut -d: -f2)
+if [ "$current" = "$PRIMARY" ] || [ "$current" = "$FALLBACK" ]; then
+    log "已连接到 $current，无须切换"
+    exit 0
+fi
+
+# ── 扫描 WiFi ──
+log "正在扫描 WiFi..."
+nmcli device wifi rescan 2>/dev/null
+sleep $SCAN_WAIT
+
+# ── 检查 PRIMARY 是否可见 ──
+if nmcli -t -f SSID device wifi list 2>/dev/null | grep -qx "$PRIMARY"; then
+    TARGET="$PRIMARY"
+    log "检测到 $PRIMARY"
+else
+    TARGET="$FALLBACK"
+    log "$PRIMARY 不可用，回退到 $TARGET"
+fi
+
+# ── 连接 ──
+log "正在连接 $TARGET ..."
+if nmcli device wifi connect "$TARGET" password "$WIFI_PASSWORD" ifname "$WIFI_IFACE" 2>&1; then
+    log "成功连接到 $TARGET"
+    exit 0
+else
+    log "错误: 无法连接到 $TARGET"
+    exit 3
+fi

keyboard_control.py

@@ -1,121 +0,0 @@
-#!/usr/bin/env python3
-"""
-Keyboard control node for robot chassis.
-W/S: forward/backward
-A/D: turn left/right
-I/K: increase/decrease linear speed
-J/L: increase/decrease angular speed
-X: stop
-Q: quit
-"""
-
-import rclpy
-from rclpy.node import Node
-from geometry_msgs.msg import Twist
-import sys
-import select
-import termios
-import tty
-
-HELP = """
-========================================
-  Keyboard Control
-========================================
-  W/S    : forward / backward
-  A/D    : turn left / turn right
-  I/K    : linear  speed +/- (step 0.05)
-  J/L    : angular speed +/- (step 0.1)
-  X/Space: stop
-  Q      : quit
-========================================
-  Current: lin=%.2f  ang=%.2f
-"""
-
-
-class KeyboardControl(Node):
-    def __init__(self):
-        super().__init__("keyboard_control")
-        self.pub = self.create_publisher(Twist, "/cmd_vel", 10)
-
-        self.lin_speed = 0.2
-        self.ang_speed = 0.5
-        self.lin_step = 0.05
-        self.ang_step = 0.1
-
-        self.print_help()
-        self.create_timer(0.05, self.read_key)
-
-    def print_help(self):
-        print(HELP % (self.lin_speed, self.ang_speed))
-
-    def get_key(self):
-        fd = sys.stdin.fileno()
-        old = termios.tcgetattr(fd)
-        try:
-            tty.setraw(fd)
-            r, _, _ = select.select([sys.stdin], [], [], 0.05)
-            if r:
-                return sys.stdin.read(1)
-            return None
-        finally:
-            termios.tcsetattr(fd, termios.TCSADRAIN, old)
-
-    def read_key(self):
-        key = self.get_key()
-        if key is None:
-            return
-
-        twist = Twist()
-
-        if key == 'w':
-            twist.linear.x = self.lin_speed
-        elif key == 's':
-            twist.linear.x = -self.lin_speed
-        elif key == 'a':
-            twist.angular.z = self.ang_speed
-        elif key == 'd':
-            twist.angular.z = -self.ang_speed
-        elif key == 'i':
-            self.lin_speed = round(self.lin_speed + self.lin_step, 2)
-            self.print_help()
-            return
-        elif key == 'k':
-            self.lin_speed = round(max(0.0, self.lin_speed - self.lin_step), 2)
-            self.print_help()
-            return
-        elif key == 'j':
-            self.ang_speed = round(self.ang_speed + self.ang_step, 2)
-            self.print_help()
-            return
-        elif key == 'l':
-            self.ang_speed = round(max(0.0, self.ang_speed - self.ang_step), 2)
-            self.print_help()
-            return
-        elif key == 'x' or key == ' ':
-            twist.linear.x = 0.0
-            twist.angular.z = 0.0
-            print("*** STOP ***")
-        elif key == 'q' or ord(key) == 3:
-            twist.linear.x = 0.0
-            twist.angular.z = 0.0
-            self.pub.publish(twist)
-            self.get_logger().info("Quitting...")
-            raise SystemExit
-        else:
-            return
-
-        self.pub.publish(twist)
-
-
-def main():
-    rclpy.init()
-    try:
-        rclpy.spin(KeyboardControl())
-    except SystemExit:
-        pass
-    finally:
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    main()

path_follower_demo.py

@@ -0,0 +1,199 @@
+#!/usr/bin/env python3
+"""
+PID 路径跟随 Demo — 最简版本
+
+订阅 /plan (nav_msgs/Path)，收到路径后立即开始 PID 跟随。
+不需要 planner_server，不需要状态机，不需要任何额外节点。
+
+用法：
+  python3 path_follower_demo.py
+
+然后发一条 /plan 即可：
+  ros2 topic pub /plan nav_msgs/msg/Path "{header: {frame_id: 'map'}, poses: [
+    {pose: {position: {x: 1.0, y: 0.0}}},
+    {pose: {position: {x: 2.0, y: 0.5}}},
+    {pose: {position: {x: 3.0, y: 0.0}}}
+  ]}"
+"""
+
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry, Path
+from geometry_msgs.msg import Twist
+import math
+import time
+
+
+class PathFollower(Node):
+
+    def __init__(self):
+        super().__init__("path_follower")
+
+        # ── PID 参数 ──
+        self.declare_parameter("kp", 0.8)
+        self.declare_parameter("ki", 0.0)
+        self.declare_parameter("kd", 0.05)
+        self.declare_parameter("linear_speed", 0.3)     # 前进速度 m/s
+        self.declare_parameter("waypoint_tolerance", 0.1)  # 到达判定距离 m
+        self.declare_parameter("max_angular", 10.0)       # 最大角速度 rad/s
+
+        # ── 订阅 /plan 和 /odom ──
+        self.plan_sub = self.create_subscription(
+            Path, "/plan", self.plan_cb, 10)
+        self.odom_sub = self.create_subscription(
+            Odometry, "/odom", self.odom_cb, 10)
+
+        # ── 发布 /cmd_vel ──
+        self.cmd_pub = self.create_publisher(Twist, "/cmd_vel", 10)
+
+        # ── 状态 ──
+        self.path = []              # [(x, y), ...] 路径点列表(世界坐标)
+        self.current_idx = 0        # 当前目标 waypoint 索引
+        self.odom = None            # 最新里程计
+        self.following = False      # 是否正在跟随
+
+        # ── PID 状态 ──
+        self._integral = 0.0
+        self._last_err = 0.0
+        self._last_time = None
+
+        # ── 主循环 20Hz ──
+        self.timer = self.create_timer(0.05, self.control_loop)
+
+        self.get_logger().info("PathFollower ready, waiting for /plan...")
+
+    # ── 回调 ──
+    def plan_cb(self, msg: Path):
+        """收到新路径，替换当前路径并开始跟随"""
+        if len(msg.poses) < 2:
+            return
+
+        self.path = [(p.pose.position.x, p.pose.position.y)
+                     for p in msg.poses]
+        self.current_idx = 0
+        self.following = True
+        self._reset_pid()
+        self.get_logger().info(
+            f"Received path with {len(self.path)} waypoints, "
+            f"from ({self.path[0][0]:.2f}, {self.path[0][1]:.2f}) "
+            f"to ({self.path[-1][0]:.2f}, {self.path[-1][1]:.2f})")
+
+    def odom_cb(self, msg: Odometry):
+        self.odom = msg
+
+    # ── PID ──
+    def _reset_pid(self):
+        self._integral = 0.0
+        self._last_err = 0.0
+        self._last_time = None
+
+    def _pid(self, error: float, dt: float) -> float:
+        kp = self.get_parameter("kp").value
+        ki = self.get_parameter("ki").value
+        kd = self.get_parameter("kd").value
+
+        self._integral += error * dt
+        if dt > 0.001:
+            derivative = (error - self._last_err) / dt
+        else:
+            derivative = 0.0
+        self._last_err = error
+
+        return kp * error + ki * self._integral + kd * derivative
+
+    # ── 主控制循环 ──
+    def control_loop(self):
+        if not self.following or self.odom is None:
+            self._publish_cmd(0.0, 0.0)
+            return
+
+        # 当前位姿
+        x = self.odom.pose.pose.position.x
+        y = self.odom.pose.pose.position.y
+        yaw = self._quat_to_yaw(self.odom.pose.pose.orientation)
+
+        # 当前目标 waypoint
+        if self.current_idx >= len(self.path):
+            self._publish_cmd(0.0, 0.0)
+            if self.following:
+                self.get_logger().info("Path completed!")
+                self.following = False
+            return
+
+        tx, ty = self.path[self.current_idx]
+        dx = tx - x
+        dy = ty - y
+        dist = math.hypot(dx, dy)
+
+        # 到达当前 waypoint → 进下一个
+        tolerance = self.get_parameter("waypoint_tolerance").value
+        if dist < tolerance:
+            self.current_idx += 1
+            if self.current_idx >= len(self.path):
+                self._publish_cmd(0.0, 0.0)
+                self.get_logger().info("Path completed!")
+                self.following = False
+                return
+            tx, ty = self.path[self.current_idx]
+            dx = tx - x
+            dy = ty - y
+
+        # 航向误差
+        target_heading = math.atan2(dy, dx)
+        heading_err = target_heading - yaw
+        heading_err = math.atan2(math.sin(heading_err), math.cos(heading_err))
+
+        # PID → 角速度
+        now = time.time()
+        dt = now - self._last_time if self._last_time else 0.05
+        self._last_time = now
+        angular = self._pid(heading_err, dt)
+
+        # 限幅
+        max_ang = self.get_parameter("max_angular").value
+        angular = max(-max_ang, min(max_ang, angular))
+
+        # 速度：误差大时减速
+        linear = self.get_parameter("linear_speed").value
+        if abs(heading_err) > 0.5:
+            linear *= 0.5  # 航向偏差超过 ~30° 时减速
+        if abs(heading_err) > 1.0:
+            linear *= 0.3  # 偏差超过 ~57° 时更慢
+
+        self._publish_cmd(linear, angular)
+
+        # 每 2 秒打印一次状态
+        if int(now * 10) % 20 == 0:
+            self.get_logger().info(
+                f"WP[{self.current_idx}/{len(self.path)}] "
+                f"target=({tx:.2f},{ty:.2f}) dist={dist:.3f} "
+                f"err={heading_err:.2f}rad cmd=(v={linear:.2f},ω={angular:.2f})")
+
+    # ── 工具 ──
+    def _publish_cmd(self, v, w):
+        twist = Twist()
+        twist.linear.x = v
+        twist.angular.z = w
+        self.cmd_pub.publish(twist)
+
+    @staticmethod
+    def _quat_to_yaw(q) -> float:
+        return math.atan2(2.0 * (q.w * q.z + q.x * q.y),
+                          1.0 - 2.0 * (q.y * q.y + q.z * q.z))
+
+
+def main():
+    rclpy.init()
+    node = PathFollower()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node._publish_cmd(0.0, 0.0)
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

scripts/PIDtracking.py

@@ -0,0 +1,464 @@
+#!/usr/bin/env python3
+"""
+PIDtracking — 跟踪数据录制 + 可视化一体脚本
+
+用法:
+  录制:  python3 PIDtracking.py record
+  制图:  python3 PIDtracking.py plot <数据目录>
+
+输出目录: ~/yiliao_ws/datas/PIDtracking/<timestamp>/
+输出文件:
+  plan.csv       — 路径点 (x, y, yaw)
+  cmd_vel.csv    — 控制命令 (t, vx, vz)
+  odom.csv       — 里程计位姿 (t, x, y, yaw)
+  goal_pose.csv  — 目标点 (x, y)
+  meta.csv       — 录制参数
+
+制图输出:
+  <dir>/plots/01_trajectory.png   — 轨迹对比
+  <dir>/plots/02_cmd_vel.png      — 控制命令时间序列
+  <dir>/plots/03_errors.png       — 横向/航向误差
+  <dir>/plots/04_curvature.png    — 曲率分析
+"""
+
+import os
+import sys
+import time
+import math
+import csv
+import threading
+import pathlib
+
+# ── 录制依赖 (ros2) ──
+RECORD_AVAILABLE = False
+try:
+    import rclpy
+    from rclpy.node import Node
+    from nav_msgs.msg import Path as NavPath, Odometry
+    from geometry_msgs.msg import Twist, PoseStamped
+    RECORD_AVAILABLE = True
+except ImportError:
+    pass
+
+# ── 制图依赖 ──
+PLOT_AVAILABLE = False
+try:
+    import numpy as np
+    import matplotlib
+    matplotlib.use("Agg")
+    import matplotlib.pyplot as plt
+    PLOT_AVAILABLE = True
+except ImportError:
+    pass
+
+# ============================================================================
+#  录制
+# ============================================================================
+def mode_record(args):
+    if not RECORD_AVAILABLE:
+        print("ERROR: rclpy not available. Run on the robot with ROS2 sourced.")
+        sys.exit(1)
+
+    base_path = pathlib.Path.home() / "yiliao_ws" / "datas" / "PIDtracking"
+    if len(args) > 2:
+        base_path = pathlib.Path(args[2])
+    ts = time.strftime("%Y%m%d_%H%M%S")
+    out_dir = base_path / ts
+    out_dir.mkdir(parents=True, exist_ok=True)
+
+    rclpy.init()
+    recorder = RecorderNode(out_dir)
+    print(f"[RECORD] Output: {out_dir}")
+    print(f"[RECORD] Subscribing: /plan, /cmd_vel, /goal_pose, /odom_combined (fallback /odom)")
+    print(f"[RECORD] Send /goal_pose to start. Ctrl+C to stop.")
+    try:
+        rclpy.spin(recorder)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        recorder.save_all()
+        recorder.destroy_node()
+        rclpy.shutdown()
+    print(f"[RECORD] Saved {len(recorder.plan_data)} plans, {len(recorder.cmd_data)} cmd_vel, "
+          f"{len(recorder.odom_data)} odom, {len(recorder.goal_data)} goals → {out_dir}")
+
+
+class RecorderNode(Node):
+    def __init__(self, out_dir):
+        super().__init__("pidtracking_recorder")
+        self.out_dir = out_dir
+        self.lock = threading.Lock()
+
+        self.plan_data = []   # [(t, [(x,y,yaw),...])]
+        self.cmd_data = []    # [(t, vx, vz)]
+        self.odom_data = []   # [(t, x, y, yaw)]
+        self.goal_data = []   # [(t, x, y)]
+
+        # 订阅
+        self.plan_sub = self.create_subscription(NavPath, "/plan", self.plan_cb, 10)
+        self.cmd_sub = self.create_subscription(Twist, "/cmd_vel", self.cmd_cb, 10)
+        self.goal_sub = self.create_subscription(PoseStamped, "/goal_pose", self.goal_cb, 10)
+
+        # odom: 优先 odom_combined（EKF融合），否则 /odom
+        odom_topic = "/odom_combined"
+        topics = self.get_topic_names_and_types()
+        if "/odom_combined" not in [t[0] for t in topics]:
+            odom_topic = "/odom"
+            self.get_logger().info(f"/odom_combined not found, using {odom_topic}")
+        self.odom_sub = self.create_subscription(Odometry, odom_topic, self.odom_cb, 10)
+
+    def plan_cb(self, msg):
+        t = time.time()
+        poses = []
+        for p in msg.poses:
+            qx, qy, qz, qw = (p.pose.orientation.x, p.pose.orientation.y,
+                               p.pose.orientation.z, p.pose.orientation.w)
+            yaw = math.atan2(2.0*(qw*qz + qx*qy), 1.0 - 2.0*(qy*qy + qz*qz))
+            poses.append((p.pose.position.x, p.pose.position.y, yaw))
+        with self.lock:
+            self.plan_data.append((t, poses))
+
+    def cmd_cb(self, msg):
+        with self.lock:
+            self.cmd_data.append((time.time(), msg.linear.x, msg.angular.z))
+
+    def odom_cb(self, msg):
+        qx, qy, qz, qw = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
+                           msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
+        yaw = math.atan2(2.0*(qw*qz + qx*qy), 1.0 - 2.0*(qy*qy + qz*qz))
+        with self.lock:
+            self.odom_data.append((time.time(), msg.pose.pose.position.x,
+                                   msg.pose.pose.position.y, yaw))
+
+    def goal_cb(self, msg):
+        with self.lock:
+            self.goal_data.append((time.time(), msg.pose.position.x, msg.pose.position.y))
+        self.get_logger().info(f"Goal: ({msg.pose.position.x:.2f}, {msg.pose.position.y:.2f})")
+
+    def save_all(self):
+        with self.lock:
+            # plan
+            with open(self.out_dir / "plan.csv", "w", newline="") as f:
+                w = csv.writer(f)
+                w.writerow(["t", "x", "y", "yaw"])
+                for t, poses in self.plan_data:
+                    for x, y, yaw in poses:
+                        w.writerow([t, x, y, yaw])
+
+            # cmd_vel
+            with open(self.out_dir / "cmd_vel.csv", "w", newline="") as f:
+                w = csv.writer(f)
+                w.writerow(["t", "vx", "vz"])
+                for t, vx, vz in self.cmd_data:
+                    w.writerow([t, vx, vz])
+
+            # odom
+            with open(self.out_dir / "odom.csv", "w", newline="") as f:
+                w = csv.writer(f)
+                w.writerow(["t", "x", "y", "yaw"])
+                for t, x, y, yaw in self.odom_data:
+                    w.writerow([t, x, y, yaw])
+
+            # goal
+            with open(self.out_dir / "goal_pose.csv", "w", newline="") as f:
+                w = csv.writer(f)
+                w.writerow(["t", "x", "y"])
+                for t, x, y in self.goal_data:
+                    w.writerow([t, x, y])
+
+            # meta
+            with open(self.out_dir / "meta.csv", "w", newline="") as f:
+                w = csv.writer(f)
+                w.writerow(["key", "value"])
+                w.writerow(["plans_received", len(self.plan_data)])
+                w.writerow(["cmd_vel_msgs", len(self.cmd_data)])
+                w.writerow(["odom_msgs", len(self.odom_data)])
+                w.writerow(["goals_received", len(self.goal_data)])
+
+
+# ============================================================================
+#  制图
+# ============================================================================
+def mode_plot(args):
+    if not PLOT_AVAILABLE:
+        print("ERROR: numpy/matplotlib not available. pip install numpy matplotlib")
+        sys.exit(1)
+
+    if len(args) < 3:
+        print("Usage: python3 PIDtracking.py plot <data_dir>")
+        sys.exit(1)
+
+    data_dir = pathlib.Path(args[2])
+    if not data_dir.exists():
+        print(f"ERROR: Directory not found: {data_dir}")
+        sys.exit(1)
+
+    out_dir = data_dir / "plots"
+    out_dir.mkdir(exist_ok=True)
+
+    # 读取数据
+    plan = _read_plan(data_dir / "plan.csv")
+    cmd_vel = _read_csv(data_dir / "cmd_vel.csv", ["t", "vx", "vz"])
+    odom = _read_csv(data_dir / "odom.csv", ["t", "x", "y", "yaw"])
+    goals = _read_csv(data_dir / "goal_pose.csv", ["t", "x", "y"])
+
+    print(f"Data: plan={len(plan)} paths, cmd_vel={len(cmd_vel)} msgs, odom={len(odom)} msgs")
+
+    if len(odom) == 0:
+        print("ERROR: No odometry data")
+        sys.exit(1)
+
+    t0 = odom[0]["t"]
+
+    # ── 图1: 轨迹 ──
+    _plot_trajectory(plan, odom, goals, out_dir)
+
+    # ── 图2: cmd_vel ──
+    if cmd_vel:
+        _plot_cmd_vel(cmd_vel, t0, out_dir)
+
+    # ── 图3: 误差 ──
+    if plan and odom:
+        _plot_errors(plan, odom, t0, out_dir)
+
+    # ── 图4: 曲率 ──
+    if plan and cmd_vel:
+        _plot_curvature(plan, out_dir)
+
+    print(f"\nPlots saved to: {out_dir}/")
+
+
+def _read_csv(path, columns):
+    if not path.exists():
+        return []
+    data = []
+    with open(path, "r") as f:
+        reader = csv.DictReader(f)
+        for row in reader:
+            item = {}
+            for col in columns:
+                item[col] = float(row[col])
+            data.append(item)
+    return data
+
+
+def _read_plan(path):
+    """读取 plan.csv: t,x,y,yaw，按 t 分组"""
+    if not path.exists():
+        return []
+    plans = []
+    current_t = None
+    current_poses = []
+    with open(path, "r") as f:
+        reader = csv.DictReader(f)
+        for row in reader:
+            t = float(row["t"])
+            x, y, yaw = float(row["x"]), float(row["y"]), float(row["yaw"])
+            if current_t is None:
+                current_t = t
+            if abs(t - current_t) > 0.01:  # 新的一组
+                plans.append({"t": current_t, "poses": current_poses})
+                current_t = t
+                current_poses = []
+            current_poses.append({"x": x, "y": y, "yaw": yaw})
+        if current_poses:
+            plans.append({"t": current_t, "poses": current_poses})
+    return plans
+
+
+def _plot_trajectory(plans, odom, goals, out_dir):
+    fig, ax = plt.subplots(figsize=(10, 10))
+
+    # 最新 plan
+    if plans:
+        last = plans[-1]["poses"]
+        if last:
+            px = [p["x"] for p in last]
+            py = [p["y"] for p in last]
+            ax.plot(px, py, "b--", lw=1.5, alpha=0.6, label="Plan")
+            ax.scatter(px[0], py[0], c="blue", s=80, marker="o", zorder=5, label="Plan Start")
+            ax.scatter(px[-1], py[-1], c="blue", s=80, marker="x", zorder=5, label="Plan Goal")
+
+    # odom
+    ox = np.array([d["x"] for d in odom])
+    oy = np.array([d["y"] for d in odom])
+    oyaw = np.array([d["yaw"] for d in odom])
+    ax.plot(ox, oy, "r-", lw=1.2, alpha=0.85, label="Actual")
+    ax.scatter(ox[0], oy[0], c="red", s=60, marker="o", zorder=5, label="Start")
+    ax.scatter(ox[-1], oy[-1], c="red", s=60, marker="x", zorder=5, label="End")
+
+    # 航向箭头
+    step = max(1, len(ox) // 25)
+    for i in range(0, len(ox), step):
+        dx = 0.06 * math.cos(oyaw[i])
+        dy = 0.06 * math.sin(oyaw[i])
+        ax.arrow(ox[i], oy[i], dx, dy, head_width=0.04, head_length=0.04,
+                 fc="orange", ec="orange", alpha=0.5, zorder=6)
+
+    # goal
+    for g in goals:
+        ax.scatter(g["x"], g["y"], c="green", s=120, marker="*", zorder=7,
+                   edgecolors="darkgreen", linewidths=0.5)
+    if goals:
+        ax.scatter([], [], c="green", s=80, marker="*", label="Goal")
+
+    ax.set_xlabel("X (m)")
+    ax.set_ylabel("Y (m)")
+    ax.set_title("Trajectory: Planned vs Actual")
+    ax.legend()
+    ax.axis("equal")
+    ax.grid(True, alpha=0.3)
+    fig.tight_layout()
+    fig.savefig(out_dir / "01_trajectory.png", dpi=150)
+    plt.close(fig)
+    print("  Saved: 01_trajectory.png")
+
+
+def _plot_cmd_vel(cmd_vel, t0, out_dir):
+    t = np.array([d["t"] for d in cmd_vel]) - t0
+    vx = np.array([d["vx"] for d in cmd_vel])
+    vz = np.array([d["vz"] for d in cmd_vel])
+
+    fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(14, 8), sharex=True)
+
+    ax1.plot(t, vx, "b-", lw=0.7)
+    ax1.set_ylabel("Linear X (m/s)")
+    ax1.set_title("cmd_vel — Linear Velocity")
+    ax1.axhline(y=0, c="gray", ls=":", lw=0.5)
+    ax1.grid(True, alpha=0.3)
+
+    ax2.plot(t, vz, "r-", lw=0.7)
+    ax2.set_xlabel("Time (s)")
+    ax2.set_ylabel("Angular Z (rad/s)")
+    ax2.set_title("cmd_vel — Angular Velocity")
+    ax2.axhline(y=0, c="gray", ls=":", lw=0.5)
+    ax2.grid(True, alpha=0.3)
+
+    fig.tight_layout()
+    fig.savefig(out_dir / "02_cmd_vel.png", dpi=150)
+    plt.close(fig)
+    print("  Saved: 02_cmd_vel.png")
+
+
+def _plot_errors(plans, odom, t0, out_dir):
+    """横向误差 + 航向误差"""
+    ox = np.array([d["x"] for d in odom])
+    oy = np.array([d["y"] for d in odom])
+    oyaw = np.array([d["yaw"] for d in odom])
+    ot = np.array([d["t"] for d in odom]) - t0
+
+    # 取最新的 plan
+    poses = plans[-1]["poses"]
+    if len(poses) < 2:
+        print("  WARNING: plan has < 2 waypoints, skipping error analysis")
+        return
+
+    pts = np.array([(p["x"], p["y"]) for p in poses])
+
+    cte = np.zeros(len(ox))
+    heading_err = np.zeros(len(ox))
+
+    for i in range(len(ox)):
+        # 横向误差
+        diffs = pts[1:] - pts[:-1]
+        seg_sq = np.sum(diffs**2, axis=1)
+        pos = np.array([ox[i], oy[i]])
+        t_proj = np.clip(np.sum((pos - pts[:-1]) * diffs, axis=1) / np.maximum(seg_sq, 1e-9), 0, 1)
+        proj = pts[:-1] + t_proj[:, np.newaxis] * diffs
+        dists = np.linalg.norm(proj - pos, axis=1)
+        cte[i] = np.min(dists)
+
+        # 航向误差
+        idx = np.argmin(dists)
+        target_idx = min(idx + 3, len(poses) - 1)
+        target_yaw = poses[target_idx]["yaw"]
+        err = target_yaw - oyaw[i]
+        while err > math.pi:  err -= 2*math.pi
+        while err < -math.pi: err += 2*math.pi
+        heading_err[i] = err
+
+    fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(14, 8), sharex=True)
+
+    ax1.plot(ot, cte, "purple", lw=0.7)
+    ax1.set_ylabel("Cross-Track Error (m)")
+    ax1.set_title("Cross-Track Error")
+    ax1.axhline(y=0, c="gray", ls=":", lw=0.5)
+    ax1.grid(True, alpha=0.3)
+
+    ax2.plot(ot, np.degrees(heading_err), "orange", lw=0.7)
+    ax2.set_xlabel("Time (s)")
+    ax2.set_ylabel("Heading Error (deg)")
+    ax2.set_title("Heading Error")
+    ax2.axhline(y=0, c="gray", ls=":", lw=0.5)
+    ax2.grid(True, alpha=0.3)
+
+    fig.tight_layout()
+    fig.savefig(out_dir / "03_errors.png", dpi=150)
+    plt.close(fig)
+    print("  Saved: 03_errors.png")
+
+
+def _plot_curvature(plans, out_dir):
+    poses = plans[-1]["poses"]
+    n = len(poses)
+    if n < 3:
+        return
+
+    # 累积路径距离 (沿路径从 pose[0] 起算)
+    cum_dist = [0.0]
+    for i in range(1, n):
+        dx = poses[i]["x"] - poses[i-1]["x"]
+        dy = poses[i]["y"] - poses[i-1]["y"]
+        cum_dist.append(cum_dist[-1] + math.hypot(dx, dy))
+
+    # 曲率 (三点法，每个曲率值对应中间点的路径位置)
+    curvatures = []
+    curv_dist = []
+    for i in range(1, n - 1):
+        x0, y0 = poses[i-1]["x"], poses[i-1]["y"]
+        x1, y1 = poses[i]["x"], poses[i]["y"]
+        x2, y2 = poses[i+1]["x"], poses[i+1]["y"]
+        a = math.hypot(x1-x0, y1-y0)
+        b = math.hypot(x2-x1, y2-y1)
+        c = math.hypot(x2-x0, y2-y0)
+        if a*b*c < 1e-12:
+            curvatures.append(0.0)
+        else:
+            s = (a+b+c)/2.0
+            area = math.sqrt(max(0, s*(s-a)*(s-b)*(s-c)))
+            curvatures.append(4.0*area/(a*b*c))
+        curv_dist.append(cum_dist[i])  # 曲率对应 pose[i] 在路径上的位置
+
+    dist_mid = np.array(curv_dist)
+    curv_arr = np.array(curvatures)
+
+    fig, ax = plt.subplots(figsize=(12, 5))
+    ax.plot(dist_mid, curv_arr, "g-", lw=1.0)
+    ax.set_xlabel("Distance along path (m)")
+    ax.set_ylabel("Curvature (1/m)")
+    ax.set_title("Path Curvature vs Distance")
+    ax.grid(True, alpha=0.3)
+    fig.tight_layout()
+    fig.savefig(out_dir / "04_curvature.png", dpi=150)
+    plt.close(fig)
+    print("  Saved: 04_curvature.png")
+
+
+# ============================================================================
+#  main
+# ============================================================================
+if __name__ == "__main__":
+    if len(sys.argv) < 2:
+        print(__doc__)
+        sys.exit(1)
+
+    cmd = sys.argv[1].lower()
+
+    if cmd == "record":
+        mode_record(sys.argv)
+    elif cmd == "plot":
+        mode_plot(sys.argv)
+    else:
+        print(f"Unknown command: {cmd}")
+        print("Use: record  or  plot <dir>")
+        sys.exit(1)

scripts/publish_sine_path.py

@@ -0,0 +1,123 @@
+#!/usr/bin/env python3
+"""
+publish_sine_path.py — 基于机器人当前位姿发布一次正弦路径到 /plan
+
+用法:
+  python3 publish_sine_path.py [amplitude] [wavelength] [length]
+  默认: amplitude=0.8m  wavelength=3.0m  length=8.0m
+
+路径在机器人前方展开:
+  local_x 沿机器人朝向正前方
+  local_y = A * sin(2pi * local_x / wavelength)
+  再旋转变换到 odom 坐标系
+
+依赖: /odom 话题
+"""
+
+import sys
+import math
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Path, Odometry
+from geometry_msgs.msg import PoseStamped
+
+
+class SinePathPublisher(Node):
+    def __init__(self, amplitude, wavelength, length):
+        super().__init__("sine_path_publisher")
+
+        self.A = amplitude
+        self.wavelength = wavelength
+        self.length = length
+        self.step = 0.05
+
+        self.robot_x = 0.0
+        self.robot_y = 0.0
+        self.robot_yaw = 0.0
+        self.has_odom = False
+
+        self.pub = self.create_publisher(Path, "/plan", 10)
+        self.odom_sub = self.create_subscription(
+            Odometry, "/odom", self.odom_cb, 10)
+
+    def odom_cb(self, msg):
+        self.robot_x = msg.pose.pose.position.x
+        self.robot_y = msg.pose.pose.position.y
+        qx = msg.pose.pose.orientation.x
+        qy = msg.pose.pose.orientation.y
+        qz = msg.pose.pose.orientation.z
+        qw = msg.pose.pose.orientation.w
+        self.robot_yaw = math.atan2(
+            2.0 * (qw * qz + qx * qy),
+            1.0 - 2.0 * (qy * qy + qz * qz))
+        self.has_odom = True
+
+    def build_and_publish(self):
+        cos_yaw = math.cos(self.robot_yaw)
+        sin_yaw = math.sin(self.robot_yaw)
+
+        path = Path()
+        path.header.frame_id = "odom"
+        path.header.stamp = self.get_clock().now().to_msg()
+
+        lx = 0.0
+        while lx <= self.length:
+            ly = self.A * math.sin(2.0 * math.pi * lx / self.wavelength)
+
+            dy_dx = self.A * (2.0 * math.pi / self.wavelength) * \
+                math.cos(2.0 * math.pi * lx / self.wavelength)
+            local_yaw = math.atan2(dy_dx, 1.0)
+
+            wx = self.robot_x + lx * cos_yaw - ly * sin_yaw
+            wy = self.robot_y + lx * sin_yaw + ly * cos_yaw
+            world_yaw = self.robot_yaw + local_yaw
+
+            pose = PoseStamped()
+            pose.header.frame_id = "odom"
+            pose.pose.position.x = wx
+            pose.pose.position.y = wy
+            pose.pose.position.z = 0.0
+            pose.pose.orientation.z = math.sin(world_yaw / 2.0)
+            pose.pose.orientation.w = math.cos(world_yaw / 2.0)
+            path.poses.append(pose)
+
+            lx += self.step
+
+        self.pub.publish(path)
+        self.get_logger().info(
+            f"Published sine path: {len(path.poses)} waypoints "
+            f"from ({self.robot_x:.2f}, {self.robot_y:.2f}, {math.degrees(self.robot_yaw):.0f}°)"
+        )
+
+
+def main():
+    rclpy.init()
+
+    A = float(sys.argv[1]) if len(sys.argv) > 1 else 0.8
+    wavelength = float(sys.argv[2]) if len(sys.argv) > 2 else 3.0
+    length = float(sys.argv[3]) if len(sys.argv) > 3 else 8.0
+
+    node = SinePathPublisher(A, wavelength, length)
+
+    # 等待 odom (最多 3 秒)
+    timeout = node.get_clock().now() + rclpy.duration.Duration(seconds=3)
+    while not node.has_odom and node.get_clock().now() < timeout:
+        rclpy.spin_once(node, timeout_sec=0.05)
+
+    if not node.has_odom:
+        node.get_logger().error("No /odom received within 3s — aborting")
+        node.destroy_node()
+        rclpy.shutdown()
+        sys.exit(1)
+
+    # 额外 spin 一下让其他订阅就绪
+    rclpy.spin_once(node, timeout_sec=0.5)
+
+    node.build_and_publish()
+
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml

@@ -1,7 +1,6 @@
 slam_toolbox:
   ros__parameters:
     use_sim_time: False
-
 bt_navigator:
   ros__parameters:
     use_sim_time: False
@@ -10,165 +9,111 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 50
     default_server_timeout: 20
-    # 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
     default_nav_to_pose_bt_xml: /home/sunrise/yiliao_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
     plugin_lib_names:
-      - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_to_pose_action_bt_node
-      - nav2_compute_path_through_poses_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
-      - nav2_smooth_path_action_bt_node
+    - nav2_smooth_path_action_bt_node
-      - nav2_follow_path_action_bt_node
+    - nav2_follow_path_action_bt_node
-      - nav2_spin_action_bt_node
+    - nav2_spin_action_bt_node
-      - nav2_wait_action_bt_node
+    - nav2_wait_action_bt_node
-      - nav2_back_up_action_bt_node
+    - nav2_back_up_action_bt_node
-      - nav2_drive_on_heading_bt_node
+    - nav2_drive_on_heading_bt_node
-      - nav2_clear_costmap_service_bt_node
+    - nav2_clear_costmap_service_bt_node
-      - nav2_is_stuck_condition_bt_node
+    - nav2_is_stuck_condition_bt_node
-      - nav2_goal_reached_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
-      - nav2_goal_updated_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
-      - nav2_globally_updated_goal_condition_bt_node
+    - nav2_globally_updated_goal_condition_bt_node
-      - nav2_is_path_valid_condition_bt_node
+    - nav2_is_path_valid_condition_bt_node
-      - nav2_initial_pose_received_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
-      - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
-      - nav2_rate_controller_bt_node
+    - nav2_rate_controller_bt_node
-      - nav2_distance_controller_bt_node
+    - nav2_distance_controller_bt_node
-      - nav2_speed_controller_bt_node
+    - nav2_speed_controller_bt_node
-      - nav2_truncate_path_action_bt_node
+    - nav2_truncate_path_action_bt_node
-      - nav2_truncate_path_local_action_bt_node
+    - nav2_truncate_path_local_action_bt_node
-      - nav2_goal_updater_node_bt_node
+    - nav2_goal_updater_node_bt_node
-      - nav2_recovery_node_bt_node
+    - nav2_recovery_node_bt_node
-      - nav2_pipeline_sequence_bt_node
+    - nav2_pipeline_sequence_bt_node
-      - nav2_round_robin_node_bt_node
+    - nav2_round_robin_node_bt_node
-      - nav2_transform_available_condition_bt_node
+    - nav2_transform_available_condition_bt_node
-      - nav2_time_expired_condition_bt_node
+    - nav2_time_expired_condition_bt_node
-      - nav2_path_expiring_timer_condition
+    - nav2_path_expiring_timer_condition
-      - nav2_distance_traveled_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
-      - nav2_single_trigger_bt_node
+    - nav2_single_trigger_bt_node
-      - nav2_is_battery_low_condition_bt_node
+    - nav2_is_battery_low_condition_bt_node
-      - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_through_poses_action_bt_node
-      - nav2_navigate_to_pose_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
-      - nav2_remove_passed_goals_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
-      - nav2_planner_selector_bt_node
+    - nav2_planner_selector_bt_node
-      - nav2_controller_selector_bt_node
+    - nav2_controller_selector_bt_node
-      - nav2_goal_checker_selector_bt_node
+    - nav2_goal_checker_selector_bt_node
-      - nav2_controller_cancel_bt_node
+    - nav2_controller_cancel_bt_node
-      - nav2_path_longer_on_approach_bt_node
+    - nav2_path_longer_on_approach_bt_node
-      - nav2_wait_cancel_bt_node
+    - nav2_wait_cancel_bt_node
-      - nav2_spin_cancel_bt_node
+    - nav2_spin_cancel_bt_node
-      - nav2_back_up_cancel_bt_node
+    - nav2_back_up_cancel_bt_node
-      - nav2_drive_on_heading_cancel_bt_node
+    - nav2_drive_on_heading_cancel_bt_node
-
 bt_navigator_rclcpp_node:
   ros__parameters:
     use_sim_time: False
-
 controller_server:
   ros__parameters:
     use_sim_time: False
     controller_frequency: 20.0
     FollowPath:
-      plugin: "nav2_mppi_controller::MPPIController"
+      plugin: nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
-      time_steps: 20
+      desired_linear_vel: 0.3
-      model_dt: 0.05
+      max_angular_vel: 1.9
-      batch_size: 200
+      min_angular_vel: 0.2
-      vx_std: 0.2
+      lookahead_dist: 0.5
-      vy_std: 0.0
+      min_lookahead_dist: 0.3
-      wz_std: 0.4
+      max_lookahead_dist: 0.8
-      vx_max: 0.5
+      lookahead_time: 1.5
-      vx_min: -0.35
+      rotate_to_heading_angular_vel: 0.8
-      vy_max: 0.0
+      transform_tolerance: 0.5
-      wz_max: 1.9
+      use_velocity_scaled_lookahead_dist: True
-      iteration_count: 1
+      min_approach_linear_vel: 0.1
-      temperature: 0.3
+      approach_velocity_scaling_dist: 0.5
-      gamma: 0.015
+      use_collision_detection: True
-      motion_model: "Ackermann"
+      max_allowed_time_to_collision: 1.0
-      visualize: false
+      use_cost_regulated_linear_velocity_scaling: True
-      TrajectoryVisualizer:
+      cost_scaling_dist: 0.6
-        trajectory_step: 5
+      cost_scaling_gain: 1.0
-        time_step: 3
+      regulated_linear_scaling_min_radius: 0.35
-      AckermannConstraints:
+      regulated_linear_scaling_min_speed: 0.1
-        min_turning_r: 0.4
+      use_rotate_to_heading: True
-      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      rotate_to_heading_min_angle: 0.5
-      ConstraintCritic:
+      max_robot_pose_search_dist: 10.0
-        enabled: true
+      use_interpolation: False
-        cost_power: 1
+      allow_reversing: False
-        cost_weight: 4.0
-      GoalCritic:
-        enabled: true
-        cost_power: 1
-        cost_weight: 5.0
-        threshold_to_consider: 1.4
-      GoalAngleCritic:
-        enabled: true
-        cost_power: 1
-        cost_weight: 3.0
-        threshold_to_consider: 0.5
-      PreferForwardCritic:
-        enabled: true
-        cost_power: 1
-        cost_weight: 2.0
-        threshold_to_consider: 0.5
-      CostCritic:
-        enabled: true
-        cost_power: 1
-        cost_weight: 3.81
-        critical_cost: 300.0
-        consider_footprint: true
-        collision_cost: 1000000.0
-        near_goal_distance: 1.0
-        trajectory_point_step: 2
-      PathAlignCritic:
-        enabled: true
-        cost_power: 1
-        cost_weight: 7.0  # PathAlignCritic
-        max_path_occupancy_ratio: 0.05
-        trajectory_point_step: 4
-        threshold_to_consider: 0.5
-        offset_from_furthest: 20
-        use_path_orientations: false
-      PathFollowCritic:
-        enabled: true
-        cost_power: 1
-        cost_weight: 5.0
-        offset_from_furthest: 5
-        threshold_to_consider: 1.4
-      PathAngleCritic:
-        enabled: true
-        cost_power: 1
-        cost_weight: 2.0
-        offset_from_furthest: 4
-        threshold_to_consider: 0.5
-        max_angle_to_furthest: 1.0
-        forward_preference: true
-
 controller_server_rclcpp_node:
   ros__parameters:
     use_sim_time: False
-
 local_costmap:
   local_costmap:
     ros__parameters:
       update_frequency: 1.0
       publish_frequency: 2.0
-      transform_tolerance: 0.5
+      transform_tolerance: 2.0
       global_frame: odom
       robot_base_frame: base_link
       use_sim_time: False
-      rolling_window: true
+      rolling_window: True
       width: 3
       height: 3
       resolution: 0.05
-      footprint: "[[0.14, 0.085],
+      footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
-        [0.14, -0.085],
-        [-0.14, -0.085],
-        [-0.14, 0.085]]"
       footprint_padding: 0.02
-      plugins: ["voxel_layer", "inflation_layer"]
+      plugins:
+      - voxel_layer
+      - inflation_layer
       inflation_layer:
-        plugin: "nav2_costmap_2d::InflationLayer"
+        plugin: nav2_costmap_2d::InflationLayer
         cost_scaling_factor: 3.0
-        inflation_radius: 0.2
+        inflation_radius: 0.3
       voxel_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
+        plugin: nav2_costmap_2d::VoxelLayer
         enabled: True
         publish_voxel_map: True
         origin_z: 0.0
@@ -182,7 +127,7 @@ local_costmap:
           max_obstacle_height: 2.0
           clearing: True
           marking: True
-          data_type: "LaserScan"
+          data_type: LaserScan
           raytrace_max_range: 3.0
           raytrace_min_range: 0.0
           obstacle_max_range: 2.5
@@ -196,27 +141,25 @@ local_costmap:
   local_costmap_rclcpp_node:
     ros__parameters:
       use_sim_time: False
-
 global_costmap:
   global_costmap:
     ros__parameters:
       use_sim_time: False
-      transform_tolerance: 0.5
+      transform_tolerance: 2.0
       update_frequency: 1.0
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_link
-      use_sim_time: False
+      footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
-      footprint: "[[0.14, 0.085],
-        [0.14, -0.085],
-        [-0.14, -0.085],
-        [-0.14, 0.085]]"
       footprint_padding: 0.02
       resolution: 0.05
-      track_unknown_space: true
+      track_unknown_space: True
-      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      plugins:
+      - static_layer
+      - obstacle_layer
+      - inflation_layer
       obstacle_layer:
-        plugin: "nav2_costmap_2d::ObstacleLayer"
+        plugin: nav2_costmap_2d::ObstacleLayer
         enabled: True
         observation_sources: scan
         scan:
@@ -224,18 +167,18 @@ global_costmap:
           max_obstacle_height: 2.0
           clearing: True
           marking: True
-          data_type: "LaserScan"
+          data_type: LaserScan
           raytrace_max_range: 3.0
           raytrace_min_range: 0.0
           obstacle_max_range: 2.5
           obstacle_min_range: 0.0
       static_layer:
-        plugin: "nav2_costmap_2d::StaticLayer"
+        plugin: nav2_costmap_2d::StaticLayer
         map_subscribe_transient_local: True
       inflation_layer:
-        plugin: "nav2_costmap_2d::InflationLayer"
+        plugin: nav2_costmap_2d::InflationLayer
         cost_scaling_factor: 3.0
-        inflation_radius: 0.2
+        inflation_radius: 0.3
       always_send_full_costmap: True
   global_costmap_client:
     ros__parameters:
@@ -243,7 +186,6 @@ global_costmap:
   global_costmap_rclcpp_node:
     ros__parameters:
       use_sim_time: False
-
 map_saver:
   ros__parameters:
     use_sim_time: False
@@ -251,93 +193,90 @@ map_saver:
     free_thresh_default: 0.25
     occupied_thresh_default: 0.65
     map_subscribe_transient_local: True
-
 planner_server:
   ros__parameters:
-    planner_plugins: ["GridBased"]
+    planner_plugins:
+    - GridBased
     use_sim_time: False
-
     GridBased:
-      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      plugin: nav2_smac_planner/SmacPlannerHybrid
-      downsample_costmap: false
+      downsample_costmap: False
       downsampling_factor: 1
       tolerance: 0.25
-      allow_unknown: true
+      allow_unknown: True
       max_iterations: 100000
       max_on_approach_iterations: 1000
       max_planning_time: 5.0
-      motion_model_for_search: "REEDS_SHEPP"
+      motion_model_for_search: REEDS_SHEPP
       angle_quantization_bins: 72
-      analytic_expansion_ratio: 3.5
+      analytic_expansion_ratio: 2.0
       analytic_expansion_max_length: 3.0
-      minimum_turning_radius: 0.40
+      minimum_turning_radius: 0.35
-      reverse_penalty: 1.3     # 后退惩罚，越小越愿意后退（默认 2.0，设为 1.3 允许灵活倒车）
+      reverse_penalty: 1.3
       change_penalty: 0.0
-      non_straight_penalty: 0.5
+      non_straight_penalty: 0.0
       cost_penalty: 5.0
       retrospective_penalty: 0.015
       lookup_table_size: 5.0
-      cache_obstacle_heuristic: false
+      cache_obstacle_heuristic: False
-      viz_expansions: false
+      viz_expansions: False
       smooth_path: True
-
       smoother:
         max_iterations: 1000
-        w_smooth: 0.3
+        w_smooth: 0.4
         w_data: 0.2
         tolerance: 1.0e-10
-        do_refinement: true
+        do_refinement: True
-        refinement_num: 2
+        refinement_num: 4
-
 planner_server_rclcpp_node:
   ros__parameters:
     use_sim_time: False
-
 smoother_server:
   ros__parameters:
     use_sim_time: False
-    smoother_plugins: ["simple_smoother"]
+    smoother_plugins:
+    - simple_smoother
     simple_smoother:
-      plugin: "nav2_smoother::SimpleSmoother"
+      plugin: nav2_smoother::SimpleSmoother
       tolerance: 1.0e-10
       max_its: 1000
       do_refinement: True
-
 behavior_server:
   ros__parameters:
     costmap_topic: local_costmap/costmap_raw
     footprint_topic: local_costmap/published_footprint
     cycle_frequency: 10.0
-    behavior_plugins: ["spin", "backup", "wait"]
+    behavior_plugins:
+    - spin
+    - backup
+    - wait
     spin:
-      plugin: "nav2_behaviors/Spin"    # 需保留以匹配默认 BT XML
+      plugin: nav2_behaviors/Spin
     backup:
-      plugin: "nav2_behaviors/BackUp"
+      plugin: nav2_behaviors/BackUp
       backup_dist: 0.8
       backup_speed: 0.18
     wait:
-      plugin: "nav2_behaviors/Wait"
+      plugin: nav2_behaviors/Wait
       wait_duration: 0.5
     global_frame: odom
     robot_base_frame: base_link
-    transform_tolerance: 0.5
+    transform_tolerance: 2.0
     use_sim_time: False
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0
     min_rotational_vel: 0.4
     rotational_acc_lim: 3.2
-
 robot_state_publisher:
   ros__parameters:
     use_sim_time: False
-
 waypoint_follower:
   ros__parameters:
     loop_rate: 20
     use_sim_time: False
-    stop_on_failure: false
+    stop_on_failure: False
-    waypoint_task_executor_plugin: "wait_at_waypoint"
+    waypoint_task_executor_plugin: wait_at_waypoint
     wait_at_waypoint:
-      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      plugin: nav2_waypoint_follower::WaitAtWaypoint
       enabled: True
       waypoint_pause_duration: 200

src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak

@@ -1,17 +1,17 @@
 slam_toolbox:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 bt_navigator:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     global_frame: map
     robot_base_frame: base_footprint
     odom_topic: /odom
     bt_loop_duration: 50
     default_server_timeout: 20
     # 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
-    default_nav_to_pose_bt_xml: /home/guoch/test_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
+    default_nav_to_pose_bt_xml: /home/sunrise/yiliao_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
     plugin_lib_names:
       - nav2_compute_path_to_pose_action_bt_node
       - nav2_compute_path_through_poses_action_bt_node
@@ -59,17 +59,17 @@ bt_navigator:
 
 bt_navigator_rclcpp_node:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 controller_server:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     controller_frequency: 20.0
     FollowPath:
       plugin: "nav2_mppi_controller::MPPIController"
-      time_steps: 36
+      time_steps: 20
       model_dt: 0.05
-      batch_size: 1000
+      batch_size: 200
       vx_std: 0.2
       vy_std: 0.0
       wz_std: 0.4
@@ -105,7 +105,7 @@ controller_server:
       PreferForwardCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 5.0
+        cost_weight: 2.0
         threshold_to_consider: 0.5
       CostCritic:
         enabled: true
@@ -119,7 +119,7 @@ controller_server:
       PathAlignCritic:
         enabled: true
         cost_power: 1
-        cost_weight: 14.0
+        cost_weight: 7.0  # PathAlignCritic
         max_path_occupancy_ratio: 0.05
         trajectory_point_step: 4
         threshold_to_consider: 0.5
@@ -142,17 +142,17 @@ controller_server:
 
 controller_server_rclcpp_node:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 local_costmap:
   local_costmap:
     ros__parameters:
-      update_frequency: 5.0
+      update_frequency: 1.0
       publish_frequency: 2.0
       transform_tolerance: 0.5
       global_frame: odom
       robot_base_frame: base_link
-      use_sim_time: True
+      use_sim_time: False
       rolling_window: true
       width: 3
       height: 3
@@ -166,7 +166,7 @@ local_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.55
+        inflation_radius: 0.2
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -192,21 +192,21 @@ local_costmap:
       always_send_full_costmap: True
   local_costmap_client:
     ros__parameters:
-      use_sim_time: True
+      use_sim_time: False
   local_costmap_rclcpp_node:
     ros__parameters:
-      use_sim_time: True
+      use_sim_time: False
 
 global_costmap:
   global_costmap:
     ros__parameters:
-      use_sim_time: True
+      use_sim_time: False
       transform_tolerance: 0.5
       update_frequency: 1.0
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_link
-      use_sim_time: True
+      use_sim_time: False
       footprint: "[[0.14, 0.085],
         [0.14, -0.085],
         [-0.14, -0.085],
@@ -235,18 +235,18 @@ global_costmap:
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
         cost_scaling_factor: 3.0
-        inflation_radius: 0.55
+        inflation_radius: 0.2
       always_send_full_costmap: True
   global_costmap_client:
     ros__parameters:
-      use_sim_time: True
+      use_sim_time: False
   global_costmap_rclcpp_node:
     ros__parameters:
-      use_sim_time: True
+      use_sim_time: False
 
 map_saver:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     save_map_timeout: 5.0
     free_thresh_default: 0.25
     occupied_thresh_default: 0.65
@@ -255,7 +255,7 @@ map_saver:
 planner_server:
   ros__parameters:
     planner_plugins: ["GridBased"]
-    use_sim_time: True
+    use_sim_time: False
 
     GridBased:
       plugin: "nav2_smac_planner/SmacPlannerHybrid"
@@ -263,7 +263,7 @@ planner_server:
       downsampling_factor: 1
       tolerance: 0.25
       allow_unknown: true
-      max_iterations: 1000000
+      max_iterations: 100000
       max_on_approach_iterations: 1000
       max_planning_time: 5.0
       motion_model_for_search: "REEDS_SHEPP"
@@ -273,10 +273,10 @@ planner_server:
       minimum_turning_radius: 0.40
       reverse_penalty: 1.3     # 后退惩罚，越小越愿意后退（默认 2.0，设为 1.3 允许灵活倒车）
       change_penalty: 0.0
-      non_straight_penalty: 1.2
+      non_straight_penalty: 0.5
-      cost_penalty: 2.0
+      cost_penalty: 5.0
       retrospective_penalty: 0.015
-      lookup_table_size: 20.0
+      lookup_table_size: 5.0
       cache_obstacle_heuristic: false
       viz_expansions: false
       smooth_path: True
@@ -291,11 +291,11 @@ planner_server:
 
 planner_server_rclcpp_node:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 smoother_server:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
     smoother_plugins: ["simple_smoother"]
     simple_smoother:
       plugin: "nav2_smoother::SimpleSmoother"
@@ -321,7 +321,7 @@ behavior_server:
     global_frame: odom
     robot_base_frame: base_link
     transform_tolerance: 0.5
-    use_sim_time: True
+    use_sim_time: False
     simulate_ahead_time: 2.0
     max_rotational_vel: 1.0
     min_rotational_vel: 0.4
@@ -329,12 +329,12 @@ behavior_server:
 
 robot_state_publisher:
   ros__parameters:
-    use_sim_time: True
+    use_sim_time: False
 
 waypoint_follower:
   ros__parameters:
     loop_rate: 20
-    use_sim_time: True
+    use_sim_time: False
     stop_on_failure: false
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:

src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak2

@@ -0,0 +1,343 @@
+slam_toolbox:
+  ros__parameters:
+    use_sim_time: False
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_footprint
+    odom_topic: /odom
+    bt_loop_duration: 50
+    default_server_timeout: 20
+    # 阿克曼底盘专用 BT：移除 Spin，恢复行为 = 清代价地图 → 后退 → 等待
+    default_nav_to_pose_bt_xml: /home/sunrise/yiliao_ws/install/gc_navigation2_slamtoolbox/share/gc_navigation2_slamtoolbox/config/nav_to_pose_ackermann.xml
+    plugin_lib_names:
+      - nav2_compute_path_to_pose_action_bt_node
+      - nav2_compute_path_through_poses_action_bt_node
+      - nav2_smooth_path_action_bt_node
+      - nav2_follow_path_action_bt_node
+      - nav2_spin_action_bt_node
+      - nav2_wait_action_bt_node
+      - nav2_back_up_action_bt_node
+      - nav2_drive_on_heading_bt_node
+      - nav2_clear_costmap_service_bt_node
+      - nav2_is_stuck_condition_bt_node
+      - nav2_goal_reached_condition_bt_node
+      - nav2_goal_updated_condition_bt_node
+      - nav2_globally_updated_goal_condition_bt_node
+      - nav2_is_path_valid_condition_bt_node
+      - nav2_initial_pose_received_condition_bt_node
+      - nav2_reinitialize_global_localization_service_bt_node
+      - nav2_rate_controller_bt_node
+      - nav2_distance_controller_bt_node
+      - nav2_speed_controller_bt_node
+      - nav2_truncate_path_action_bt_node
+      - nav2_truncate_path_local_action_bt_node
+      - nav2_goal_updater_node_bt_node
+      - nav2_recovery_node_bt_node
+      - nav2_pipeline_sequence_bt_node
+      - nav2_round_robin_node_bt_node
+      - nav2_transform_available_condition_bt_node
+      - nav2_time_expired_condition_bt_node
+      - nav2_path_expiring_timer_condition
+      - nav2_distance_traveled_condition_bt_node
+      - nav2_single_trigger_bt_node
+      - nav2_is_battery_low_condition_bt_node
+      - nav2_navigate_through_poses_action_bt_node
+      - nav2_navigate_to_pose_action_bt_node
+      - nav2_remove_passed_goals_action_bt_node
+      - nav2_planner_selector_bt_node
+      - nav2_controller_selector_bt_node
+      - nav2_goal_checker_selector_bt_node
+      - nav2_controller_cancel_bt_node
+      - nav2_path_longer_on_approach_bt_node
+      - nav2_wait_cancel_bt_node
+      - nav2_spin_cancel_bt_node
+      - nav2_back_up_cancel_bt_node
+      - nav2_drive_on_heading_cancel_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    controller_frequency: 20.0
+    FollowPath:
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 20
+      model_dt: 0.05
+      batch_size: 200
+      vx_std: 0.2
+      vy_std: 0.0
+      wz_std: 0.8
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.0
+      wz_max: 1.9
+      iteration_count: 1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "Ackermann"
+      visualize: false
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.4
+      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 2.0
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 8.0
+        critical_cost: 300.0
+        consider_footprint: true
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0  # PathAlignCritic
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 10
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 1.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        forward_preference: true
+
+controller_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 2.0
+      transform_tolerance: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: "[[0.14, 0.085],
+        [0.14, -0.085],
+        [-0.14, -0.085],
+        [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      plugins: ["voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.3
+      voxel_layer:
+        plugin: "nav2_costmap_2d::VoxelLayer"
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: False
+      transform_tolerance: 2.0
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: False
+      footprint: "[[0.14, 0.085],
+        [0.14, -0.085],
+        [-0.14, -0.085],
+        [-0.14, 0.085]]"
+      footprint_padding: 0.02
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: "LaserScan"
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.3
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+map_saver:
+  ros__parameters:
+    use_sim_time: False
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    planner_plugins: ["GridBased"]
+    use_sim_time: False
+
+    GridBased:
+      plugin: "nav2_smac_planner/SmacPlannerHybrid"
+      downsample_costmap: false
+      downsampling_factor: 1
+      tolerance: 0.25
+      allow_unknown: true
+      max_iterations: 100000
+      max_on_approach_iterations: 1000
+      max_planning_time: 5.0
+      motion_model_for_search: "REEDS_SHEPP"
+      angle_quantization_bins: 72
+      analytic_expansion_ratio: 2.0
+      analytic_expansion_max_length: 3.0
+      minimum_turning_radius: 0.40
+      reverse_penalty: 1.3     # 后退惩罚，越小越愿意后退（默认 2.0，设为 1.3 允许灵活倒车）
+      change_penalty: 0.0
+      non_straight_penalty: 0.0
+      cost_penalty: 5.0
+      retrospective_penalty: 0.015
+      lookup_table_size: 5.0
+      cache_obstacle_heuristic: false
+      viz_expansions: false
+      smooth_path: True
+
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.4
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: true
+        refinement_num: 4
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+smoother_server:
+  ros__parameters:
+    use_sim_time: False
+    smoother_plugins: ["simple_smoother"]
+    simple_smoother:
+      plugin: "nav2_smoother::SimpleSmoother"
+      tolerance: 1.0e-10
+      max_its: 1000
+      do_refinement: True
+
+behavior_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    behavior_plugins: ["spin", "backup", "wait"]
+    spin:
+      plugin: "nav2_behaviors/Spin"    # 需保留以匹配默认 BT XML
+    backup:
+      plugin: "nav2_behaviors/BackUp"
+      backup_dist: 0.8
+      backup_speed: 0.18
+    wait:
+      plugin: "nav2_behaviors/Wait"
+      wait_duration: 0.5
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_tolerance: 2.0
+    use_sim_time: False
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 20
+    use_sim_time: False
+    stop_on_failure: false
+    waypoint_task_executor_plugin: "wait_at_waypoint"
+    wait_at_waypoint:
+      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+      enabled: True
+      waypoint_pause_duration: 200

src/origincar_base/config/ekf.yaml

@@ -20,7 +20,7 @@ ekf_filter_node:
         odom0_config: [true, false, false,
                        false, false, false,
                        true,  true,  false,
-                       false, false, true,
+                       false, false, false,
                        false, false, false]
         odom0_queue_size: 10
         odom0_nodelay: false

src/origincar_base/config/ekf.yaml.bak

@@ -0,0 +1,48 @@
+### ekf config file ###
+ekf_filter_node:
+    ros__parameters:
+        frequency: 20.0
+        sensor_timeout: 2.0
+        two_d_mode: true
+        transform_time_offset: 0.0
+        transform_timeout: 0.2
+        print_diagnostics: false
+        debug: false
+        publish_tf: true
+        publish_acceleration: false
+
+        map_frame: map
+        odom_frame: odom
+        base_link_frame: base_footprint
+        world_frame: odom
+
+        odom0: odom
+        odom0_config: [true, false, false,
+                       false, false, false,
+                       true,  true,  false,
+                       false, false, true,
+                       false, false, false]
+        odom0_queue_size: 10
+        odom0_nodelay: false
+        odom0_differential: true
+        odom0_relative: false
+
+        imu0: /imu/data_raw
+        imu0_config: [false, false, false,
+                      false, false, true,
+                      false, false, false,
+                      false, false, true,
+                      false, false, false]
+        imu0_nodelay: false
+        imu0_differential: false
+        imu0_relative: true
+        imu0_queue_size: 10
+        imu0_remove_gravitational_acceleration: true
+
+        use_control: false
+        stamped_control: false
+        control_timeout: 0.2
+        control_config: [true, false, false, false, false, true]
+        acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
+        deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
+        acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]

src/origincar_base/launch/base_serial.launch.py

@@ -1,51 +1,24 @@
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
-from launch.conditions import IfCondition, UnlessCondition
 import launch_ros.actions
 
 def generate_launch_description():
-    akmcar = LaunchConfiguration('akmcar', default='false')
-
     robot_parameters = [
         {'usart_port_name': '/dev/ttyACM0',
          'serial_baud_rate': 115200,
          'robot_frame_id': 'base_link',
          'odom_frame_id': 'odom',
          'cmd_vel': 'cmd_vel',
+         'akm_cmd_vel': 'none',
          'product_number': 0,
-         # Odom covariance (higher = more uncertainty, EKF trusts odom less)
          'odom_pose_cov_x': 0.01,
          'odom_pose_cov_y': 0.01,
          'odom_pose_cov_yaw': 0.0225}
     ]
 
     return LaunchDescription([
-        DeclareLaunchArgument(
-            'akmcar',
-            default_value='false',
-            description='Use simulation (Gazebo) clock if true'
-        ),
-
         launch_ros.actions.Node(
-            condition=IfCondition(akmcar),
             package='origincar_base',
             executable='origincar_base_node',
-            parameters=robot_parameters + [{'akm_cmd_vel': 'ackermann_cmd'}],
+            parameters=robot_parameters,
-            remappings=[('/cmd_vel', 'cmd_vel')],
-        ),
-
-        launch_ros.actions.Node(
-            condition=IfCondition(akmcar),
-            package='origincar_base',
-            executable='cmd_vel_to_ackermann_drive.py',
-            name='cmd_vel_to_ackermann_drive',
-        ),
-
-        launch_ros.actions.Node(
-            condition=UnlessCondition(akmcar),
-            package='origincar_base',
-            executable='origincar_base_node',
-            parameters=robot_parameters + [{'akm_cmd_vel': 'none'}],
         )
     ])

src/origincar_base/launch/base_serial.launch.py.bak

@@ -0,0 +1,51 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
+import launch_ros.actions
+
+def generate_launch_description():
+    akmcar = LaunchConfiguration('akmcar', default='false')
+
+    robot_parameters = [
+        {'usart_port_name': '/dev/ttyACM0',
+         'serial_baud_rate': 115200,
+         'robot_frame_id': 'base_link',
+         'odom_frame_id': 'odom',
+         'cmd_vel': 'cmd_vel',
+         'product_number': 0,
+         # Odom covariance (higher = more uncertainty, EKF trusts odom less)
+         'odom_pose_cov_x': 0.01,
+         'odom_pose_cov_y': 0.01,
+         'odom_pose_cov_yaw': 0.0225}
+    ]
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'akmcar',
+            default_value='false',
+            description='Use simulation (Gazebo) clock if true'
+        ),
+
+        launch_ros.actions.Node(
+            condition=IfCondition(akmcar),
+            package='origincar_base',
+            executable='origincar_base_node',
+            parameters=robot_parameters + [{'akm_cmd_vel': 'ackermann_cmd'}],
+            remappings=[('/cmd_vel', 'cmd_vel')],
+        ),
+
+        launch_ros.actions.Node(
+            condition=IfCondition(akmcar),
+            package='origincar_base',
+            executable='cmd_vel_to_ackermann_drive.py',
+            name='cmd_vel_to_ackermann_drive',
+        ),
+
+        launch_ros.actions.Node(
+            condition=UnlessCondition(akmcar),
+            package='origincar_base',
+            executable='origincar_base_node',
+            parameters=robot_parameters + [{'akm_cmd_vel': 'none'}],
+        )
+    ])

src/origincar_base/launch/origincar_bringup.launch.py

@@ -23,7 +23,7 @@ def generate_launch_description():
     carto_slam = LaunchConfiguration('carto_slam', default='false')
     carto_slam_dec = DeclareLaunchArgument('carto_slam',default_value='false')
 
-    akmcar = LaunchConfiguration('akmcar', default='true')
+    akmcar = LaunchConfiguration('akmcar', default='false')
     akmcar_dec = DeclareLaunchArgument('akmcar', default_value='true',
         description='阿克曼底盘模式 (true=阿克曼, false=差速)')
             

src/past_control/CMakeLists.txt

@@ -0,0 +1,85 @@
+cmake_minimum_required(VERSION 3.5)
+project(past_control)
+
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(origincar_msg REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+
+# generate custom messages
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Obstacle.msg"
+  "msg/ObstacleArray.msg"
+  DEPENDENCIES std_msgs
+  ADD_LINTER_TESTS
+)
+
+include_directories(include)
+
+# lane_follower_node
+add_executable(lane_follower_node
+  src/lane_follower_node.cpp
+)
+ament_target_dependencies(lane_follower_node
+  rclcpp std_msgs geometry_msgs
+)
+
+# obstacle_detector_node
+add_executable(obstacle_detector_node
+  src/obstacle_detector_node.cpp
+)
+ament_target_dependencies(obstacle_detector_node
+  rclcpp std_msgs geometry_msgs nav_msgs tf2 tf2_ros visualization_msgs
+)
+rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} rosidl_typesupport_cpp)
+target_link_libraries(obstacle_detector_node ${cpp_typesupport_target})
+
+# racing_orchestrator — subscribes to /plan (Nav2 planner_server) + PID following
+add_executable(racing_orchestrator
+  src/racing_orchestrator.cpp
+)
+ament_target_dependencies(racing_orchestrator
+  rclcpp rclcpp_action std_msgs nav_msgs nav2_msgs geometry_msgs tf2 tf2_ros origincar_msg visualization_msgs
+)
+
+install(TARGETS
+  lane_follower_node
+  obstacle_detector_node
+  racing_orchestrator
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/past_control/config/past_control.yaml

@@ -0,0 +1,51 @@
+# past_control — Racing control stack configuration
+# No a_star_planner_node: global planning handled by Nav2 planner_server via /plan
+
+# ============================================================
+#  lane_follower_node — Visual lane centering
+# ============================================================
+lane_follower_node:
+  ros__parameters:
+    follow_linear_speed: 0.3
+    follow_angular_ratio: 1.0
+    image_width: 640.0
+
+# ============================================================
+#  obstacle_detector_node — DNN-based obstacle perception
+# ============================================================
+obstacle_detector_node:
+  ros__parameters:
+    confidence_threshold: 0.5
+    processing_latency: 0.1
+    camera_hfov: 1.0472       # 60 degrees
+    camera_height: 0.3
+    camera_pitch: 0.0
+    max_detection_range: 3.0
+    publish_rate: 10.0
+
+# ============================================================
+#  racing_orchestrator — Task FSM + PID path following (Nav2 /plan) + arbitration
+# ============================================================
+racing_orchestrator:
+  ros__parameters:
+    # PID path following (path source: Nav2 planner_server /plan)
+    follow_linear_speed: 0.4
+    guide_step: 5
+    arrive_square: 0.25       # distance^2 threshold (0.5m)
+    angular_kp: 10.0
+    angular_ki: 0.0
+    angular_kd: 0.1
+    angular_integral_max: 1.0
+    angular_output_max: 2.0
+    angular_max_err: 3.14
+
+    # Obstacle avoidance fallback
+    avoid_linear_speed: 0.2
+    avoid_angular_z: 0.8
+
+    # QR scan mode
+    qr_scan_speed: 0.15
+    qr_scan_duration: 3.0
+
+    # Control loop rate
+    control_rate: 20.0

src/past_control/include/past_control/tools.h

@@ -0,0 +1,87 @@
+#ifndef PAST_CONTROL__TOOLS_H
+#define PAST_CONTROL__TOOLS_H
+
+#include <algorithm>
+#include <chrono>
+
+namespace past_control
+{
+
+inline double limit(double val, double min_val, double max_val)
+{
+  return std::min(std::max(val, min_val), max_val);
+}
+
+class PID
+{
+public:
+  PID()
+  : kp_(0.0), ki_(0.0), kd_(0.0),
+    integral_max_(0.0), output_max_(0.0),
+    integral_(0.0), prev_error_(0.0), first_run_(true),
+    max_err_(0.0)
+  {
+  }
+
+  void init(double kp, double ki, double kd,
+            double integral_max, double output_max, double max_err = 0.0)
+  {
+    kp_ = kp;
+    ki_ = ki;
+    kd_ = kd;
+    integral_max_ = integral_max;
+    output_max_ = output_max;
+    max_err_ = max_err;
+    reset();
+  }
+
+  void reset()
+  {
+    integral_ = 0.0;
+    prev_error_ = 0.0;
+    first_run_ = true;
+  }
+
+  double update(double error, double dt = 0.02)
+  {
+    // Clamp error if max_err_ > 0
+    if (max_err_ > 0.0) {
+      error = limit(error, -max_err_, max_err_);
+    }
+
+    // Proportional
+    double p_out = kp_ * error;
+
+    // Integral (with clamping)
+    integral_ += error * dt;
+    integral_ = limit(integral_, -integral_max_, integral_max_);
+    double i_out = ki_ * integral_;
+
+    // Derivative (skip on first call)
+    double d_out = 0.0;
+    if (!first_run_) {
+      double derivative = (error - prev_error_) / dt;
+      d_out = kd_ * derivative;
+    }
+    first_run_ = false;
+    prev_error_ = error;
+
+    // Output clamping
+    double output = p_out + i_out + d_out;
+    output = limit(output, -output_max_, output_max_);
+
+    return output;
+  }
+
+private:
+  double kp_, ki_, kd_;
+  double integral_max_, output_max_;
+  double integral_;
+  double prev_error_;
+  bool first_run_;
+  double max_err_;  // initialized to 0 in constructor
+};
+
+}  // namespace past_control
+
+#endif  // PAST_CONTROL__TOOLS_H

src/past_control/launch/past_control.launch.py

@@ -0,0 +1,52 @@
+"""
+Launch file for past_control — racing control stack.
+
+Launches 3 nodes (a_star_planner removed — Nav2 planner_server handles global planning):
+  1. lane_follower_node     — visual lane centering
+  2. obstacle_detector_node — DNN obstacle perception
+  3. racing_orchestrator    — task FSM + PID following (/plan from Nav2) + cmd_vel arbitration
+
+Usage:
+  ros2 launch past_control past_control.launch.py
+"""
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    pkg_dir = get_package_share_directory("past_control")
+    config_path = os.path.join(pkg_dir, "config", "past_control.yaml")
+
+    lane_follower = Node(
+        package="past_control",
+        executable="lane_follower_node",
+        name="lane_follower_node",
+        output="screen",
+        parameters=[config_path],
+    )
+
+    obstacle_detector = Node(
+        package="past_control",
+        executable="obstacle_detector_node",
+        name="obstacle_detector_node",
+        output="screen",
+        parameters=[config_path],
+    )
+
+    racing_orchestrator = Node(
+        package="past_control",
+        executable="racing_orchestrator",
+        name="racing_orchestrator",
+        output="screen",
+        parameters=[config_path],
+    )
+
+    return LaunchDescription([
+        lane_follower,
+        obstacle_detector,
+        racing_orchestrator,
+    ])

src/past_control/msg/Obstacle.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+float64 x
+float64 y
+float64 radius
+string type

src/past_control/msg/ObstacleArray.msg

@@ -0,0 +1 @@
+Obstacle[] obstacles

src/past_control/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>past_control</name>
+  <version>0.0.0</version>
+  <description>Racing control stack: lane follower, obstacle detector, and racing orchestrator (Nav2 /plan subscriber + PID following)</description>
+  <maintainer email="2314753575@qq.com">sunrise</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>nav2_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>visualization_msgs</depend>
+  <depend>origincar_msg</depend>
+  <depend>builtin_interfaces</depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/past_control/src/lane_follower_node.cpp

@@ -0,0 +1,81 @@
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "std_msgs/msg/float32_multi_array.hpp"
+#include "std_msgs/msg/float32_multi_array.hpp"
+
+class LaneFollowerNode : public rclcpp::Node
+{
+public:
+  LaneFollowerNode()
+  : Node("lane_follower_node")
+  {
+    this->declare_parameter("follow_linear_speed", 0.3);
+    this->declare_parameter("follow_angular_ratio", 1.0);
+    this->declare_parameter("image_width", 640.0);
+
+    follow_linear_speed_ = this->get_parameter("follow_linear_speed").as_double();
+    follow_angular_ratio_ = this->get_parameter("follow_angular_ratio").as_double();
+    image_width_ = this->get_parameter("image_width").as_double();
+
+    // Subscriber: racing track center detection
+    // Expects Float32MultiArray with [center_x] in image coordinates,
+    // or a custom format. We assume center_x relative to image center.
+    track_sub_ = this->create_subscription<std_msgs::msg::Float32MultiArray>(
+      "racing_track_center_detection", 10,
+      std::bind(&LaneFollowerNode::trackCallback, this, std::placeholders::_1));
+
+    // Publisher: lane cmd_vel (NOT /cmd_vel directly)
+    cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
+      "/lane_cmd_vel", 10);
+
+    RCLCPP_INFO(this->get_logger(),
+      "LaneFollowerNode started. linear=%.2f, angular_ratio=%.2f",
+      follow_linear_speed_, follow_angular_ratio_);
+  }
+
+private:
+  void trackCallback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
+  {
+    if (msg->data.empty()) {
+      // No track detected — stop
+      geometry_msgs::msg::Twist cmd;
+      cmd.linear.x = 0.0;
+      cmd.angular.z = 0.0;
+      cmd_vel_pub_->publish(cmd);
+      return;
+    }
+
+    // center_x from detection: pixel offset from image center
+    // range: [-image_width/2, image_width/2]
+    double center_offset = msg->data[0];  // in pixels
+
+    // Normalize to [-1, 1]
+    double normalized_error = center_offset / (image_width_ / 2.0);
+    normalized_error = std::max(-1.0, std::min(1.0, normalized_error));
+
+    // Angular velocity: steer toward center line
+    // Positive error = line is to the right → positive angular to turn right
+    double angular_z = follow_angular_ratio_ * normalized_error;
+
+    geometry_msgs::msg::Twist cmd;
+    cmd.linear.x = follow_linear_speed_;
+    cmd.angular.z = angular_z;
+    cmd_vel_pub_->publish(cmd);
+  }
+
+  rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr track_sub_;
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+
+  double follow_linear_speed_;
+  double follow_angular_ratio_;
+  double image_width_;
+};
+
+int main(int argc, char* argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<LaneFollowerNode>());
+  rclcpp::shutdown();
+  return 0;
+}

src/past_control/src/obstacle_detector_node.cpp

@@ -0,0 +1,222 @@
+#include <memory>
+#include <mutex>
+#include <cmath>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/float32_multi_array.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "std_msgs/msg/header.hpp"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Matrix3x3.h"
+
+#include "past_control/msg/obstacle_array.hpp"
+#include "past_control/msg/obstacle.hpp"
+
+// DNN detection format (per the existing hobot_dnn convention)
+// Typically a custom array of detections: [class_id, x1, y1, x2, y2, confidence, ...]
+// We'll use Float32MultiArray for input
+
+class ObstacleDetectorNode : public rclcpp::Node
+{
+public:
+  ObstacleDetectorNode()
+  : Node("obstacle_detector_node"), has_odom_(false)
+  {
+    this->declare_parameter("confidence_threshold", 0.5);
+    this->declare_parameter("processing_latency", 0.1);  // seconds
+    this->declare_parameter("camera_hfov", 1.0472);      // 60 degrees
+    this->declare_parameter("camera_height", 0.3);       // meters above ground
+    this->declare_parameter("camera_pitch", 0.0);        // radians
+    this->declare_parameter("max_detection_range", 3.0); // meters
+    this->declare_parameter("publish_rate", 10.0);
+
+    confidence_threshold_ = this->get_parameter("confidence_threshold").as_double();
+    processing_latency_ = this->get_parameter("processing_latency").as_double();
+    camera_hfov_ = this->get_parameter("camera_hfov").as_double();
+    camera_height_ = this->get_parameter("camera_height").as_double();
+    camera_pitch_ = this->get_parameter("camera_pitch").as_double();
+    max_detection_range_ = this->get_parameter("max_detection_range").as_double();
+
+    // Subscribers
+    dnn_sub_ = this->create_subscription<std_msgs::msg::Float32MultiArray>(
+      "hobot_dnn_detection", 10,
+      std::bind(&ObstacleDetectorNode::dnnCallback, this, std::placeholders::_1));
+
+    odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+      "/odom", 10,
+      std::bind(&ObstacleDetectorNode::odomCallback, this, std::placeholders::_1));
+
+    // Publishers
+    obstacles_pub_ = this->create_publisher<past_control::msg::ObstacleArray>(
+      "/obstacles", 10);
+
+    marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
+      "/obstacle_markers", 10);
+
+    double rate = this->get_parameter("publish_rate").as_double();
+    int period_ms = static_cast<int>(1000.0 / rate);
+    timer_ = this->create_wall_timer(
+      std::chrono::milliseconds(period_ms),
+      std::bind(&ObstacleDetectorNode::publishLoop, this));
+
+    RCLCPP_INFO(this->get_logger(), "ObstacleDetectorNode started");
+  }
+
+private:
+  void dnnCallback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    // Parse DNN detections
+    // Expected format: interleaved [class_id, x_center, y_center, width, height, conf]
+    // Normalized coordinates (0-1) within image frame
+    const int fields_per_detection = 6;
+    int num_detections = msg->data.size() / fields_per_detection;
+
+    latest_detections_.clear();
+    for (int i = 0; i < num_detections; ++i) {
+      int base = i * fields_per_detection;
+      Detection det;
+      det.class_id  = static_cast<int>(msg->data[base + 0]);
+      det.x_center  = msg->data[base + 1];  // normalized [0,1]
+      det.y_center  = msg->data[base + 2];  // normalized [0,1]
+      det.width     = msg->data[base + 3];  // normalized
+      det.height    = msg->data[base + 4];  // normalized
+      det.confidence = msg->data[base + 5];
+
+      if (det.confidence >= confidence_threshold_) {
+        latest_detections_.push_back(det);
+      }
+    }
+
+    has_detections_ = true;
+  }
+
+  void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    cur_x_ = msg->pose.pose.position.x;
+    cur_y_ = msg->pose.pose.position.y;
+
+    // Extract yaw from quaternion
+    double qx = msg->pose.pose.orientation.x;
+    double qy = msg->pose.pose.orientation.y;
+    double qz = msg->pose.pose.orientation.z;
+    double qw = msg->pose.pose.orientation.w;
+    cur_yaw_ = std::atan2(2.0*(qw*qz + qx*qy), 1.0 - 2.0*(qy*qy + qz*qz));
+    has_odom_ = true;
+  }
+
+  void publishLoop()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    past_control::msg::ObstacleArray obs_array;
+    visualization_msgs::msg::MarkerArray marker_array;
+
+    if (!has_odom_ || !has_detections_) {
+      // Publish empty
+      obstacles_pub_->publish(obs_array);
+      marker_pub_->publish(marker_array);
+      return;
+    }
+
+    int marker_id = 0;
+    for (const auto& det : latest_detections_) {
+      // Convert pixel coords to world coords using simple pinhole model
+      // x_center normalized [0,1] → angle offset from camera optical axis
+      double pixel_offset = det.x_center - 0.5;  // [-0.5, 0.5]
+      double angle_offset = pixel_offset * camera_hfov_;  // radians
+
+      double obstacle_angle = cur_yaw_ + angle_offset;
+
+      // Estimate distance from bounding box height
+      // Larger bbox = closer object (simple inverse relationship)
+      double est_distance = (1.0 - det.height) * max_detection_range_ + 0.5;
+
+      // World coordinates of obstacle
+      double obs_x = cur_x_ + est_distance * std::cos(obstacle_angle);
+      double obs_y = cur_y_ + est_distance * std::sin(obstacle_angle);
+      double obs_radius = 0.15;  // default obstacle radius
+
+      // Add to obstacle array
+      past_control::msg::Obstacle obs;
+      obs.header.stamp = this->now();
+      obs.header.frame_id = "odom";
+      obs.x = obs_x;
+      obs.y = obs_y;
+      obs.radius = obs_radius;
+      obs.type = "circle";
+      obs_array.obstacles.push_back(obs);
+
+      // Add marker
+      visualization_msgs::msg::Marker marker;
+      marker.header.stamp = this->now();
+      marker.header.frame_id = "odom";
+      marker.ns = "obstacles";
+      marker.id = marker_id++;
+      marker.type = visualization_msgs::msg::Marker::CYLINDER;
+      marker.action = visualization_msgs::msg::Marker::ADD;
+      marker.pose.position.x = obs_x;
+      marker.pose.position.y = obs_y;
+      marker.pose.position.z = 0.0;
+      marker.pose.orientation.w = 1.0;
+      marker.scale.x = obs_radius * 2;
+      marker.scale.y = obs_radius * 2;
+      marker.scale.z = 0.3;
+      marker.color.r = 1.0f;
+      marker.color.g = 0.0f;
+      marker.color.b = 0.0f;
+      marker.color.a = 0.8f;
+      marker.lifetime = rclcpp::Duration::from_seconds(0.5);
+      marker_array.markers.push_back(marker);
+    }
+
+    obstacles_pub_->publish(obs_array);
+    marker_pub_->publish(marker_array);
+  }
+
+  struct Detection
+  {
+    int class_id;
+    double x_center, y_center;
+    double width, height;
+    double confidence;
+  };
+
+  // Subscribers
+  rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr dnn_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+
+  // Publishers
+  rclcpp::Publisher<past_control::msg::ObstacleArray>::SharedPtr obstacles_pub_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  // State
+  std::vector<Detection> latest_detections_;
+  double cur_x_, cur_y_, cur_yaw_;
+  bool has_odom_, has_detections_;
+  std::mutex mutex_;
+
+  // Parameters
+  double confidence_threshold_;
+  double processing_latency_;
+  double camera_hfov_;
+  double camera_height_;
+  double camera_pitch_;
+  double max_detection_range_;
+};
+
+int main(int argc, char* argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<ObstacleDetectorNode>());
+  rclcpp::shutdown();
+  return 0;
+}

src/past_control/src/racing_orchestrator.cpp

@@ -0,0 +1,448 @@
+#include <chrono>
+#include <memory>
+#include <mutex>
+#include <cmath>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav2_msgs/action/compute_path_to_pose.hpp"
+#include "origincar_msg/msg/sign.hpp"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Matrix3x3.h"
+
+#include "past_control/tools.h"
+
+using namespace std::chrono_literals;
+
+// Task states
+enum class TaskState
+{
+  IDLE    = 0,
+  GOING   = 1,
+  RETURN  = 2,
+  QR_SCAN = 3,
+  RESET   = 4
+};
+
+class RacingOrchestrator : public rclcpp::Node
+{
+public:
+  using ComputePathToPose = nav2_msgs::action::ComputePathToPose;
+  using GoalHandleComputePathToPose = rclcpp_action::ClientGoalHandle<ComputePathToPose>;
+
+  RacingOrchestrator()
+  : Node("racing_orchestrator"),
+    state_(TaskState::IDLE),
+    has_odom_(false), has_goal_(false),
+    has_path_(false), got_lane_cmd_(false),
+    is_back_(false), activate_avoid_(false),
+    waypoint_index_(0), guide_step_(5),
+    follow_linear_speed_(0.4),
+    sub_target_(true),
+    cmd_vel_linear_x_(0.0), cmd_vel_angular_z_(0.0),
+    cur_x_(0.0), cur_y_(0.0), cur_yaw_(0.0)
+  {
+    // Parameters
+    // PID path following
+    this->declare_parameter("follow_linear_speed", 0.4);
+    this->declare_parameter("guide_step", 5);
+    this->declare_parameter("arrive_square", 0.25);
+    this->declare_parameter("angular_kp", 1.5);
+    this->declare_parameter("angular_ki", 0.0);
+    this->declare_parameter("angular_kd", 0.1);
+    this->declare_parameter("angular_integral_max", 1.0);
+    this->declare_parameter("angular_output_max", 2.0);
+    this->declare_parameter("angular_max_err", 3.14);
+
+    // Obstacle avoidance
+    this->declare_parameter("avoid_linear_speed", 0.2);
+    this->declare_parameter("avoid_angular_z", 0.8);
+
+    // QR scan params
+    this->declare_parameter("qr_scan_speed", 0.15);
+    this->declare_parameter("qr_scan_duration", 3.0);
+
+    // Control rate
+    this->declare_parameter("control_rate", 20.0);
+
+    // Load params
+    follow_linear_speed_ = this->get_parameter("follow_linear_speed").as_double();
+    guide_step_ = this->get_parameter("guide_step").as_int();
+    arrive_square_ = this->get_parameter("arrive_square").as_double();
+    avoid_linear_speed_ = this->get_parameter("avoid_linear_speed").as_double();
+    avoid_angular_z_ = this->get_parameter("avoid_angular_z").as_double();
+    qr_scan_speed_ = this->get_parameter("qr_scan_speed").as_double();
+    qr_scan_duration_ = this->get_parameter("qr_scan_duration").as_double();
+
+    // Init PID
+    double kp = this->get_parameter("angular_kp").as_double();
+    double ki = this->get_parameter("angular_ki").as_double();
+    double kd = this->get_parameter("angular_kd").as_double();
+    double i_max = this->get_parameter("angular_integral_max").as_double();
+    double o_max = this->get_parameter("angular_output_max").as_double();
+    double max_err = this->get_parameter("angular_max_err").as_double();
+
+    angular_pid_.init(kp, ki, kd, i_max, o_max, max_err);
+
+    // Subscribers
+    sign_sub_ = this->create_subscription<origincar_msg::msg::Sign>(
+      "sign4return", 10,
+      std::bind(&RacingOrchestrator::signCallback, this, std::placeholders::_1));
+
+    sign_foxglove_sub_ = this->create_subscription<origincar_msg::msg::Sign>(
+      "sign_foxglove", 10,
+      std::bind(&RacingOrchestrator::signFoxgloveCallback, this, std::placeholders::_1));
+
+    path_sub_ = this->create_subscription<nav_msgs::msg::Path>(
+      "/plan", 10,
+      std::bind(&RacingOrchestrator::pathCallback, this, std::placeholders::_1));
+
+    goal_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
+      "/goal_pose", 10,
+      std::bind(&RacingOrchestrator::goalCallback, this, std::placeholders::_1));
+
+    odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+      "/odom", 10,
+      std::bind(&RacingOrchestrator::odomCallback, this, std::placeholders::_1));
+
+    lane_cmd_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
+      "/lane_cmd_vel", 10,
+      std::bind(&RacingOrchestrator::laneCmdCallback, this, std::placeholders::_1));
+
+    // Publishers
+    cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
+
+    // ComputePathToPose action client (triggers Nav2 planner_server)
+    planner_client_ = rclcpp_action::create_client<ComputePathToPose>(
+      this, "compute_path_to_pose");
+
+    // Control loop
+    double rate = this->get_parameter("control_rate").as_double();
+    int period_ms = static_cast<int>(1000.0 / rate);
+    timer_ = this->create_wall_timer(
+      std::chrono::milliseconds(period_ms),
+      std::bind(&RacingOrchestrator::controlLoop, this));
+
+    RCLCPP_INFO(this->get_logger(), "RacingOrchestrator started. State: IDLE");
+  }
+
+private:
+  // ─────────────────────────────────────────────
+  //  A. Task State Machine — sign callbacks
+  // ─────────────────────────────────────────────
+
+  void signCallback(const origincar_msg::msg::Sign::SharedPtr msg)
+  {
+    processSign(msg->sign_data);
+  }
+
+  void signFoxgloveCallback(const origincar_msg::msg::Sign::SharedPtr msg)
+  {
+    processSign(msg->sign_data);
+  }
+
+  void processSign(int sign_data)
+  {
+    RCLCPP_INFO(this->get_logger(), "Received sign: %d", sign_data);
+
+    if (sign_data == -1) {
+      state_ = TaskState::RESET;
+      has_path_ = false;
+      global_path_ = nav_msgs::msg::Path();
+      waypoint_index_ = 0;
+      angular_pid_.reset();
+      RCLCPP_INFO(this->get_logger(), "State -> RESET");
+    }
+    else if (sign_data == 3 || sign_data == 4) {
+      state_ = TaskState::QR_SCAN;
+      qr_scan_start_ = this->now();
+      RCLCPP_INFO(this->get_logger(), "State -> QR_SCAN");
+    }
+    else if (sign_data == 5) {
+      state_ = TaskState::GOING;
+      is_back_ = false;
+      sub_target_ = true;
+      has_path_ = false;
+      waypoint_index_ = 0;
+      angular_pid_.reset();
+      // Trigger Nav2 replanning if we have a goal
+      if (has_goal_) {
+        requestPlan();
+      }
+      RCLCPP_INFO(this->get_logger(), "State -> GOING (forward)");
+    }
+    else if (sign_data == -2) {
+      state_ = TaskState::RETURN;
+      is_back_ = true;
+      has_path_ = false;
+      waypoint_index_ = 0;
+      angular_pid_.reset();
+      if (has_goal_) {
+        requestPlan();
+      }
+      RCLCPP_INFO(this->get_logger(), "State -> RETURN");
+    }
+  }
+
+  // ─────────────────────────────────────────────
+  //  B. Callbacks for path, goal, odom, lane cmd_vel
+  // ─────────────────────────────────────────────
+
+  void pathCallback(const nav_msgs::msg::Path::SharedPtr msg)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (msg->poses.empty()) {
+      has_path_ = false;
+      return;
+    }
+    global_path_ = *msg;
+    has_path_ = true;
+    waypoint_index_ = 0;
+    angular_pid_.reset();
+
+    // Auto-transition IDLE -> GOING when receiving an external path
+    // (e.g. publish_sine_path.py publishes /plan without /goal_pose)
+    if (state_ == TaskState::IDLE || state_ == TaskState::RESET) {
+      state_ = TaskState::GOING;
+      is_back_ = false;
+      sub_target_ = true;
+      RCLCPP_INFO(this->get_logger(),
+        "Auto State -> GOING (external path, %zu waypoints)",
+        msg->poses.size());
+    }
+  }
+
+  void goalCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
+  {
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+      goal_pose_ = *msg;
+      has_goal_ = true;
+      RCLCPP_INFO(this->get_logger(), "New goal: (%.2f, %.2f)",
+                  msg->pose.position.x, msg->pose.position.y);
+
+      // Auto-transition IDLE → GOING if not already active
+      if (state_ == TaskState::IDLE || state_ == TaskState::RESET) {
+        state_ = TaskState::GOING;
+        is_back_ = false;
+        sub_target_ = true;
+        has_path_ = false;
+        waypoint_index_ = 0;
+        angular_pid_.reset();
+        RCLCPP_INFO(this->get_logger(), "Auto State -> GOING (goal received)");
+      }
+    }
+    // Request plan outside mutex to avoid blocking other callbacks
+    requestPlan();
+  }
+
+  void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    cur_x_ = msg->pose.pose.position.x;
+    cur_y_ = msg->pose.pose.position.y;
+
+    double qx = msg->pose.pose.orientation.x;
+    double qy = msg->pose.pose.orientation.y;
+    double qz = msg->pose.pose.orientation.z;
+    double qw = msg->pose.pose.orientation.w;
+    cur_yaw_ = std::atan2(2.0 * (qw * qz + qx * qy),
+                          1.0 - 2.0 * (qy * qy + qz * qz));
+    has_odom_ = true;
+  }
+
+  void laneCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+    lane_cmd_vel_ = *msg;
+    got_lane_cmd_ = true;
+  }
+
+  // ─────────────────────────────────────────────
+  //  C. ComputePathToPose action client
+  // ─────────────────────────────────────────────
+
+  void requestPlan()
+  {
+    if (!planner_client_->wait_for_action_server(std::chrono::seconds(2))) {
+      RCLCPP_WARN(this->get_logger(),
+                  "Planner action server (compute_path_to_pose) not available");
+      return;
+    }
+
+    auto goal_msg = ComputePathToPose::Goal();
+    goal_msg.goal = goal_pose_;
+    goal_msg.planner_id = "GridBased";
+
+    auto send_goal_options = rclcpp_action::Client<ComputePathToPose>::SendGoalOptions();
+    send_goal_options.result_callback =
+      [this](const GoalHandleComputePathToPose::WrappedResult& result) {
+        if (result.code == rclcpp_action::ResultCode::SUCCEEDED) {
+          RCLCPP_INFO(this->get_logger(), "Plan received: %zu waypoints",
+                      result.result->path.poses.size());
+        } else {
+          RCLCPP_WARN(this->get_logger(), "Plan request failed");
+        }
+      };
+
+    RCLCPP_INFO(this->get_logger(), "Requesting plan from Nav2 planner_server...");
+    planner_client_->async_send_goal(goal_msg, send_goal_options);
+  }
+
+  // ─────────────────────────────────────────────
+  //  D. Main Control Loop — cmd_vel arbitration
+  // ─────────────────────────────────────────────
+
+  void controlLoop()
+  {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    // Priority 1: Task state control
+    switch (state_)
+    {
+      case TaskState::IDLE:
+        publishStop();
+        return;
+
+      case TaskState::RESET:
+        publishStop();
+        state_ = TaskState::IDLE;
+        return;
+
+      case TaskState::QR_SCAN:
+      {
+        auto elapsed = this->now() - qr_scan_start_;
+        if (elapsed.seconds() < 1.0) {
+          geometry_msgs::msg::Twist cmd;
+          cmd.linear.x = qr_scan_speed_;
+          cmd.angular.z = 0.0;
+          cmd_vel_pub_->publish(cmd);
+        }
+        else if (elapsed.seconds() < qr_scan_duration_) {
+          publishStop();
+        }
+        else {
+          state_ = TaskState::GOING;
+          RCLCPP_INFO(this->get_logger(), "QR scan complete, resuming");
+        }
+        return;
+      }
+
+      case TaskState::GOING:
+      case TaskState::RETURN:
+        break;
+    }
+
+    // Priority 2: Obstacle avoidance
+    if (activate_avoid_) {
+      geometry_msgs::msg::Twist cmd;
+      cmd.linear.x = avoid_linear_speed_;
+      cmd.angular.z = avoid_angular_z_;
+      cmd_vel_pub_->publish(cmd);
+      return;
+    }
+
+    // Priority 3: Lane following (sub_target_ == false)
+    if (!sub_target_ && got_lane_cmd_) {
+      cmd_vel_pub_->publish(lane_cmd_vel_);
+      return;
+    }
+
+    // Priority 4: PID path following (path from Nav2 planner_server via /plan)
+    if (sub_target_ && has_path_ && has_odom_) {
+      if (waypoint_index_ >= static_cast<int>(global_path_.poses.size())) {
+        RCLCPP_INFO(this->get_logger(), "Path complete - stopping");
+        publishStop();
+        has_path_ = false;
+        return;
+      }
+
+      int target_idx = std::min(waypoint_index_ + guide_step_,
+                                static_cast<int>(global_path_.poses.size()) - 1);
+      const auto& target = global_path_.poses[target_idx];
+
+      double dx = target.pose.position.x - cur_x_;
+      double dy = target.pose.position.y - cur_y_;
+      double distance_sq = dx * dx + dy * dy;
+
+      if (distance_sq < arrive_square_) {
+        waypoint_index_++;
+        angular_pid_.reset();
+      }
+
+      double target_yaw = std::atan2(dy, dx);
+      double heading_error = target_yaw - cur_yaw_;
+
+      while (heading_error > M_PI)  heading_error -= 2.0 * M_PI;
+      while (heading_error < -M_PI) heading_error += 2.0 * M_PI;
+
+      double dt = 1.0 / this->get_parameter("control_rate").as_double();
+      double angular_z = angular_pid_.update(heading_error, dt);
+
+      geometry_msgs::msg::Twist cmd;
+      cmd.linear.x = follow_linear_speed_;
+      cmd.angular.z = angular_z;
+      cmd_vel_pub_->publish(cmd);
+      return;
+    }
+
+    // Priority 5: No path - stop and wait
+    publishStop();
+  }
+
+  void publishStop()
+  {
+    geometry_msgs::msg::Twist cmd;
+    cmd.linear.x = 0.0;
+    cmd.angular.z = 0.0;
+    cmd_vel_pub_->publish(cmd);
+  }
+
+  // ─────────────────────────────────────────────
+  //  Members (order must match initializer list)
+  // ─────────────────────────────────────────────
+
+  TaskState state_;
+  bool has_odom_, has_goal_;
+  bool has_path_, got_lane_cmd_;
+  bool is_back_, activate_avoid_;
+  int waypoint_index_;
+  int guide_step_;
+  double follow_linear_speed_;
+  bool sub_target_;
+  double cmd_vel_linear_x_, cmd_vel_angular_z_;
+  double cur_x_, cur_y_, cur_yaw_;
+  double arrive_square_;
+  past_control::PID angular_pid_;
+  double avoid_linear_speed_;
+  double avoid_angular_z_;
+  double qr_scan_speed_;
+  double qr_scan_duration_;
+  rclcpp::Time qr_scan_start_;
+  nav_msgs::msg::Path global_path_;
+  geometry_msgs::msg::Twist lane_cmd_vel_;
+  geometry_msgs::msg::PoseStamped goal_pose_;
+
+  rclcpp::Subscription<origincar_msg::msg::Sign>::SharedPtr sign_sub_;
+  rclcpp::Subscription<origincar_msg::msg::Sign>::SharedPtr sign_foxglove_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr lane_cmd_sub_;
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+  rclcpp_action::Client<ComputePathToPose>::SharedPtr planner_client_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  std::mutex mutex_;
+};
+
+int main(int argc, char* argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<RacingOrchestrator>());
+  rclcpp::shutdown();
+  return 0;
+}

src/planner/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.5)
+project(planner)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra)
+endif()
+
+find_package(ament_cmake REQUIRED)
+
+# Dummy executable to satisfy ament_cmake + install of launch/config
+add_executable(planner_version src/planner_version.cpp)
+
+install(TARGETS planner_version
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/planner/config/planner.yaml

@@ -0,0 +1,232 @@
+# ============================================================================
+# planner.yaml — Nav2 minimal global planning stack
+# 节点: slam_toolbox + planner_server + global_costmap + local_costmap
+# 来源: gc_navigation2_slamtoolbox (slam + nav params) 剪裁
+# ============================================================================
+
+# ---------------------------------------------------------------------------
+# slam_toolbox — online_async 实时建图 → 发布 /map + map→odom transform
+# 来源: slam_toolbox_mapping.yaml
+# ---------------------------------------------------------------------------
+slam_toolbox:
+  ros__parameters:
+    use_sim_time: False
+
+    # Solver
+    solver_plugin: solver_plugins::CeresSolver
+    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
+    ceres_preconditioner: SCHUR_JACOBI
+    ceres_trust_strategy: LEVENBERG_MARQUARDT
+    ceres_dogleg_type: TRADITIONAL_DOGLEG
+    ceres_loss_function: None
+
+    # ROS base
+    odom_frame: odom
+    map_frame: map
+    base_frame: base_link
+    scan_topic: /scan
+    mode: mapping
+    use_map_saver: true
+
+    # Debug & performance
+    debug_logging: false
+    throttle_scans: 1
+    transform_publish_period: 0.02
+    map_update_interval: 3.0
+    resolution: 0.05
+    min_laser_range: 0.15
+    max_laser_range: 20.0
+    minimum_time_interval: 0.5
+    transform_timeout: 0.2
+    tf_buffer_duration: 30.0
+    stack_size_to_use: 40000000
+    enable_interactive_mode: true
+
+    # Mapping
+    use_scan_matching: true
+    use_scan_barycenter: true
+    minimum_travel_distance: 0.5
+    minimum_travel_heading: 0.1
+    scan_buffer_size: 10
+    scan_buffer_maximum_scan_distance: 10.0
+    link_match_minimum_response_fine: 0.1
+    link_scan_maximum_distance: 1.5
+
+    # Loop closure
+    do_loop_closing: true
+    loop_match_minimum_chain_size: 10
+    loop_match_maximum_variance_coarse: 3.0
+    loop_match_minimum_response_coarse: 0.35
+    loop_match_minimum_response_fine: 0.45
+    loop_search_maximum_distance: 3.0
+
+    # Scan matching
+    correlation_search_space_dimension: 0.5
+    correlation_search_space_resolution: 0.01
+    correlation_search_space_smear_deviation: 0.1
+    loop_search_space_dimension: 8.0
+    loop_search_space_resolution: 0.05
+    loop_search_space_smear_deviation: 0.03
+
+    # Matcher params
+    distance_variance_penalty: 0.5
+    angle_variance_penalty: 1.0
+    fine_search_angle_offset: 0.00349
+    coarse_search_angle_offset: 0.349
+    coarse_angle_resolution: 0.0349
+    minimum_angle_penalty: 0.9
+    minimum_distance_penalty: 0.5
+    use_response_expansion: true
+    min_pass_through: 2
+    occupancy_threshold: 0.1
+    scan_queue_size: 20
+
+# ---------------------------------------------------------------------------
+# planner_server — SmacPlannerHybrid 全局路径规划 → /plan
+# 来源: gc_navigation_slam.yaml planner_server section
+# ---------------------------------------------------------------------------
+planner_server:
+  ros__parameters:
+    planner_plugins:
+    - GridBased
+    use_sim_time: False
+    GridBased:
+      plugin: nav2_smac_planner/SmacPlannerHybrid
+      downsample_costmap: False
+      downsampling_factor: 1
+      tolerance: 0.25
+      allow_unknown: True
+      max_iterations: 100000
+      max_on_approach_iterations: 1000
+      max_planning_time: 5.0
+      motion_model_for_search: REEDS_SHEPP
+      angle_quantization_bins: 72
+      analytic_expansion_ratio: 2.0
+      analytic_expansion_max_length: 3.0
+      minimum_turning_radius: 0.35
+      reverse_penalty: 1.3
+      change_penalty: 0.0
+      non_straight_penalty: 0.0
+      cost_penalty: 5.0
+      retrospective_penalty: 0.015
+      lookup_table_size: 5.0
+      cache_obstacle_heuristic: False
+      viz_expansions: False
+      smooth_path: True
+      smoother:
+        max_iterations: 1000
+        w_smooth: 0.4
+        w_data: 0.2
+        tolerance: 1.0e-10
+        do_refinement: True
+        refinement_num: 4
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+# ---------------------------------------------------------------------------
+# global_costmap — 全局代价地图（订阅 /scan + /map）
+# 来源: gc_navigation_slam.yaml global_costmap section
+# ---------------------------------------------------------------------------
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      use_sim_time: False
+      transform_tolerance: 2.0
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
+      footprint_padding: 0.02
+      resolution: 0.05
+      track_unknown_space: True
+      plugins:
+      - static_layer
+      - obstacle_layer
+      - inflation_layer
+      obstacle_layer:
+        plugin: nav2_costmap_2d::ObstacleLayer
+        enabled: True
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: LaserScan
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        plugin: nav2_costmap_2d::StaticLayer
+        map_subscribe_transient_local: True
+      inflation_layer:
+        plugin: nav2_costmap_2d::InflationLayer
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.2
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+# ---------------------------------------------------------------------------
+# local_costmap — 局部代价地图（obstacle_detector 可注入）
+# 来源: gc_navigation_slam.yaml local_costmap section
+# ---------------------------------------------------------------------------
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 2.0
+      transform_tolerance: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: False
+      rolling_window: True
+      width: 3
+      height: 3
+      resolution: 0.05
+      footprint: '[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]'
+      footprint_padding: 0.02
+      plugins:
+      - voxel_layer
+      - inflation_layer
+      inflation_layer:
+        plugin: nav2_costmap_2d::InflationLayer
+        cost_scaling_factor: 3.0
+        inflation_radius: 0.2
+      voxel_layer:
+        plugin: nav2_costmap_2d::VoxelLayer
+        enabled: True
+        publish_voxel_map: True
+        origin_z: 0.0
+        z_resolution: 0.05
+        z_voxels: 16
+        max_obstacle_height: 2.0
+        mark_threshold: 0
+        observation_sources: scan
+        scan:
+          topic: /scan
+          max_obstacle_height: 2.0
+          clearing: True
+          marking: True
+          data_type: LaserScan
+          raytrace_max_range: 3.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          obstacle_min_range: 0.0
+      static_layer:
+        map_subscribe_transient_local: True
+      always_send_full_costmap: True
+  local_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False

src/planner/launch/planner.launch.py

@@ -0,0 +1,77 @@
+# ============================================================================
+# planner.launch.py
+# Nav2 最小全局规划模块
+# 启动: slam_toolbox + planner_server + lifecycle_manager
+# costmap 由 planner_server 内部实例化，不需要单独启动
+# 不启动: controller / behavior / bt_navigator / velocity_smoother / smoother
+#
+# 参考: gc_nav2_with_slam_online_real.launch.py
+# ============================================================================
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    """启动 Nav2 最小规划栈：slam + planner"""
+    ld = LaunchDescription()
+
+    # === 包路径 ===
+    pkg_dir = get_package_share_directory('planner')
+
+    # === 参数 ===
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    params_file = LaunchConfiguration(
+        'params_file',
+        default=os.path.join(pkg_dir, 'config', 'planner.yaml'))
+
+    declare_params = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(pkg_dir, 'config', 'planner.yaml'),
+        description='Path to the planner parameters file')
+    ld.add_action(declare_params)
+
+    # =====================================================================
+    # 1. slam_toolbox — 异步建图 (online_async)，发布 /map + map→odom
+    # =====================================================================
+    slam_toolbox_node = Node(
+        package='slam_toolbox',
+        executable='async_slam_toolbox_node',
+        name='slam_toolbox',
+        output='screen',
+        parameters=[params_file,
+                     {'use_sim_time': use_sim_time}],
+    )
+    ld.add_action(slam_toolbox_node)
+
+    # =====================================================================
+    # 2. planner_server — SmacPlannerHybrid，ComputePathToPose action
+    #    planner_server 内部实例化 global_costmap（从 planner.yaml 读取参数）
+    # =====================================================================
+    planner_server_node = Node(
+        package='nav2_planner',
+        executable='planner_server',
+        name='planner_server',
+        output='screen',
+        parameters=[params_file],
+    )
+    ld.add_action(planner_server_node)
+
+    # =====================================================================
+    # 3. lifecycle_manager — 自动激活 planner_server (lifecycle 节点)
+    # =====================================================================
+    lifecycle_manager_node = Node(
+        package='nav2_lifecycle_manager',
+        executable='lifecycle_manager',
+        name='lifecycle_manager_planner',
+        output='screen',
+        parameters=[{'autostart': True},
+                    {'node_names': ['planner_server']}],
+    )
+    ld.add_action(lifecycle_manager_node)
+
+    return ld

src/planner/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>planner</name>
+  <version>0.0.0</version>
+  <description>Minimal Nav2 global planning stack: slam_toolbox + planner_server + costmaps + lifecycle_manager</description>
+  <maintainer email="2314753575@qq.com">sunrise</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>slam_toolbox</depend>
+  <depend>nav2_planner</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>nav2_lifecycle_manager</depend>
+  <depend>nav2_common</depend>
+  <depend>nav2_util</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/planner/src/planner_version.cpp

@@ -0,0 +1,11 @@
+#include <iostream>
+
+int main(int argc, char* argv[])
+{
+  std::cout << "planner package: Nav2 minimal global planning stack" << std::endl;
+  std::cout << "  - slam_toolbox (online_async)" << std::endl;
+  std::cout << "  - planner_server (SmacPlannerHybrid)" << std::endl;
+  std::cout << "  - global_costmap + local_costmap" << std::endl;
+  std::cout << "  - lifecycle_manager" << std::endl;
+  return 0;
+}

src/qr_detection/src/qr_dete_depth.cpp

@@ -22,11 +22,13 @@ public:
         std::bind(&MinimalHbmemSubscriber::image_callback, this, std::placeholders::_1));
 
     // 创建订阅器，订阅 'sign4return' 话题
-    subscription_sign_ =
+    // subscription_sign_ =
-      this->create_subscription<std_msgs::msg::Int32>(
+    //   this->create_subscription<std_msgs::msg::Int32>(
-        "sign4return",
+    //     "sign4return",
-        10,
+    //     10,
-        std::bind(&MinimalHbmemSubscriber::sign_callback, this, std::placeholders::_1));
+    //     std::bind(&MinimalHbmemSubscriber::sign_callback, this, std::placeholders::_1));
+    // 创建发布器，发布 'sign4return' 话题
+    sign_publisher_ = this->create_publisher<std_msgs::msg::Int32>("sign4return", 10);
 
     // 创建 publisher，topic 为 "qr_results"
     publisher_ =
@@ -87,6 +89,23 @@ private:
                     symbol->get_type_name().c_str(), symbol->get_data().c_str());
       }
       qr_results_buff_ = qr_results; // 更新缓存
+
+      // 检测字符合法性
+      // if (ResultLegal(qr_results)){
+      //   // 发布sign4return话题
+      //   auto message = std_msgs::msg::Int32();
+      //   message.data = 5; // 5表示检测到二维码
+      //   sign_publisher_->publish(message);
+      //   RCLCPP_INFO(this->get_logger(), "QR Results Legal");
+      // }
+      // else{
+      //   RCLCPP_INFO(this->get_logger(), "\033[31m QR Results Illegal \033[00m");
+      // }
+
+      // 发布sign4return话题
+      auto message = std_msgs::msg::Int32();
+      message.data = 5; // 5表示检测到二维码
+      sign_publisher_->publish(message);
     } else {
       qr_results = "QR Code not detected";
       RCLCPP_INFO(this->get_logger(), "QR Code not detected");
@@ -104,24 +123,36 @@ private:
   }
 
   // 消息回调函数，处理 sign4return 话题
-  void sign_callback(const std_msgs::msg::Int32::SharedPtr msg)
+  // void sign_callback(const std_msgs::msg::Int32::SharedPtr msg)
-  {
+  // {
-    if (msg->data == 0)
+  //   if (msg->data == 0)
-    {
+  //   {
-      detect_qr_code_ = true; // 启动二维码检测
+  //     detect_qr_code_ = true; // 启动二维码检测
-      RCLCPP_INFO(this->get_logger(), "QR detection started");
+  //     RCLCPP_INFO(this->get_logger(), "QR detection started");
-    }
+  //   }
-    else if (msg->data == 5)
+  //   else if (msg->data == 5)
-    {
+  //   {
-      detect_qr_code_ = false; // 停止二维码检测
+  //     detect_qr_code_ = false; // 停止二维码检测
-      RCLCPP_INFO(this->get_logger(), "QR detection stopped");
+  //     RCLCPP_INFO(this->get_logger(), "QR detection stopped");
-    }
+  //   }
+  // }
+
+  // 识别结果合法性检测
+  bool ResultLegal(std::string& input){
+    // 合法字符串集合
+    static const std::unordered_set<std::string> kValidStrings = {
+        "1", "2", "顺时针", "逆时针", "顺", "逆"
+    };
+    return kValidStrings.find(input) != kValidStrings.end();
   }
 
+
   // /aurora/rgb/image_raw 订阅器
   rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_image_;
   // sign4return 订阅器
-  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_sign_;
+  // rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_sign_;
+  // sign4return 发布器
+  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sign_publisher_;
   // QR code results 发布器
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
 

src/racing_control/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(racing_control)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/racing_control/include/racing_control/racing_control.hpp

@@ -0,0 +1,35 @@
+/*
+功能描述：
+    1. 状态指令：
+        1 - 比赛开始，进行任务一寻找二维码
+        2 - 找到二维码，执行任务一到任务二过渡阶段导航
+        3 - 到达任务二起始阶段，执行顺/逆时针绕圈（可集成图生文）
+        4 - 减速拍照，图像传入图生文节点，随后回到3（备用）
+        5 - 完整走完一圈，执行任务三
+    2. 全部流程
+        启动小车->slamtoolbox开始建图，并开始发布导航命令（这时候小车还不能动）->打开电机开关，小车开始行动->走到一半扫到二维码，发布在/qr_results上->停掉二维码节点（sign=5）和导航1，同时开始导航2到任务二入口并顺逆时针转圈->到达指定位置触发一次vlm请求->开始语音播报同时完成任务二、三
+*/
+
+#include "rclcpp/rclcpp.hpp"
+
+static int QR_SEARCHING = 1;
+static int ENTRY = 2;
+static int CIRCLE = 3;
+static int VLM = 4;
+static int TASK3 = 5;
+
+
+class RacingControl : public rclcpp::Node
+{
+public:
+    RacingControl() : Node("racing_control")
+    {
+        // 初始化状态指令
+        state_command_ = QR_SEARCHING;
+    }  
+private:
+    // 状态指令变量
+    int state_command_;
+};
+
+

src/racing_control/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>racing_control</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="2314753575@qq.com">sunrise</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/vlm_detect/setup.py

@@ -21,6 +21,8 @@ setup(
     entry_points={
       'console_scripts': [
         'vlm_node = vlm_detect.vlm_node:main',
+            'test_publisher = vlm_detect.test_publisher:main',
+            'tts_node = vlm_detect.tts_node:main',
       ],
     },
 )

src/vlm_detect/vlm_detect/test_publisher.py

@@ -0,0 +1,88 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+测试发布者：发送图片和触发信号给 VLM 节点
+用法: ros2 run vlm_detect test_publisher --ros-args -p image_path:="/path/to/image.jpg"
+"""
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Int32, String
+from sensor_msgs.msg import CompressedImage
+import cv2
+import time
+
+
+class TestPublisher(Node):
+    def __init__(self):
+        super().__init__("test_publisher")
+
+        # 声明参数
+        self.declare_parameter(
+            "image_path", "/home/sunrise/yiliao_ws/my_model/image.png"
+        )
+        self.declare_parameter("interval", 5.0)  # 每 N 秒触发一次
+
+        image_path = self.get_parameter("image_path").value
+        interval = self.get_parameter("interval").value
+
+        # 发布者
+        self.image_pub = self.create_publisher(CompressedImage, "/image_mjpeg", 10)
+        self.sign_pub = self.create_publisher(Int32, "/sign4return", 10)
+
+        # 订阅结果
+        self.result_sub = self.create_subscription(
+            String, "/vlm_result", self.result_callback, 10
+        )
+
+        # 加载图片
+        self.image_data = cv2.imread(image_path)
+        if self.image_data is None:
+            self.get_logger().error(f"无法读取图片: {image_path}")
+            raise FileNotFoundError(f"Image not found: {image_path}")
+
+        encode_params = [cv2.IMWRITE_JPEG_QUALITY, 90]
+        _, jpeg_data = cv2.imencode(".jpg", self.image_data, encode_params)
+        self.jpeg_bytes = jpeg_data.tobytes()
+        self.get_logger().info(f"已加载图片: {image_path}")
+
+        # 定时器
+        self.timer = self.create_timer(interval, self.timer_callback)
+        self.get_logger().info(f"每 {interval}s 发送一次图片和触发信号")
+
+    def timer_callback(self):
+        # 1. 发送压缩图像
+        img_msg = CompressedImage()
+        img_msg.header.stamp = self.get_clock().now().to_msg()
+        img_msg.format = "jpeg"
+        img_msg.data = self.jpeg_bytes
+        self.image_pub.publish(img_msg)
+        self.get_logger().info("已发送图片")
+
+        # 2. 等待一小段时间让订阅者收到图片
+        time.sleep(1.0)
+
+        # 3. 发送触发信号 (sign=9)
+        sign_msg = Int32()
+        sign_msg.data = 9
+        self.sign_pub.publish(sign_msg)
+        self.get_logger().info("已发送触发信号 (sign=9)，等待 VLM 结果...")
+
+    def result_callback(self, msg):
+        self.get_logger().info(f"🔍 VLM 识别结果: {msg.data}")
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = TestPublisher()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/vlm_detect/vlm_detect/tts_node.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+import rclpy, subprocess, requests, os
+from rclpy.node import Node
+from std_msgs.msg import String
+
+VLM_HOST = "http://192.168.10.173:8000"
+# USB Audio Device (Card 1)
+AUDIO_SINK = "alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo"
+AUDIO_ENV = {**os.environ, "PULSE_SINK": AUDIO_SINK}
+
+class TTSNode(Node):
+    def __init__(self):
+        super().__init__("tts_node")
+        self.sub = self.create_subscription(String, "/vlm_result", self.callback, 10)
+        self.get_logger().info("TTS 播报节点已启动 (USB Audio Device, edge-tts 自然语音)")
+
+    def callback(self, msg):
+        text = msg.data
+        self.get_logger().info(f"播报: {text}")
+        try:
+            resp = requests.post(f"{VLM_HOST}/v1/tts",
+                json={"text": text, "voice": "zh-CN-XiaoxiaoNeural"}, timeout=60)
+            mp3 = "/tmp/tts_out.mp3"
+            with open(mp3, "wb") as f:
+                f.write(resp.content)
+            subprocess.Popen(["ffplay", "-nodisp", "-autoexit", mp3],
+                stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
+                env=AUDIO_ENV)
+        except Exception as e:
+            self.get_logger().error(f"TTS 失败,降级 espeak: {e}")
+            subprocess.Popen(["espeak-ng", "-v", "zh", "-s", "150", text],
+                env=AUDIO_ENV)
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = TTSNode()
+    try: rclpy.spin(node)
+    except KeyboardInterrupt: pass
+    finally: node.destroy_node(); rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

src/vlm_detect/vlm_detect/vlm_node.py

@@ -15,12 +15,12 @@ import numpy as np
 
 class VLMProcessor(Node):
     def __init__(self):
-        super().__init__('vlm_dtetct')
+        super().__init__('vlm_detect')
         
         # 初始化 OpenAI 客户端
         self.client = OpenAI(
-            base_url="http://192.168.1.103:8000/v1",
+            base_url="http://192.168.10.173:8000/v1",  # 本地 API 地址
-            api_key="EMPTY"
+            api_key="EMPTY",  # 不需要真实 API key
         )
      
         # ROS2 组件
@@ -31,7 +31,7 @@ class VLMProcessor(Node):
         # 订阅图像话题
         self.image_sub = self.create_subscription(
             CompressedImage,
-            '/image',
+            '/image_mjpeg',
             self.image_callback,
             10
         )
@@ -57,18 +57,20 @@ class VLMProcessor(Node):
         """保存最新的图像"""
         with self.image_lock:
             try:
+                # bridge = CvBridge()
                 np_arr = np.frombuffer(msg.data, np.uint8)
                 # 使用 OpenCV 解码图像
                 cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
+                # cv_image =bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
                 self.latest_image = cv_image
                 self.get_logger().debug("recive picture")
             except Exception as e:
-                self.get_logger().error("err picture")
+                self.get_logger().error(f"err picture: {e}")
 
     def sign_callback(self, msg):
         """处理触发信号"""
         if msg.data == 9:
-            self.get_logger().info("收到触发信号 (6)，开始处理图像...")
+            self.get_logger().info(f"收到触发信号 ({msg.data})，开始处理图像...")
             
             # 检查是否有可用图像
             with self.image_lock:
@@ -79,12 +81,12 @@ class VLMProcessor(Node):
                 # 保存临时图像文件
                 temp_path = "/tmp/vlm_temp_image.jpg"
                 cv2.imwrite(temp_path, self.latest_image)
-                self.get_logger().info("已保存临时图像}")
+                self.get_logger().info(f"已保存临时图像: {temp_path}")
             
             # 处理图像
             try:
                 description = self.process_image(temp_path)
-                self.get_logger().info(f"图像描述结果:")
+                self.get_logger().info(f"图像描述结果: {description}")
                 
                 # 发布结果
                 result_msg = String()
@@ -94,7 +96,7 @@ class VLMProcessor(Node):
                 # 清理临时文件
                 os.remove(temp_path)
             except Exception as e:
-                self.get_logger().error(f"处理图像时出错:{e}")
+                self.get_logger().error(f"处理图像时出错: {e}")
 
     def process_image(self, image_path):
         """使用 VLM 模型处理图像"""
@@ -106,26 +108,26 @@ class VLMProcessor(Node):
         start_time = time.time()
         
         response = self.client.chat.completions.create(
-            model="/home/yyh/vllm_test/model/InternVL3-1B",
+            model="./OpenGVLab/InternVL3-1B/",
             messages=[
                 {
                     "role": "user",
                     "content": [
-                        {"type": "text", "text": "描述图片中的动漫病人"},
+                        {"type": "text", "text": "描述图片中有一个病人的特征，字数控制在20字以内。"},
                         {
                             "type": "image_url",
                             "image_url": {
                                 "url": f"data:image/jpeg;base64,{base64_image}"
-                            }
+                            },
-                        }
+                        },
                     ]
                 }
             ],
-            max_tokens=100
+            max_tokens=100,
         )
         
         processing_time = time.time() - start_time
-        self.get_logger().info("VLM 处理耗时秒")
+        self.get_logger().info(f"VLM 处理耗时 {processing_time:.1f}s")
         
         return response.choices[0].message.content
 

tools/measure_turning_radius.py

@@ -0,0 +1,307 @@
+#!/usr/bin/env python3
+"""
+Measure actual turning radius at different speeds for Ackermann robot.
+
+Usage (on RDKx5):
+    python3 measure_turning_radius.py [speed1 speed2 ...]
+
+    Default speeds: 0.1 0.2 0.3 0.4 0.5
+
+Saves data to: ~/turning_radius_data/YYYYMMDD_HHMMSS/
+    raw_v{速度}.csv      — 每条 odom 原始数据
+    summary.csv          — 汇总结果
+    trajectory_v{速度}.png — 轨迹图 (可选)
+
+Safety:
+    - Large open space required (at least 2x the max turning radius)
+    - Ctrl+C to abort at any time
+    - Robot stops automatically after each test
+"""
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+import math
+import sys
+import time
+import os
+import csv
+from datetime import datetime
+import numpy as np
+
+
+class TurningRadiusMeasurer(Node):
+    def __init__(self, speeds, data_dir, max_ang=5.0):
+        super().__init__("turning_radius_measure")
+        self.pub = self.create_publisher(Twist, "/cmd_vel", 10)
+        self.odom_sub = self.create_subscription(Odometry, "/odom", self.odom_cb, 10)
+
+        self.speeds = speeds
+        self.max_ang = max_ang
+        self.data_dir = data_dir
+        self.results = []
+
+        self._poses = []
+        self._v_samples = []
+        self._w_samples = []
+        self._timestamps = []
+        self._collecting = False
+        self._start_yaw = None
+
+    def odom_cb(self, msg):
+        if not self._collecting:
+            return
+        pos = msg.pose.pose.position
+        quat = msg.pose.pose.orientation
+        yaw = self._quat_to_yaw(quat)
+        v = msg.twist.twist.linear.x
+        w = msg.twist.twist.angular.z
+        t = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
+
+        self._poses.append((pos.x, pos.y, yaw))
+        self._v_samples.append(v)
+        self._w_samples.append(w)
+        self._timestamps.append(t)
+
+        if self._start_yaw is None:
+            self._start_yaw = yaw
+
+    def _quat_to_yaw(self, q):
+        siny = 2.0 * (q.w * q.z + q.x * q.y)
+        cosy = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
+        return math.atan2(siny, cosy)
+
+    def _unwrap_delta(self, poses):
+        deltas = []
+        for i in range(1, len(poses)):
+            d = poses[i][2] - poses[i-1][2]
+            d = math.atan2(math.sin(d), math.cos(d))
+            deltas.append(d)
+        return deltas
+
+    def _total_angle(self):
+        if len(self._poses) < 2:
+            return 0.0
+        deltas = self._unwrap_delta(self._poses)
+        return sum(abs(d) for d in deltas)
+
+    def run_test(self, lin_speed):
+        self._poses = []
+        self._v_samples = []
+        self._w_samples = []
+        self._timestamps = []
+        self._collecting = False
+        self._start_yaw = None
+
+        ang_speed = self.max_ang
+
+        self.get_logger().info(
+            "Testing v=%.2f m/s, ω=%.2f rad/s" % (lin_speed, ang_speed))
+        self.get_logger().info("Starting in 2 seconds...")
+        time.sleep(2)
+
+        twist = Twist()
+        twist.linear.x = lin_speed
+        twist.angular.z = ang_speed
+        self.pub.publish(twist)
+
+        self._collecting = True
+        self.get_logger().info("Collecting...")
+
+        while rclpy.ok():
+            rclpy.spin_once(self, timeout_sec=0.05)
+            if self._total_angle() > 6.0:
+                break
+
+        self._collecting = False
+        twist.linear.x = 0.0
+        twist.angular.z = 0.0
+        self.pub.publish(twist)
+
+        self._compute_result(lin_speed, ang_speed)
+
+    def _compute_result(self, lin_speed, ang_speed):
+        if len(self._v_samples) < 10:
+            self.get_logger().warn("Not enough data points!")
+            return
+
+        # Save raw data
+        self._save_raw_data(lin_speed)
+
+        # Compute radius
+        v_trim = self._v_samples[10:]
+        w_trim = self._w_samples[10:]
+
+        v_mean = sum(abs(v) for v in v_trim) / len(v_trim)
+        w_mean = sum(abs(w) for w in w_trim) / len(w_trim)
+
+        if w_mean < 0.01:
+            self.get_logger().warn("Angular velocity too low, can't compute radius")
+            return
+
+        R_vw = v_mean / w_mean
+        R_fit = self._fit_circle_radius()
+
+        # Print
+        self.get_logger().info("=" * 50)
+        self.get_logger().info("v_cmd=%.2f  ω_cmd=%.2f" % (lin_speed, ang_speed))
+        self.get_logger().info("v_avg=%.3f  ω_avg=%.3f" % (v_mean, w_mean))
+        self.get_logger().info("R (v/ω):  %.3f m" % R_vw)
+        if R_fit is not None:
+            self.get_logger().info("R (fit):  %.3f m" % R_fit)
+        self.get_logger().info("Data saved: %s" % self.data_dir)
+        self.get_logger().info("=" * 50)
+
+        self.results.append({
+            "v_cmd": lin_speed, "w_cmd": ang_speed,
+            "v_avg": v_mean, "w_avg": w_mean,
+            "R_vw": R_vw, "R_fit": R_fit if R_fit else -1,
+            "n_samples": len(v_trim)
+        })
+
+        # Try plot
+        self._try_plot(lin_speed)
+
+    def _save_raw_data(self, lin_speed):
+        """Save raw odometry to CSV"""
+        fname = os.path.join(self.data_dir, "raw_v%.2f.csv" % lin_speed)
+        with open(fname, "w", newline="") as f:
+            w = csv.writer(f)
+            w.writerow(["t", "x", "y", "yaw", "v_linear", "v_angular"])
+            for i in range(len(self._poses)):
+                x, y, yaw = self._poses[i]
+                v = self._v_samples[i]
+                ang = self._w_samples[i]
+                t = self._timestamps[i] - self._timestamps[0] if self._timestamps else i
+                w.writerow(["%.4f" % t, "%.6f" % x, "%.6f" % y,
+                           "%.6f" % yaw, "%.6f" % v, "%.6f" % ang])
+        self.get_logger().info("  raw data: %s (%d points)" % (fname, len(self._poses)))
+
+    def _try_plot(self, lin_speed):
+        """Try to save a trajectory plot (skips if matplotlib not available)"""
+        try:
+            import matplotlib
+            matplotlib.use("Agg")
+            import matplotlib.pyplot as plt
+
+            xs = [p[0] for p in self._poses]
+            ys = [p[1] for p in self._poses]
+
+            fig, ax = plt.subplots(figsize=(6, 6))
+            ax.plot(xs, ys, "b-", linewidth=0.8, label="trajectory")
+            ax.plot(xs[0], ys[0], "go", label="start")
+            ax.plot(xs[-1], ys[-1], "ro", label="end")
+            ax.set_aspect("equal")
+            ax.set_xlabel("x (m)")
+            ax.set_ylabel("y (m)")
+            ax.set_title("v=%.2f m/s  (%.1f° turn)" %
+                         (lin_speed, math.degrees(self._total_angle())))
+            ax.legend()
+            ax.grid(True, alpha=0.3)
+
+            fname = os.path.join(self.data_dir, "trajectory_v%.2f.png" % lin_speed)
+            fig.savefig(fname, dpi=120)
+            plt.close(fig)
+            self.get_logger().info("  plot saved: %s" % fname)
+        except ImportError:
+            pass
+
+    def save_summary(self):
+        """Save summary CSV"""
+        fname = os.path.join(self.data_dir, "summary.csv")
+        with open(fname, "w", newline="") as f:
+            w = csv.writer(f)
+            w.writerow(["v_cmd", "w_cmd", "v_avg", "w_avg",
+                        "R_vw_m", "R_fit_m", "n_samples"])
+            for r in self.results:
+                w.writerow([r["v_cmd"], r["w_cmd"], r["v_avg"], r["w_avg"],
+                           r["R_vw"], r["R_fit"], r["n_samples"]])
+        print("\nSummary saved: %s" % fname)
+        print("Raw data dir:  %s" % self.data_dir)
+
+    def _fit_circle_radius(self):
+        if len(self._poses) < 10:
+            return None
+
+        xs = np.array([p[0] for p in self._poses])
+        ys = np.array([p[1] for p in self._poses])
+
+        A = np.column_stack([xs, ys, np.ones_like(xs)])
+        b = -(xs**2 + ys**2)
+        try:
+            sol, _, _, _ = np.linalg.lstsq(A, b, rcond=None)
+            a, b_coef, c = sol
+            r_sq = (a**2 + b_coef**2) / 4.0 - c
+            if r_sq > 0:
+                return float(np.sqrt(r_sq))
+        except np.linalg.LinAlgError:
+            pass
+        return None
+
+    def print_summary(self):
+        print("\n" + "=" * 65)
+        print("  TURNING RADIUS SUMMARY")
+        print("=" * 65)
+        print("  v_cmd   ω_cmd    v_avg    ω_avg    R(v/ω)     R(fit)")
+        print("-" * 65)
+        for r in self.results:
+            rf = "%.3f" % r["R_fit"] if r["R_fit"] > 0 else "N/A"
+            print("  %5.2f   %5.2f   %6.3f   %6.3f   %7.3f   %s" %
+                  (r["v_cmd"], r["w_cmd"], r["v_avg"], r["w_avg"],
+                   r["R_vw"], rf))
+        print("=" * 65)
+
+
+def main():
+    rclpy.init()
+
+    speeds = [0.1, 0.15, 0.2, 0.25, 0.3, 0.35]
+    if len(sys.argv) > 1:
+        try:
+            speeds = [float(a) for a in sys.argv[1:]]
+        except ValueError:
+            print("Usage: python3 measure_turning_radius.py [v1 v2 v3 ...]")
+            return
+
+    # Create data dir
+    data_dir = os.path.join(
+        os.path.expanduser("~/yiliao_ws"),
+        "turning_radius_data",
+        datetime.now().strftime("%Y%m%d_%H%M%S"))
+    os.makedirs(data_dir, exist_ok=True)
+
+    print("=" * 60)
+    print("  Turning Radius Measurement")
+    print("=" * 60)
+    print("  Speeds:     %s" % speeds)
+    print("  Max steer:  5.0 rad/s")
+    print("  Data dir:   %s" % data_dir)
+    print("=" * 60)
+    print("  WARNING: Large open space required (2m+)!")
+    input("  Press ENTER to start, Ctrl+C to abort...")
+
+    node = TurningRadiusMeasurer(speeds, data_dir, max_ang=5.0)
+
+    try:
+        for v in speeds:
+            node.run_test(v)
+            time.sleep(0.5)
+
+        node.print_summary()
+        node.save_summary()
+
+    except KeyboardInterrupt:
+        twist = Twist()
+        node.pub.publish(twist)
+        print("\nAborted.")
+        node.print_summary()
+        node.save_summary()
+
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

tools/set_volume.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+import subprocess, sys
+
+# ============================================
+#  修改下面这个数字即可调整音量，范围 0 ~ 100
+# ============================================
+VOLUME = 80  # 百分比
+
+SINK = "alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo"
+
+def run(cmd):
+    subprocess.run(cmd, capture_output=True, text=True)
+
+def get_volume():
+    r = subprocess.run(["pactl", "get-sink-volume", SINK],
+                       capture_output=True, text=True)
+    for line in r.stdout.splitlines():
+        if "front-left:" in line:
+            print(f"当前音量: {line.strip()}")
+            return
+    print(r.stdout.strip())
+
+if len(sys.argv) > 1:
+    try:
+        v = int(sys.argv[1])
+        if 0 <= v <= 100:
+            VOLUME = v
+        else:
+            print("音量需在 0~100 之间")
+            sys.exit(1)
+    except ValueError:
+        print(f"用法: python3 set_volume.py [0~100]")
+        sys.exit(1)
+
+print(f"设置音量为 {VOLUME}% ...")
+run(["pactl", "set-sink-volume", SINK, f"{VOLUME}%"])
+
+# 有硬件 PCM 则同步设置
+r = subprocess.run(["amixer", "-c", "1", "set", "PCM", f"{VOLUME}%", "unmute"],
+                   capture_output=True, text=True)
+if r.returncode == 0:
+    print("已同步硬件 PCM")
+
+get_volume()

tools/udp_to_cmdvel.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+"""
+UDP → /cmd_vel bridge for keyboard control.
+Receives (float64 lin_x, float64 ang_z) from Windows client.
+
+Deploy on RDKx5: /home/sunrise/yiliao_ws/udp_to_cmdvel.py
+Run: python3 udp_to_cmdvel.py
+"""
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+import socket
+import struct
+import sys
+
+
+class UDPToCmdVel(Node):
+    def __init__(self, port=9999):
+        super().__init__("udp_to_cmdvel")
+        self.pub = self.create_publisher(Twist, "/cmd_vel", 10)
+        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        self.sock.bind(("0.0.0.0", port))
+        self.sock.settimeout(0.1)
+        self.get_logger().info("Listening on UDP port %d → /cmd_vel" % port)
+        self.create_timer(0.05, self.loop)
+
+    def loop(self):
+        try:
+            data, addr = self.sock.recvfrom(16)
+            lin, ang = struct.unpack('dd', data)
+            msg = Twist()
+            msg.linear.x = lin
+            msg.angular.z = ang
+            self.pub.publish(msg)
+        except socket.timeout:
+            pass
+
+    def destroy_node(self):
+        self.sock.close()
+        super().destroy_node()
+
+
+def main():
+    rclpy.init()
+    port = int(sys.argv[1]) if len(sys.argv) > 1 else 9999
+    node = UDPToCmdVel(port=port)
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

tools/windows_keyboard_control.py

@@ -0,0 +1,137 @@
+#!/usr/bin/env python3
+"""
+Windows keyboard control → UDP → RDKx5
+Simultaneous keys supported (W+A = forward-left, etc.)
+
+Usage:
+    1. pip install keyboard
+    2. Run this script on Windows
+    3. RDKx5 must be running udp_to_cmdvel node
+
+Controls:
+    W/S      : forward / backward
+    A/D      : turn left / right
+    Q/E      : left-forward / right-forward (single-key diagonal)
+    Z/C      : left-backward / right-backward
+    Space    : emergency stop
+    I/K      : linear  speed +/-
+    J/L      : angular speed +/-
+    Esc      : quit
+"""
+
+import keyboard
+import socket
+import struct
+import time
+import sys
+
+# ========== CONFIG ==========
+ROBOT_IP = "192.168.10.210"
+UDP_PORT = 9999
+# ============================
+
+HELP = """
+========================================
+  Windows Keyboard Control → RDKx5
+========================================
+  W/S     : forward / backward
+  A/D     : turn left / right
+  Q/E     : left-forward / right-forward
+  Z/C     : left-backward / right-backward
+  Space   : STOP
+  I/K     : linear  speed +/- (step 0.05)
+  J/L     : angular speed +/- (step 0.1)
+  Esc     : quit
+========================================
+  Current: lin=%.2f  ang=%.2f
+"""
+
+
+def main():
+    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+    lin_speed = 0.2
+    ang_speed = 0.5
+    lin_step = 0.05
+    ang_step = 0.1
+
+    def print_help():
+        print(HELP % (lin_speed, ang_speed))
+
+    print("Connecting to %s:%d ..." % (ROBOT_IP, UDP_PORT))
+    print_help()
+
+    while True:
+        if keyboard.is_pressed('esc'):
+            sock.sendto(struct.pack('dd', 0.0, 0.0), (ROBOT_IP, UDP_PORT))
+            print("\nQuit.")
+            break
+
+        # Speed adjustments (one-shot, debounced by keyboard library)
+        if keyboard.is_pressed('i'):
+            lin_speed = round(lin_speed + lin_step, 2)
+            print_help()
+            time.sleep(0.15)
+            continue
+        if keyboard.is_pressed('k'):
+            lin_speed = round(max(0.0, lin_speed - lin_step), 2)
+            print_help()
+            time.sleep(0.15)
+            continue
+        if keyboard.is_pressed('j'):
+            ang_speed = round(ang_speed + ang_step, 2)
+            print_help()
+            time.sleep(0.15)
+            continue
+        if keyboard.is_pressed('l'):
+            ang_speed = round(max(0.0, ang_speed - ang_step), 2)
+            print_help()
+            time.sleep(0.15)
+            continue
+
+        # Emergency stop
+        if keyboard.is_pressed('space'):
+            sock.sendto(struct.pack('dd', 0.0, 0.0), (ROBOT_IP, UDP_PORT))
+            print("*** STOP ***")
+            time.sleep(0.05)
+            continue
+
+        # Combined movement: multi-key support
+        lin = 0.0
+        ang = 0.0
+
+        if keyboard.is_pressed('w'):
+            lin += lin_speed
+        if keyboard.is_pressed('s'):
+            lin -= lin_speed
+        if keyboard.is_pressed('a'):
+            ang += ang_speed
+        if keyboard.is_pressed('d'):
+            ang -= ang_speed
+
+        # Single-key diagonals (override if used)
+        if keyboard.is_pressed('q'):
+            lin, ang = lin_speed, ang_speed
+        if keyboard.is_pressed('e'):
+            lin, ang = lin_speed, -ang_speed
+        if keyboard.is_pressed('z'):
+            lin, ang = -lin_speed, ang_speed
+        if keyboard.is_pressed('c'):
+            lin, ang = -lin_speed, -ang_speed
+
+        sock.sendto(struct.pack('dd', round(lin, 3), round(ang, 3)),
+                    (ROBOT_IP, UDP_PORT))
+
+        time.sleep(0.05)  # 20Hz
+
+
+if __name__ == "__main__":
+    print("Installing keyboard library if needed...")
+    try:
+        import keyboard
+    except ImportError:
+        print("ERROR: 'keyboard' library not installed.")
+        print("Run: pip install keyboard")
+        sys.exit(1)
+
+    main()

vlm_server.py

@@ -0,0 +1,207 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+VLM 图生文推理服务 — AndesVL-1B (千问体系)
+为 RDK X5 提供 OpenAI 兼容 API（纯 CPU 推理）
+
+启动方式: python vlm_server.py
+API 地址: http://192.168.10.210:8000
+"""
+
+import base64
+import io
+import time
+import uuid
+import logging
+import sys
+from contextlib import asynccontextmanager
+
+import torch
+import uvicorn
+from fastapi import FastAPI, HTTPException
+from fastapi.middleware.cors import CORSMiddleware
+from pydantic import BaseModel
+from PIL import Image
+from transformers import AutoModel, AutoTokenizer, AutoImageProcessor
+
+# ========== 配置 ==========
+MODEL_PATH = "/home/root/models/AndesVL-1B-Instruct"   # RDK X5 上模型路径
+HOST = "0.0.0.0"
+PORT = 8000
+
+# ========== 日志 ==========
+logging.basicConfig(
+    level=logging.INFO,
+    format="%(asctime)s [%(levelname)s] %(message)s",
+    handlers=[logging.StreamHandler(sys.stdout)],
+)
+logger = logging.getLogger("vlm_server")
+
+# ========== 全局变量 ==========
+model = None
+tokenizer = None
+image_processor = None
+
+
+def load_model():
+    """加载 AndesVL-1B 模型"""
+    global model, tokenizer, image_processor
+
+    logger.info(f"模型路径: {MODEL_PATH}")
+    logger.info("加载 tokenizer & image processor...")
+    tokenizer = AutoTokenizer.from_pretrained(MODEL_PATH, trust_remote_code=True)
+    image_processor = AutoImageProcessor.from_pretrained(
+        MODEL_PATH, trust_remote_code=True
+    )
+
+    logger.info("加载 AndesVL-1B 模型 (fp32 CPU, 约 18 秒)...")
+    t0 = time.time()
+    model = AutoModel.from_pretrained(
+        MODEL_PATH,
+        trust_remote_code=True,
+        torch_dtype=torch.float32,
+        low_cpu_mem_usage=True,
+    )
+    model.eval()
+    logger.info(f"模型加载完成，耗时 {time.time() - t0:.1f}s")
+    logger.info("AndesVL-1B 推理服务就绪！")
+
+
+@asynccontextmanager
+async def lifespan(app: FastAPI):
+    load_model()
+    yield
+
+
+# ========== FastAPI 应用 ==========
+app = FastAPI(title="VLM Server (AndesVL-1B)", lifespan=lifespan)
+
+app.add_middleware(
+    CORSMiddleware,
+    allow_origins=["*"],
+    allow_credentials=True,
+    allow_methods=["*"],
+    allow_headers=["*"],
+)
+
+
+# ========== Pydantic 模型 (OpenAI 格式) ==========
+class ImageUrl(BaseModel):
+    url: str
+
+
+class ContentPart(BaseModel):
+    type: str
+    text: str | None = None
+    image_url: ImageUrl | None = None
+
+
+class Message(BaseModel):
+    role: str
+    content: str | list[ContentPart]
+
+
+class ChatCompletionRequest(BaseModel):
+    model: str
+    messages: list[Message]
+    max_tokens: int = 100
+    temperature: float | None = None
+
+
+# ========== 辅助函数 ==========
+def decode_base64_image(data_url: str) -> Image.Image:
+    """从 data URL 解码图像"""
+    if "," in data_url:
+        base64_str = data_url.split(",", 1)[1]
+    else:
+        base64_str = data_url
+    image_bytes = base64.b64decode(base64_str)
+    return Image.open(io.BytesIO(image_bytes)).convert("RGB")
+
+
+def build_messages(messages: list[Message]) -> list[dict]:
+    """OpenAI 格式 → AndesVL chat 格式"""
+    result = []
+    for msg in messages:
+        if isinstance(msg.content, str):
+            result.append({"role": msg.role, "content": msg.content})
+        else:
+            content_list = []
+            for part in msg.content:
+                if part.type == "text" and part.text:
+                    content_list.append({"type": "text", "text": part.text})
+                elif part.type == "image_url" and part.image_url:
+                    image = decode_base64_image(part.image_url.url)
+                    content_list.append({"type": "image", "image": image})
+            result.append({"role": msg.role, "content": content_list})
+    return result
+
+
+# ========== API 路由 ==========
+@app.get("/v1/models")
+async def list_models():
+    return {
+        "object": "list",
+        "data": [
+            {
+                "id": "./OpenGVLab/InternVL3-1B/",
+                "object": "model",
+                "created": 1700000000,
+                "owned_by": "local",
+            }
+        ],
+    }
+
+
+@app.post("/v1/chat/completions")
+async def chat_completions(request: ChatCompletionRequest):
+    try:
+        messages = build_messages(request.messages)
+        logger.info(f"收到请求, model={request.model}, max_tokens={request.max_tokens}")
+
+        start_time = time.time()
+        response_text = model.chat(
+            messages,
+            tokenizer,
+            image_processor,
+            max_new_tokens=request.max_tokens,
+        )
+        elapsed = time.time() - start_time
+        logger.info(f"推理完成，耗时 {elapsed:.1f}s, 结果: {response_text[:50]}...")
+
+        return {
+            "id": f"chatcmpl-{uuid.uuid4().hex[:12]}",
+            "object": "chat.completion",
+            "created": int(time.time()),
+            "model": request.model,
+            "choices": [
+                {
+                    "index": 0,
+                    "message": {"role": "assistant", "content": response_text},
+                    "finish_reason": "stop",
+                }
+            ],
+            "usage": {
+                "prompt_tokens": 0,
+                "completion_tokens": 0,
+                "total_tokens": 0,
+            },
+        }
+
+    except Exception as e:
+        logger.error(f"推理出错: {e}", exc_info=True)
+        raise HTTPException(status_code=500, detail=str(e))
+
+
+@app.get("/health")
+async def health():
+    return {"status": "ok", "model_loaded": model is not None}
+
+
+# ========== 入口 ==========
+if __name__ == "__main__":
+    logger.info("=" * 50)
+    logger.info("VLM 图生文推理服务启动中...")
+    logger.info(f"监听: http://{HOST}:{PORT}")
+    logger.info("=" * 50)
+    uvicorn.run(app, host=HOST, port=PORT, log_level="info")

调试记录.md

@@ -167,3 +167,16 @@ Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
 
 【验证】
 编译通过。需要重启 lslidar_driver_node 使修复生效。
+
+
+## 6/16 调试记录
+记录了深度相机的RGB图像最远能够识别的大小（目前A4）
+![](调试记录.Assets/1.png)
+
+
+## 6/18 调试记录
+创建了电脑遥控的脚本，测试了不同速度下小车转弯半径的大小
+修改了雷达驱动脚本，从软件上修复了重复掉线重连的情况
+
+## 6/20 调试记录
+所有工具/辅助类脚本全部放在tools文件夹下