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

可导航

Browse Source
This commit is contained in:
ywb
2026-05-16 22:58:04 +08:00
parent 22d38957f1
commit e6a8d495f7
5 changed files with 272 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_app/app.py
+9
View File
@@ -433,6 +433,15 @@ def api_navigate_stop():
return jsonify({"ok": False, "error": "导航器未初始化"}), 400
@app.route("/api/navigate/cancel", methods=["POST"])
def api_navigate_cancel():
"""取消当前导航（别名）"""
if gs.navigator:
gs.navigator.stop()
return jsonify({"ok": True, "message": "导航已取消"})
return jsonify({"ok": True, "message": "无活动导航"})
@app.route("/api/navigate/status", methods=["GET"])
def api_navigate_status():
"""获取导航状态"""
agv_app/static/js/setting.js
+81
View File
@@ -48,6 +48,11 @@ const app = createApp({
agvPosition: null,
agvSpeed: 0.5,
agvMoveInterval: null,
initPoseLoading: false,
initPoseMsg: '',
nav2Available: false,
navStatus: 'idle',
navCurrentPos: null,
agvCameraUrl: API + '/api/camera/refresh',
agvCameraTimer: null,
initPoseLoading: false,
@@ -56,6 +61,12 @@ const app = createApp({
mounted() {
this.refresh()
this.refreshAngles()
this.refreshNavStatus()
setInterval(() => {
if (this.nav2Available && this.navStatus === 'navigating') {
this.refreshNavStatus()
}
}, 3000)
},
watch: {
// 监听点位数据变化，自动刷新地图
@@ -999,6 +1010,76 @@ const app = createApp({
alert('❌ 复位请求失败: ' + e.message)
}
},
// === AMCL 初始化定位 ===
async initAmclPose() {
if (!this.agvConnected) { alert('请先连接AGV'); return }
this.initPoseLoading = true
this.initPoseMsg = ''
try {
var res = await fetch(API + '/api/mission/init_pose', { method: 'POST' })
var data = await res.json()
if (data.ok) {
this.initPoseMsg = data.message || '✅ 已初始化定位'
setTimeout(() => { this.initPoseMsg = '' }, 5000)
await this.refreshNavStatus()
} else {
alert('❌ 初始化失败: ' + (data.error || '未知错误'))
}
} catch (e) {
alert('❌ 请求失败: ' + e.message)
} finally {
this.initPoseLoading = false
}
},
// === 导航到点位 ===
async navigateToPoint() {
if (!this.pointEditor.x && !this.pointEditor.y) {
alert('该点位坐标无效，请先读取或输入坐标'); return
}
if (!this.nav2Available) { alert('Nav2 不可用，请先连接AGV并初始化定位'); return }
if (!confirm(`确定导航到点位 (${this.pointEditor.x.toFixed(2)}, ${this.pointEditor.y.toFixed(2)})？`)) return
try {
var res = await fetch(API + '/api/navigate/to', {
method: 'POST',
headers: { 'Content-Type': 'application/json' },
body: JSON.stringify({ x: this.pointEditor.x, y: this.pointEditor.y })
})
var data = await res.json()
if (data.ok) {
alert('✅ 导航已启动，请观察AGV移动')
this.closePointEdit()
this.$nextTick(() => this.refreshNavStatus())
} else {
alert('❌ 导航失败: ' + (data.error || '未知错误'))
}
} catch (e) {
alert('❌ 请求失败: ' + e.message)
}
},
// === Nav2 状态刷新 ===
async refreshNavStatus() {
try {
var res = await fetch(API + '/api/navigate/status')
var data = await res.json()
this.nav2Available = data.nav2_available
this.navStatus = data.status
this.navCurrentPos = data.current_position
} catch (e) {
this.nav2Available = false
}
},
// === 取消导航 ===
async cancelNav() {
if (!this.agvConnected) return
try {
await fetch(API + '/api/navigate/cancel', { method: 'POST' })
await this.refreshNavStatus()
} catch (e) {}
},
}
})
const vm = app.mount('#app')
agv_app/templates/setting.html
+19 -7
View File
@@ -4,7 +4,7 @@
<meta charset="UTF-8">
<meta name="viewport" content="width=device-width, initial-scale=1.0">
<title>设置 - AGV 拍摄系统</title>
<link rel="stylesheet" href="/static/css/style.css?v=20260515a">
<link rel="stylesheet" href="/static/css/style.css?v=20260517a">
</head>
<body>
<div id="app">
@@ -239,10 +239,10 @@
<label>列数 N</label>
<input type="number" v-model.number="missionConfig.cols" min="1" max="20" placeholder="4">
</div>
<div class="form-group" style="align-self:end">
<div class="form-group" style="align-self:end;display:flex;gap:6px;flex-wrap:nowrap">
<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>
<button class="btn btn-secondary" @click="saveMissionConfig">💾 保存网格</button>
<button class="btn btn-warning" @click="initPose" :disabled="initPoseLoading">📍 初始化位置</button>
</div>
</div>
@@ -446,6 +446,7 @@
<div class="btn-row">
<button class="btn btn-primary" @click="loadPointFromAgv" :disabled="!agvConnected">📍 从AGV读取</button>
<button class="btn btn-success" @click="savePoint">💾 保存</button>
<button class="btn btn-secondary" @click="navigateToPoint" :disabled="!agvConnected || !nav2Available">🚗 导航到该点位</button>
<button class="btn btn-warning" @click="clearPoint" :disabled="canClearPoint(editingPoint.pointRow, editingPoint.col)">🗑️ 清空</button>
<button class="btn btn-secondary" @click="closePointEdit">取消</button>
</div>
@@ -497,8 +498,19 @@
</div>
<div class="agv-status-bar">
<span>🔋 电压: <strong>{% raw %}{{ agvBattery !== null ? agvBattery + 'V' : '—' }}{% endraw %}</strong></span>
<span v-if="agvPosition">📍 位置: <strong>X={% raw %}{{ agvPosition[0] ? agvPosition[0].toFixed(2) : '?' }}{% endraw %} Y={% raw %}{{ agvPosition[1] ? agvPosition[1].toFixed(2) : '?' }}{% endraw %}</strong></span>
<span v-if="agvPosition">📍 位置: <strong>X={% raw %}{{ agvPosition[0] !== undefined ? agvPosition[0].toFixed(2) : '?' }}{% endraw %} Y={% raw %}{{ agvPosition[1] !== undefined ? agvPosition[1].toFixed(2) : '?' }}{% endraw %} yaw={% raw %}{{ agvPosition[2] !== undefined ? (agvPosition[2] * 180 / Math.PI).toFixed(1) : '?' }}{% endraw }}°</strong></span>
<button class="btn btn-small" @click="refreshAgvPosition">🔄 刷新</button>
<button class="btn btn-small" :class="{'btn-primary': !initPoseLoading}" :disabled="initPoseLoading" @click="initAmclPose">
{% raw %}{{ initPoseLoading ? '初始化中...' : '🎯 初始化定位' }}{% endraw %}
</button>
<span v-if="initPoseMsg" style="margin-left:8px;color:#4caf50;font-size:13px">{% raw %}{{ initPoseMsg }}{% endraw %}</span>
</div>
<!-- Nav2 导航状态 -->
<div v-if="nav2Available" class="nav2-status-bar" style="margin-top:8px;padding:8px 12px;background:#1a2332;border-radius:6px;font-size:13px">
<span>🧭 Nav2: <strong :style="navStatus === 'succeeded' ? 'color:#4caf50' : navStatus === 'navigating' ? 'color:#ff9800' : 'color:#9aa0a6'">{% raw %}{{ navStatus }}{% endraw %}</strong></span>
<span v-if="navCurrentPos" style="margin-left:12px">📍 当前位置: <strong>X={% raw %}{{ navCurrentPos[0] !== undefined ? navCurrentPos[0].toFixed(2) : '?' }}{% endraw %} Y={% raw %}{{ navCurrentPos[1] !== undefined ? navCurrentPos[1].toFixed(2) : '?' }}{% endraw %}</strong></span>
<button class="btn btn-small" style="margin-left:8px" @click="refreshNavStatus">🔄 刷新</button>
<button v-if="navStatus === 'navigating'" class="btn btn-small btn-secondary" @click="cancelNav" style="margin-left:4px">取消导航</button>
</div>
<div class="agv-control-panel">
<div class="agv-dir-row">
@@ -540,7 +552,7 @@
</main>
</div>
<script src="/static/js/vue3.global.prod.js?v=20260515a"></script>
<script src="/static/js/setting.js?v=20260516a"></script>
<script src="/static/js/vue3.global.prod.js?v=20260517a"></script>
<script src="/static/js/setting.js?v=20260517a"></script>
</body>
</html>
agv_app/utils/agv_controller_ros2.py
+1 -1
View File
@@ -28,7 +28,7 @@ class AGVController:
def _run_ros2_cmd(self, cmd: str, timeout: float = 5.0) -> tuple:
"""执行 ros2 命令"""
full_cmd = f"bash -l -c 'source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash && {cmd}'"
full_cmd = f"bash -c 'source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash && export ROS_DOMAIN_ID=0 && {cmd}'"
try:
result = subprocess.run(
full_cmd,
agv_app/utils/nav2_navigator.py
+162 -49
View File
@@ -183,8 +183,10 @@ class Nav2Navigator:
qz = math.sin(yaw / 2.0)
qw = math.cos(yaw / 2.0)
# 构建 heredoc 内容
heredoc = (
# 构建 goal YAML（用 bash heredoc 方式避免转义问题）
# 写入临时文件
import tempfile
goal_yaml = (
f"pose:\n"
f" header:\n"
f" stamp:\n"
@@ -201,9 +203,20 @@ class Nav2Navigator:
f" y: 0.0\n"
f" z: {qz}\n"
f" w: {qw}\n"
f"behavior_tree: ''\n"
)
cmd = f'{setup} && ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose - --feedback'
goal_file = "/tmp/nav2_goal_{}.yaml".format(os.getpid())
with open(goal_file, "w") as f:
f.write(goal_yaml)
cmd = (
f'bash -l -c \''
f'source /opt/ros/humble/setup.bash && '
f'source /home/elephant/agv_pro_ros2/install/setup.bash && '
f'ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose '
f'"$(cat {goal_file})" --feedback\''
)
process = subprocess.Popen(
cmd, shell=True, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
@@ -219,18 +232,103 @@ class Nav2Navigator:
break
lines.append(line)
# 写入 goal（通过 bash heredoc）
# 写入 goal（通过 bash 脚本方式）
script_content = (
f'#!/bin/bash\n'
f'source /opt/ros/humble/setup.bash\n'
f'source /home/elephant/agv_pro_ros2/install/setup.bash\n'
f'GOAL=$(cat {goal_file})\n'
f'ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "$GOAL" --feedback\n'
)
script_file = "/tmp/nav2_action.sh"
with open(script_file, "w") as sf:
sf.write(script_content)
os.chmod(script_file, 0o755)
out_thread = threading.Thread(target=reader, args=(process.stdout, stdout_lines))
err_thread = threading.Thread(target=reader, args=(process.stderr, stderr_lines))
out_thread.start()
err_thread.start()
# 写入 heredoc 数据
# 用 subprocess.Popen([script_path]) 替代 stdin
stop_reading.set()
if process.poll() is None:
process.terminate()
process.wait(timeout=3)
act_proc = subprocess.Popen(
[script_file],
stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
)
stdout_lines2 = []
stderr_lines2 = []
stop_reading2 = threading.Event()
def reader2(pipe, lines):
for line in iter(pipe.readline, ''):
if stop_reading2.is_set():
break
lines.append(line)
t_out2 = threading.Thread(target=reader2, args=(act_proc.stdout, stdout_lines2))
t_err2 = threading.Thread(target=reader2, args=(act_proc.stderr, stderr_lines2))
t_out2.start()
t_err2.start()
# 轮询等待 action 完成（每 2s 检查一次结果）
result_received = False
for _ in range(60): # 最多等 120 秒
time.sleep(2)
elapsed = time.time() - start_time
# 检查是否有结果
full_out = "".join(stdout_lines2)
full_err = "".join(stderr_lines2)
combined = full_out + full_err
result_idx = combined.find("Result:")
if result_idx >= 0:
import re
status_m = re.search(r'status[\s:]+(\w+)', combined[result_idx:])
if status_m:
st = status_m.group(1).lower()
if st in ('succeeded', 'SUCCEEDED'):
logger.info("✅ Nav2 导航成功到达目标")
self._result_status = "succeeded"
result_received = True
break
elif st in ('failed', 'FAILED', 'aborted'):
logger.warning(f"⚠️ Nav2 导航失败: status={st}")
self._result_status = "failed"
result_received = True
break
elif st in ('canceled', 'cancelled'):
logger.info("Nav2 导航被取消")
self._result_status = "cancelled"
result_received = True
break
# 检查进程是否已结束但无结果
if act_proc.poll() is not None and not result_received:
self._result_status = "failed"
break
if self._cancel_event.is_set():
break
stop_reading2.set()
if act_proc.poll() is None:
act_proc.terminate()
try:
process.stdin.write(heredoc + "\n")
process.stdin.flush()
process.stdin.close()
except Exception as e:
logger.error(f"写入 heredoc 失败: {e}")
act_proc.wait(timeout=3)
except subprocess.TimeoutExpired:
act_proc.kill()
t_out2.join(timeout=2)
t_err2.join(timeout=2)
stdout_lines = stdout_lines2
stderr_lines = stderr_lines2
# 轮询检查结果
while not self._cancel_event.is_set() and elapsed < timeout_sec:
@@ -238,44 +336,51 @@ class Nav2Navigator:
elapsed = time.time() - start_time
full_out = "".join(stdout_lines)
if "succeeded" in full_out.lower():
logger.info("✅ Nav2 导航成功到达目标")
self._result_status = "succeeded"
result_received = True
break
if any(k in full_out.lower() for k in ["failed", "aborted"]):
logger.warning(f"⚠️ Nav2 导航失败: {full_out[-200:]}")
self._result_status = "failed"
result_received = True
break
if any(k in full_out.lower() for k in ["canceled", "cancelled"]):
logger.info("Nav2 导航被取消")
self._result_status = "cancelled"
result_received = True
break
full_err = "".join(stderr_lines)
combined = full_out + full_err
# 检查进程是否结束但没读到结果
if process.poll() is not None and not result_received:
if full_out:
logger.warning(f"Nav2 进程意外结束，输出: {full_out[:200]}")
# 查找 Result 节中的 status 字段
import re
result_idx = combined.find("Result:")
if result_idx >= 0:
status_m = re.search(r'status[\s:]+(\w+)', combined[result_idx:])
if status_m:
st = status_m.group(1).lower()
if st in ('succeeded', 'SUCCEEDED'):
logger.info("✅ Nav2 导航成功到达目标")
self._result_status = "succeeded"
result_received = True
break
elif st in ('failed', 'FAILED', 'aborted'):
logger.warning(f"⚠️ Nav2 导航失败: status={st}")
self._result_status = "failed"
result_received = True
break
elif st in ('canceled', 'cancelled'):
logger.info("Nav2 导航被取消")
self._result_status = "cancelled"
result_received = True
break
if not result_received:
self._result_status = "failed"
break
# 善后
stop_reading.set()
if process.poll() is None:
process.terminate()
if act_proc.poll() is None:
act_proc.terminate()
try:
process.wait(timeout=3)
act_proc.wait(timeout=3)
except subprocess.TimeoutExpired:
process.kill()
act_proc.kill()
out_thread.join(timeout=2)
err_thread.join(timeout=2)
if self._cancel_event.is_set() and not result_received:
logger.info("取消 Nav2 导航...")
cancel_cmd = f"bash -l -c '{setup} && ros2 action cancel /navigate_to_pose 2>/dev/null || ros2 action cancel /navigate_through_poses 2>/dev/null || true'"
cancel_cmd = f'bash -l -c \'{setup} && ros2 action cancel /navigate_to_pose 2>/dev/null || ros2 action cancel /navigate_through_poses 2>/dev/null || true\''
subprocess.run(cancel_cmd, shell=True, timeout=3)
self._result_status = "cancelled"
@@ -350,7 +455,7 @@ class Nav2Navigator:
])
poses_yaml = "poses:\n" + "\n".join(poses_lines)
cmd = f'{setup} && ros2 action send_goal /navigate_through_poses nav2_msgs/action/NavigateThroughPoses - --feedback'
cmd = f'bash -l -c \'{setup} && ros2 action send_goal /navigate_through_poses nav2_msgs/action/NavigateThroughPoses - --feedback\''
process = subprocess.Popen(
cmd, shell=True, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
@@ -381,21 +486,29 @@ class Nav2Navigator:
while not self._cancel_event.is_set():
time.sleep(1.0)
full_out = "".join(stdout_lines)
full_err = "".join(stderr_lines)
combined = full_out + full_err
if "succeeded" in full_out.lower():
logger.info(f"✅ Nav2 路径点导航成功完成 {len(poses)} 个点")
self._result_status = "succeeded"
result_received = True
break
if any(k in full_out.lower() for k in ["failed", "aborted"]):
logger.warning(f"⚠️ Nav2 路径点导航失败")
self._result_status = "failed"
result_received = True
break
if any(k in full_out.lower() for k in ["canceled", "cancelled"]):
self._result_status = "cancelled"
result_received = True
break
result_idx = combined.find("Result:")
if result_idx >= 0:
import re
status_m = re.search(r'status[\s:]+(\w+)', combined[result_idx:])
if status_m:
st = status_m.group(1).lower()
if st in ('succeeded', 'SUCCEEDED'):
logger.info(f"✅ Nav2 路径点导航成功完成 {len(poses)} 个点")
self._result_status = "succeeded"
result_received = True
break
elif st in ('failed', 'FAILED', 'aborted'):
logger.warning(f"⚠️ Nav2 路径点导航失败")
self._result_status = "failed"
result_received = True
break
elif st in ('canceled', 'cancelled'):
self._result_status = "cancelled"
result_received = True
break
if process.poll() is not None and not result_received:
self._result_status = "failed"
@@ -423,7 +536,7 @@ class Nav2Navigator:
if self.status == Nav2Status.NAVIGATING:
self._cancel_event.set()
setup = "source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash"
cancel_cmd = f"bash -l -c '{setup} && ros2 action cancel /navigate_to_pose 2>/dev/null || ros2 action cancel /navigate_through_poses 2>/dev/null || true'"
cancel_cmd = f'bash -l -c \'{setup} && ros2 action cancel /navigate_to_pose 2>/dev/null || ros2 action cancel /navigate_through_poses 2>/dev/null || true\''
subprocess.run(cancel_cmd, shell=True, timeout=3)
self.status = Nav2Status.CANCELLED
logger.info("导航已停止")