FaulknerWu/smart-inspection
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Nav2导航 + 初始化位置功能

Browse Source 
- 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 状态，需通过子进程调用
This commit is contained in:
ywb
2026-05-16 14:22:10 +08:00
parent f2130acfaa
commit a9840c38ef
5 changed files with 91 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_app/app.py
+27
View File
@@ -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():
"""获取任务配置（网格尺寸和空位矩阵）"""
agv_app/static/js/setting.js
+17
View File
@@ -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')
agv_app/templates/setting.html
+2 -1
View File
@@ -242,6 +242,7 @@
<div class="form-group" style="align-self:end">
<button class="btn btn-primary" @click="generateGrid">🔲 生成网格</button>
<button class="btn btn-secondary" @click="saveMissionConfig" style="margin-left:6px">💾 保存网格</button>
<button class="btn btn-warning" @click="initPose" :disabled="initPoseLoading" style="margin-left:6px">📍 初始化位置</button>
</div>
</div>
@@ -540,6 +541,6 @@
</div>
<script src="/static/js/vue3.global.prod.js?v=20260515a"></script>
<script src="/static/js/setting.js?v=20260515a"></script>
<script src="/static/js/setting.js?v=20260516a"></script>
</body>
</html>
agv_app/utils/mission_executor.py
+11 -10
View File
@@ -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"
)
agv_app/utils/nav2_navigator.py
+34 -30
View File
@@ -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"
)