diff --git a/agv_app/app.py b/agv_app/app.py index b27e229..b215f0d 100644 --- a/agv_app/app.py +++ b/agv_app/app.py @@ -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(): """获取导航状态""" diff --git a/agv_app/static/js/setting.js b/agv_app/static/js/setting.js index 4604ad2..ffcde9b 100644 --- a/agv_app/static/js/setting.js +++ b/agv_app/static/js/setting.js @@ -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') diff --git a/agv_app/templates/setting.html b/agv_app/templates/setting.html index f511b5a..5f9cc55 100644 --- a/agv_app/templates/setting.html +++ b/agv_app/templates/setting.html @@ -4,7 +4,7 @@ 设置 - AGV 拍摄系统 - +
@@ -239,10 +239,10 @@
-
+
- - + +
@@ -446,6 +446,7 @@
+
@@ -497,8 +498,19 @@
🔋 电压: {% raw %}{{ agvBattery !== null ? agvBattery + 'V' : '—' }}{% endraw %} - 📍 位置: X={% raw %}{{ agvPosition[0] ? agvPosition[0].toFixed(2) : '?' }}{% endraw %} Y={% raw %}{{ agvPosition[1] ? agvPosition[1].toFixed(2) : '?' }}{% endraw %} + 📍 位置: 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 }}° + + {% raw %}{{ initPoseMsg }}{% endraw %} +
+ +
+ 🧭 Nav2: {% raw %}{{ navStatus }}{% endraw %} + 📍 当前位置: X={% raw %}{{ navCurrentPos[0] !== undefined ? navCurrentPos[0].toFixed(2) : '?' }}{% endraw %} Y={% raw %}{{ navCurrentPos[1] !== undefined ? navCurrentPos[1].toFixed(2) : '?' }}{% endraw %} + +
@@ -540,7 +552,7 @@
- - + + diff --git a/agv_app/utils/agv_controller_ros2.py b/agv_app/utils/agv_controller_ros2.py index 6572dc4..303af38 100644 --- a/agv_app/utils/agv_controller_ros2.py +++ b/agv_app/utils/agv_controller_ros2.py @@ -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, diff --git a/agv_app/utils/nav2_navigator.py b/agv_app/utils/nav2_navigator.py index d0447ab..2fdc0af 100644 --- a/agv_app/utils/nav2_navigator.py +++ b/agv_app/utils/nav2_navigator.py @@ -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("导航已停止")