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" )