diff --git a/.gitignore b/.gitignore index e260e7d..e694731 100644 --- a/.gitignore +++ b/.gitignore @@ -10,3 +10,4 @@ log # VSCode database .vscode +datas diff --git a/README.md b/README.md index 039630c..0d163fd 100644 --- a/README.md +++ b/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`：用于板端接收键盘遥控命令（与上一个文件联动） diff --git a/bashes/auto-wifi-connect.service b/bashes/auto-wifi-connect.service new file mode 100644 index 0000000..7be630e --- /dev/null +++ b/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 diff --git a/bashes/auto-wifi-connect.sh b/bashes/auto-wifi-connect.sh new file mode 100755 index 0000000..36762b9 --- /dev/null +++ b/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 diff --git a/keyboard_control.py b/keyboard_control.py deleted file mode 100755 index 3f9ec01..0000000 --- a/keyboard_control.py +++ /dev/null @@ -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() diff --git a/my_model/image.png b/my_model/image.png new file mode 100644 index 0000000..2ce763e Binary files /dev/null and b/my_model/image.png differ diff --git a/path_follower_demo.py b/path_follower_demo.py new file mode 100755 index 0000000..03536d0 --- /dev/null +++ b/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() diff --git a/scripts/PIDtracking.py b/scripts/PIDtracking.py new file mode 100755 index 0000000..eadd8ce --- /dev/null +++ b/scripts/PIDtracking.py @@ -0,0 +1,464 @@ +#!/usr/bin/env python3 +""" +PIDtracking — 跟踪数据录制 + 可视化一体脚本 + +用法: + 录制: python3 PIDtracking.py record + 制图: python3 PIDtracking.py plot <数据目录> + +输出目录: ~/yiliao_ws/datas/PIDtracking// +输出文件: + plan.csv — 路径点 (x, y, yaw) + cmd_vel.csv — 控制命令 (t, vx, vz) + odom.csv — 里程计位姿 (t, x, y, yaw) + goal_pose.csv — 目标点 (x, y) + meta.csv — 录制参数 + +制图输出: + /plots/01_trajectory.png — 轨迹对比 + /plots/02_cmd_vel.png — 控制命令时间序列 + /plots/03_errors.png — 横向/航向误差 + /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 ") + 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 ") + sys.exit(1) diff --git a/scripts/__pycache__/publish_sine_path.cpython-310.pyc b/scripts/__pycache__/publish_sine_path.cpython-310.pyc new file mode 100644 index 0000000..9184b23 Binary files /dev/null and b/scripts/__pycache__/publish_sine_path.cpython-310.pyc differ diff --git a/scripts/publish_sine_path.py b/scripts/publish_sine_path.py new file mode 100755 index 0000000..1537a5c --- /dev/null +++ b/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() diff --git a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml b/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml index 9ebc641..4d4e494 100644 --- a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml +++ b/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_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 - + - 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.4 - 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: 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 - + plugin: nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + desired_linear_vel: 0.3 + max_angular_vel: 1.9 + min_angular_vel: 0.2 + lookahead_dist: 0.5 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.8 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 0.8 + transform_tolerance: 0.5 + use_velocity_scaled_lookahead_dist: True + min_approach_linear_vel: 0.1 + approach_velocity_scaling_dist: 0.5 + use_collision_detection: True + max_allowed_time_to_collision: 1.0 + use_cost_regulated_linear_velocity_scaling: True + cost_scaling_dist: 0.6 + cost_scaling_gain: 1.0 + regulated_linear_scaling_min_radius: 0.35 + regulated_linear_scaling_min_speed: 0.1 + use_rotate_to_heading: True + rotate_to_heading_min_angle: 0.5 + max_robot_pose_search_dist: 10.0 + use_interpolation: False + allow_reversing: False 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], - [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 - 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 - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + track_unknown_space: True + 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" - downsample_costmap: false + plugin: nav2_smac_planner/SmacPlannerHybrid + 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 - reverse_penalty: 1.3 # 后退惩罚，越小越愿意后退（默认 2.0，设为 1.3 允许灵活倒车） + minimum_turning_radius: 0.35 + 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 - viz_expansions: false + cache_obstacle_heuristic: 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 - refinement_num: 2 - + 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"] + 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 - waypoint_task_executor_plugin: "wait_at_waypoint" + stop_on_failure: False + 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 diff --git a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak b/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak index 31f37c5..9ebc641 100644 --- a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak +++ b/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 - cost_penalty: 2.0 + non_straight_penalty: 0.5 + 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: diff --git a/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak2 b/src/gc_navigation2_slamtoolbox/params/gc_navigation_slam.yaml.bak2 new file mode 100644 index 0000000..b7aad0c --- /dev/null +++ b/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 diff --git a/src/origincar_base/config/ekf.yaml b/src/origincar_base/config/ekf.yaml index bd1d6e9..b9513f6 100644 --- a/src/origincar_base/config/ekf.yaml +++ b/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 diff --git a/src/origincar_base/config/ekf.yaml.bak b/src/origincar_base/config/ekf.yaml.bak new file mode 100644 index 0000000..bd1d6e9 --- /dev/null +++ b/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] diff --git a/src/origincar_base/launch/base_serial.launch.py b/src/origincar_base/launch/base_serial.launch.py index 5093bed..f003eef 100644 --- a/src/origincar_base/launch/base_serial.launch.py +++ b/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'}], - 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'}], + parameters=robot_parameters, ) ]) diff --git a/src/origincar_base/launch/base_serial.launch.py.bak b/src/origincar_base/launch/base_serial.launch.py.bak new file mode 100644 index 0000000..5093bed --- /dev/null +++ b/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'}], + ) + ]) diff --git a/src/origincar_base/launch/origincar_bringup.launch.py b/src/origincar_base/launch/origincar_bringup.launch.py index 0b56728..10d485a 100644 --- a/src/origincar_base/launch/origincar_bringup.launch.py +++ b/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=差速)') diff --git a/src/past_control/CMakeLists.txt b/src/past_control/CMakeLists.txt new file mode 100644 index 0000000..3807393 --- /dev/null +++ b/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() diff --git a/src/past_control/config/past_control.yaml b/src/past_control/config/past_control.yaml new file mode 100644 index 0000000..834cfff --- /dev/null +++ b/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 diff --git a/src/past_control/include/past_control/tools.h b/src/past_control/include/past_control/tools.h new file mode 100644 index 0000000..6fb9d0b --- /dev/null +++ b/src/past_control/include/past_control/tools.h @@ -0,0 +1,87 @@ +#ifndef PAST_CONTROL__TOOLS_H +#define PAST_CONTROL__TOOLS_H + +#include +#include + +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 diff --git a/src/past_control/launch/past_control.launch.py b/src/past_control/launch/past_control.launch.py new file mode 100644 index 0000000..10cd2e1 --- /dev/null +++ b/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, + ]) diff --git a/src/past_control/msg/Obstacle.msg b/src/past_control/msg/Obstacle.msg new file mode 100644 index 0000000..84b2996 --- /dev/null +++ b/src/past_control/msg/Obstacle.msg @@ -0,0 +1,5 @@ +std_msgs/Header header +float64 x +float64 y +float64 radius +string type diff --git a/src/past_control/msg/ObstacleArray.msg b/src/past_control/msg/ObstacleArray.msg new file mode 100644 index 0000000..39bcba2 --- /dev/null +++ b/src/past_control/msg/ObstacleArray.msg @@ -0,0 +1 @@ +Obstacle[] obstacles diff --git a/src/past_control/package.xml b/src/past_control/package.xml new file mode 100644 index 0000000..eea06ce --- /dev/null +++ b/src/past_control/package.xml @@ -0,0 +1,36 @@ + + + + past_control + 0.0.0 + Racing control stack: lane follower, obstacle detector, and racing orchestrator (Nav2 /plan subscriber + PID following) + sunrise + TODO: License declaration + + ament_cmake + + rosidl_default_generators + rosidl_interface_packages + + rclcpp + rclcpp_action + std_msgs + nav_msgs + nav2_msgs + geometry_msgs + sensor_msgs + tf2 + tf2_ros + visualization_msgs + origincar_msg + builtin_interfaces + + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/past_control/src/lane_follower_node.cpp b/src/past_control/src/lane_follower_node.cpp new file mode 100644 index 0000000..380d648 --- /dev/null +++ b/src/past_control/src/lane_follower_node.cpp @@ -0,0 +1,81 @@ +#include +#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( + "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( + "/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::SharedPtr track_sub_; + rclcpp::Publisher::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()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/past_control/src/obstacle_detector_node.cpp b/src/past_control/src/obstacle_detector_node.cpp new file mode 100644 index 0000000..e545ef3 --- /dev/null +++ b/src/past_control/src/obstacle_detector_node.cpp @@ -0,0 +1,222 @@ +#include +#include +#include +#include + +#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( + "hobot_dnn_detection", 10, + std::bind(&ObstacleDetectorNode::dnnCallback, this, std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + "/odom", 10, + std::bind(&ObstacleDetectorNode::odomCallback, this, std::placeholders::_1)); + + // Publishers + obstacles_pub_ = this->create_publisher( + "/obstacles", 10); + + marker_pub_ = this->create_publisher( + "/obstacle_markers", 10); + + double rate = this->get_parameter("publish_rate").as_double(); + int period_ms = static_cast(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 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(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 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 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::SharedPtr dnn_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + + // Publishers + rclcpp::Publisher::SharedPtr obstacles_pub_; + rclcpp::Publisher::SharedPtr marker_pub_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // State + std::vector 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()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/past_control/src/racing_orchestrator.cpp b/src/past_control/src/racing_orchestrator.cpp new file mode 100644 index 0000000..6543f45 --- /dev/null +++ b/src/past_control/src/racing_orchestrator.cpp @@ -0,0 +1,448 @@ +#include +#include +#include +#include + +#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; + + 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( + "sign4return", 10, + std::bind(&RacingOrchestrator::signCallback, this, std::placeholders::_1)); + + sign_foxglove_sub_ = this->create_subscription( + "sign_foxglove", 10, + std::bind(&RacingOrchestrator::signFoxgloveCallback, this, std::placeholders::_1)); + + path_sub_ = this->create_subscription( + "/plan", 10, + std::bind(&RacingOrchestrator::pathCallback, this, std::placeholders::_1)); + + goal_sub_ = this->create_subscription( + "/goal_pose", 10, + std::bind(&RacingOrchestrator::goalCallback, this, std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + "/odom", 10, + std::bind(&RacingOrchestrator::odomCallback, this, std::placeholders::_1)); + + lane_cmd_sub_ = this->create_subscription( + "/lane_cmd_vel", 10, + std::bind(&RacingOrchestrator::laneCmdCallback, this, std::placeholders::_1)); + + // Publishers + cmd_vel_pub_ = this->create_publisher("/cmd_vel", 10); + + // ComputePathToPose action client (triggers Nav2 planner_server) + planner_client_ = rclcpp_action::create_client( + this, "compute_path_to_pose"); + + // Control loop + double rate = this->get_parameter("control_rate").as_double(); + int period_ms = static_cast(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 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 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 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 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::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 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(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(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::SharedPtr sign_sub_; + rclcpp::Subscription::SharedPtr sign_foxglove_sub_; + rclcpp::Subscription::SharedPtr path_sub_; + rclcpp::Subscription::SharedPtr goal_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr lane_cmd_sub_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp_action::Client::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()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/planner/CMakeLists.txt b/src/planner/CMakeLists.txt new file mode 100644 index 0000000..443d085 --- /dev/null +++ b/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() diff --git a/src/planner/config/planner.yaml b/src/planner/config/planner.yaml new file mode 100644 index 0000000..31c2ffd --- /dev/null +++ b/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 diff --git a/src/planner/launch/planner.launch.py b/src/planner/launch/planner.launch.py new file mode 100644 index 0000000..aa7669f --- /dev/null +++ b/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 diff --git a/src/planner/package.xml b/src/planner/package.xml new file mode 100644 index 0000000..bf4169c --- /dev/null +++ b/src/planner/package.xml @@ -0,0 +1,26 @@ + + + + planner + 0.0.0 + Minimal Nav2 global planning stack: slam_toolbox + planner_server + costmaps + lifecycle_manager + sunrise + TODO: License declaration + + ament_cmake + + rclcpp + slam_toolbox + nav2_planner + nav2_costmap_2d + nav2_lifecycle_manager + nav2_common + nav2_util + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/planner/src/planner_version.cpp b/src/planner/src/planner_version.cpp new file mode 100644 index 0000000..dc219bf --- /dev/null +++ b/src/planner/src/planner_version.cpp @@ -0,0 +1,11 @@ +#include + +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; +} diff --git a/src/qr_detection/src/qr_dete_depth.cpp b/src/qr_detection/src/qr_dete_depth.cpp index 364ec40..c572c73 100644 --- a/src/qr_detection/src/qr_dete_depth.cpp +++ b/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_ = - this->create_subscription( - "sign4return", - 10, - std::bind(&MinimalHbmemSubscriber::sign_callback, this, std::placeholders::_1)); + // subscription_sign_ = + // this->create_subscription( + // "sign4return", + // 10, + // std::bind(&MinimalHbmemSubscriber::sign_callback, this, std::placeholders::_1)); + // 创建发布器，发布 'sign4return' 话题 + sign_publisher_ = this->create_publisher("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) - { - if (msg->data == 0) - { - detect_qr_code_ = true; // 启动二维码检测 - RCLCPP_INFO(this->get_logger(), "QR detection started"); - } - else if (msg->data == 5) - { - detect_qr_code_ = false; // 停止二维码检测 - RCLCPP_INFO(this->get_logger(), "QR detection stopped"); - } + // void sign_callback(const std_msgs::msg::Int32::SharedPtr msg) + // { + // if (msg->data == 0) + // { + // detect_qr_code_ = true; // 启动二维码检测 + // RCLCPP_INFO(this->get_logger(), "QR detection started"); + // } + // else if (msg->data == 5) + // { + // detect_qr_code_ = false; // 停止二维码检测 + // RCLCPP_INFO(this->get_logger(), "QR detection stopped"); + // } + // } + + // 识别结果合法性检测 + bool ResultLegal(std::string& input){ + // 合法字符串集合 + static const std::unordered_set kValidStrings = { + "1", "2", "顺时针", "逆时针", "顺", "逆" + }; + return kValidStrings.find(input) != kValidStrings.end(); } + // /aurora/rgb/image_raw 订阅器 rclcpp::Subscription::SharedPtr subscription_image_; // sign4return 订阅器 - rclcpp::Subscription::SharedPtr subscription_sign_; + // rclcpp::Subscription::SharedPtr subscription_sign_; + // sign4return 发布器 + rclcpp::Publisher::SharedPtr sign_publisher_; // QR code results 发布器 rclcpp::Publisher::SharedPtr publisher_; diff --git a/src/racing_control/CMakeLists.txt b/src/racing_control/CMakeLists.txt new file mode 100644 index 0000000..301be46 --- /dev/null +++ b/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( 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() diff --git a/src/racing_control/include/racing_control/racing_control.hpp b/src/racing_control/include/racing_control/racing_control.hpp new file mode 100644 index 0000000..d2b8b2d --- /dev/null +++ b/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_; +}; + + diff --git a/src/racing_control/package.xml b/src/racing_control/package.xml new file mode 100644 index 0000000..e5f978f --- /dev/null +++ b/src/racing_control/package.xml @@ -0,0 +1,18 @@ + + + + racing_control + 0.0.0 + TODO: Package description + sunrise + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/racing_control/src/racing_control.cpp b/src/racing_control/src/racing_control.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/vlm_detect/setup.py b/src/vlm_detect/setup.py index bc6e34e..cb4a7ca 100644 --- a/src/vlm_detect/setup.py +++ b/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', ], }, ) diff --git a/src/vlm_detect/vlm_detect/__pycache__/__init__.cpython-310.pyc b/src/vlm_detect/vlm_detect/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000..efe8441 Binary files /dev/null and b/src/vlm_detect/vlm_detect/__pycache__/__init__.cpython-310.pyc differ diff --git a/src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc b/src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc new file mode 100644 index 0000000..02e57c1 Binary files /dev/null and b/src/vlm_detect/vlm_detect/__pycache__/tts_node.cpython-310.pyc differ diff --git a/src/vlm_detect/vlm_detect/test_publisher.py b/src/vlm_detect/vlm_detect/test_publisher.py new file mode 100644 index 0000000..d0daf89 --- /dev/null +++ b/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() diff --git a/src/vlm_detect/vlm_detect/tts_node.py b/src/vlm_detect/vlm_detect/tts_node.py new file mode 100644 index 0000000..030ca11 --- /dev/null +++ b/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() diff --git a/src/vlm_detect/vlm_detect/vlm_node.py b/src/vlm_detect/vlm_detect/vlm_node.py index ee1eb21..5348bd1 100644 --- a/src/vlm_detect/vlm_detect/vlm_node.py +++ b/src/vlm_detect/vlm_detect/vlm_node.py @@ -15,14 +15,14 @@ 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", - api_key="EMPTY" + base_url="http://192.168.10.173:8000/v1", # 本地 API 地址 + api_key="EMPTY", # 不需要真实 API key ) - + # ROS2 组件 self.bridge = CvBridge() self.latest_image = None @@ -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 diff --git a/tools/measure_turning_radius.py b/tools/measure_turning_radius.py new file mode 100755 index 0000000..f687711 --- /dev/null +++ b/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() diff --git a/tools/set_volume.py b/tools/set_volume.py new file mode 100644 index 0000000..3a2b90d --- /dev/null +++ b/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() diff --git a/tools/udp_to_cmdvel.py b/tools/udp_to_cmdvel.py new file mode 100755 index 0000000..27c5201 --- /dev/null +++ b/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() diff --git a/tools/windows_keyboard_control.py b/tools/windows_keyboard_control.py new file mode 100644 index 0000000..e6ebe9d --- /dev/null +++ b/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() diff --git a/vlm_server.py b/vlm_server.py new file mode 100644 index 0000000..524ac78 --- /dev/null +++ b/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") diff --git a/调试记录.Assets/1.png b/调试记录.Assets/1.png new file mode 100644 index 0000000..6b00c7d Binary files /dev/null and b/调试记录.Assets/1.png differ diff --git a/调试记录.log b/调试记录.md similarity index 95% rename from 调试记录.log rename to 调试记录.md index e1c185b..4f7fac9 100644 --- a/调试记录.log +++ b/调试记录.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文件夹下