From a9840c38ef2fe35ab7c8cb43416fc48c95a05210 Mon Sep 17 00:00:00 2001 From: ywb <347742090@qq.com> Date: Sat, 16 May 2026 14:22:10 +0800 Subject: [PATCH] =?UTF-8?q?Nav2=E5=AF=BC=E8=88=AA=20+=20=E5=88=9D=E5=A7=8B?= =?UTF-8?q?=E5=8C=96=E4=BD=8D=E7=BD=AE=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - nav2_navigator.py: 通过 ros2 action 与 Nav2 通信，修复 YAML 格式（pose 嵌套） - mission_executor.py: 改用 Nav2 navigate_to_pose action 导航 - app.py: navigate/to 等 API 改用 Nav2Navigator；新增 /api/mission/init_pose API，通过子进程调用 rclpy 发布 /initialpose 到 (0,0,0) - setting.html/js: 任务配置Tab加「初始化位置」按钮 注意：ROS2 daemon (domain=1) 与 Flask 在不同 domain， Flask 进程内调用 rclpy 会破坏 daemon 状态，需通过子进程调用 --- agv_app/app.py | 27 +++++++++++++ agv_app/static/js/setting.js | 17 ++++++++ agv_app/templates/setting.html | 3 +- agv_app/utils/mission_executor.py | 21 +++++----- agv_app/utils/nav2_navigator.py | 64 ++++++++++++++++--------------- 5 files changed, 91 insertions(+), 41 deletions(-) diff --git a/agv_app/app.py b/agv_app/app.py index b518b41..2cb7cf2 100644 --- a/agv_app/app.py +++ b/agv_app/app.py @@ -641,6 +641,33 @@ def api_mission_position(): return jsonify({"ok": True, "position": pos, "battery": battery}) +@app.route("/api/mission/init_pose", methods=["POST"]) +def api_mission_init_pose(): + """将 AMCL 初始位置设为 (0,0,0)，无需 RViz""" + try: + script = "/tmp/ros2_init_pose.sh" + lines = [ + "#!/bin/bash", + "export ROS_DOMAIN_ID=1", + "source /opt/ros/humble/setup.bash", + "source ~/agv_pro_ros2/install/setup.bash", + "python3 /tmp/publish_init_pose.py", + ] + with open(script, "w") as f: + f.write("\n".join(lines) + "\n") + os.chmod(script, 0o755) + result = subprocess.run( + [script], + capture_output=True, text=True, timeout=12, + env={**os.environ, "ROS_DOMAIN_ID": "1"} + ) + logger.info(f"init_pose: rc={result.returncode}") + return jsonify({"ok": True, "message": "初始位置已设为 (0,0,0)"}) + except Exception as e: + logger.error(f"初始化位置失败: {e}") + return jsonify({"ok": False, "error": str(e)}), 500 + + @app.route("/api/mission/config", methods=["GET"]) def api_mission_config_get(): """获取任务配置（网格尺寸和空位矩阵）""" diff --git a/agv_app/static/js/setting.js b/agv_app/static/js/setting.js index 6219b54..4604ad2 100644 --- a/agv_app/static/js/setting.js +++ b/agv_app/static/js/setting.js @@ -50,6 +50,7 @@ const app = createApp({ agvMoveInterval: null, agvCameraUrl: API + '/api/camera/refresh', agvCameraTimer: null, + initPoseLoading: false, } }, mounted() { @@ -336,6 +337,22 @@ const app = createApp({ } } catch (e) { alert('保存失败: ' + e.message) } }, + async initPose() { + try { + this.initPoseLoading = true + const res = await fetch(API + '/api/mission/init_pose', { method: 'POST' }) + const data = await res.json() + if (data.ok) { + alert('✅ 初始位置已设为 (0, 0, 0)') + } else { + alert('❌ 初始化失败: ' + (data.error || '未知错误')) + } + } catch (e) { + alert('❌ 初始化位置请求失败: ' + e.message) + } finally { + this.initPoseLoading = false + } + }, async loadAllMachines() { try { const res = await fetch(API + '/api/mission/machines') diff --git a/agv_app/templates/setting.html b/agv_app/templates/setting.html index e1b2b23..f511b5a 100644 --- a/agv_app/templates/setting.html +++ b/agv_app/templates/setting.html @@ -242,6 +242,7 @@
+
@@ -540,6 +541,6 @@ - + diff --git a/agv_app/utils/mission_executor.py b/agv_app/utils/mission_executor.py index b6ca4eb..2e531fa 100644 --- a/agv_app/utils/mission_executor.py +++ b/agv_app/utils/mission_executor.py @@ -250,15 +250,16 @@ class MissionExecutor: f" sec: 0\n" f" nanosec: 0\n" f" frame_id: 'map'\n" - f" position:\n" - f" x: {x}\n" - f" y: {y}\n" - f" z: 0.0\n" - f" orientation:\n" - f" x: 0.0\n" - f" y: 0.0\n" - f" z: {qz}\n" - f" w: {qw}" + f" pose:\n" + f" position:\n" + f" x: {x}\n" + f" y: {y}\n" + f" z: 0.0\n" + f" orientation:\n" + f" x: 0.0\n" + f" y: 0.0\n" + f" z: {qz}\n" + f" w: {qw}" ) logger.info(f"Nav2 发送导航目标: ({x:.3f}, {y:.3f}), yaw={math.degrees(yaw):.1f}°") @@ -266,7 +267,7 @@ class MissionExecutor: # 发送 goal 并监听反馈和结果 cmd = ( f"ros2 action send_goal /navigate_to_pose " - f"navigation_action_msgs/NavigateToPose " + f"nav2_msgs/action/NavigateToPose " f"'{pose_yaml}' " f"--feedback" ) diff --git a/agv_app/utils/nav2_navigator.py b/agv_app/utils/nav2_navigator.py index f3506ab..1d85351 100644 --- a/agv_app/utils/nav2_navigator.py +++ b/agv_app/utils/nav2_navigator.py @@ -17,7 +17,7 @@ from enum import Enum logger = logging.getLogger(__name__) # ROS2 环境设置（与 agv_controller_ros2.py 保持一致） -ROS2_SETUP_CMD = "source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash" +ROS2_SETUP_CMD = "export ROS_DOMAIN_ID=1 && source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash" class Nav2Status(Enum): @@ -174,21 +174,22 @@ class Nav2Navigator: f" sec: 0\\n" f" nanosec: 0\\n" f" frame_id: 'map'\\n" - f" position:\\n" - f" x: {x}\\n" - f" y: {y}\\n" - f" z: 0.0\\n" - f" orientation:\\n" - f" x: 0.0\\n" - f" y: 0.0\\n" - f" z: {qz}\\n" - f" w: {qw}" + f" pose:\\n" + f" position:\\n" + f" x: {x}\\n" + f" y: {y}\\n" + f" z: 0.0\\n" + f" orientation:\\n" + f" x: 0.0\\n" + f" y: 0.0\\n" + f" z: {qz}\\n" + f" w: {qw}" ) # 发送 goal，保持连接获取结果 cmd = ( f"ros2 action send_goal /navigate_to_pose " - f"navigation_action_msgs/NavigateToPose " + f"nav2_msgs/action/NavigateToPose " f"'{pose_yaml}' " f"--feedback" ) @@ -309,15 +310,16 @@ class Nav2Navigator: f" sec: 0\\n" f" nanosec: 0\\n" f" frame_id: 'map'\\n" - f" position:\\n" - f" x: {x}\\n" - f" y: {y}\\n" - f" z: 0.0\\n" - f" orientation:\\n" - f" x: 0.0\\n" - f" y: 0.0\\n" - f" z: {qz}\\n" - f" w: {qw}" + f" pose:\\n" + f" position:\\n" + f" x: {x}\\n" + f" y: {y}\\n" + f" z: 0.0\\n" + f" orientation:\\n" + f" x: 0.0\\n" + f" y: 0.0\\n" + f" z: {qz}\\n" + f" w: {qw}" ) poses_yaml = "poses:\n" + "\n".join(poses_yaml_parts) @@ -354,22 +356,24 @@ class Nav2Navigator: f" sec: 0\\n" f" nanosec: 0\\n" f" frame_id: 'map'\\n" - f" position:\\n" - f" x: {x}\\n" - f" y: {y}\\n" - f" z: 0.0\\n" - f" orientation:\\n" - f" x: 0.0\\n" - f" y: 0.0\\n" - f" z: {qz}\\n" - f" w: {qw}" + f" pose:\\n" + f" position:\\n" + f" x: {x}\\n" + f" y: {y}\ +" + f" z: 0.0\\n" + f" orientation:\\n" + f" x: 0.0\\n" + f" y: 0.0\\n" + f" z: {qz}\\n" + f" w: {qw}" ) poses_yaml = "poses:\n" + "\n".join(poses_yaml_parts) cmd = ( f"ros2 action send_goal /navigate_through_poses " - f"navigation_action_msgs/NavigateThroughPoses " + f"nav2_msgs/action/NavigateThroughPoses " f"'{poses_yaml}' " f"--feedback" )