""" Nav2 导航模块 - 使用官方 nav2_simple_commander.BasicNavigator API 参考: https://github.com/elephantrobotics/agv_pro_ros2/blob/humble/agv_pro_navigation2/scripts/example_nav_to_pose.py """ import time import math import logging import threading import subprocess import os import re from typing import List, Tuple, Optional, Dict from enum import Enum logger = logging.getLogger(__name__) # 延迟导入 rclpy 和 nav2_simple_commander（避免在模块加载时初始化 ROS） _rclpy = None _BasicNavigator = None _TaskResult = None _PoseStamped = None _Duration = None def _init_ros(): """延迟初始化 ROS2 依赖""" global _rclpy, _BasicNavigator, _TaskResult, _PoseStamped, _Duration if _rclpy is None: import rclpy _rclpy = rclpy from geometry_msgs.msg import PoseStamped from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult from rclpy.duration import Duration _PoseStamped = PoseStamped _BasicNavigator = BasicNavigator _TaskResult = TaskResult _Duration = Duration def _run_ros2_bash(cmd: str, timeout: float = 5.0) -> Tuple[int, str, str]: """执行 ros2 命令（直接运行，不二次包装）""" setup = "source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash && export ROS_DOMAIN_ID=1" full_cmd = f"bash -l -c '{setup} && {cmd}'" try: result = subprocess.run(full_cmd, shell=True, capture_output=True, text=True, timeout=timeout) return result.returncode, result.stdout.strip(), result.stderr.strip() except subprocess.TimeoutExpired: return -1, "", "Timeout" class Nav2Status(Enum): IDLE = "idle" NAVIGATING = "navigating" SUCCEEDED = "succeeded" FAILED = "failed" CANCELLED = "cancelled" UNKNOWN = "unknown" class Nav2Navigator: """Nav2 导航器 — 使用官方 BasicNavigator API""" def __init__(self): self.status = Nav2Status.IDLE self._nav_thread: Optional[threading.Thread] = None self._cancel_event = threading.Event() self._result_status = None self._current_pose = [0.0, 0.0, 0.0] self._navigator = None # BasicNavigator 实例 self._rclpy_initialized = False def _ensure_rclpy(self): """确保 rclpy 已初始化（每个线程只能调用一次）""" _init_ros() if not self._rclpy_initialized: if not _rclpy.ok(): _rclpy.init() self._rclpy_initialized = True def _get_navigator(self): """获取或创建 BasicNavigator 实例""" self._ensure_rclpy() if self._navigator is None: self._navigator = _BasicNavigator() return self._navigator def _check_nav2_available(self) -> bool: """检查 Nav2 action server 是否可用""" try: nav = self._get_navigator() # BasicNavigator 内部会检查 Nav2 是否活跃 return True except Exception as e: logger.debug(f"Nav2 不可用: {e}") return False def _get_current_pose(self) -> List[float]: """从 /amcl_pose 获取当前位置 [x, y, yaw]""" try: rc, out, err = _run_ros2_bash("timeout 3 ros2 topic echo /amcl_pose --once 2>/dev/null", timeout=4) if rc == 0 and out: import yaml clean = re.sub(r'\x1b\[[0-9;]*[a-zA-Z]', '', out) yaml_str = re.sub(r'^[\d\-:. ]+\[RTPS[^\]]*\].*\n?', '', clean, flags=re.MULTILINE).strip() yaml_str = yaml_str.split('---')[0] data = yaml.safe_load(yaml_str) if data: pos = data.get("pose", {}).get("pose", {}).get("position", {}) orient = data.get("pose", {}).get("pose", {}).get("orientation", {}) x = pos.get("x", 0.0) y = pos.get("y", 0.0) qz = orient.get("z", 0.0) qw = orient.get("w", 1.0) yaw = math.atan2(2.0 * qw * qz, 1.0 - 2.0 * qz * qz) self._current_pose = [x, y, yaw] return self._current_pose except Exception as e: logger.debug(f"获取 amcl_pose 失败: {e}") return self._current_pose def navigate_to_pose(self, x: float, y: float, yaw: float = None, timeout_sec: float = 120.0, blocking: bool = True) -> bool: """导航到目标坐标（使用官方 BasicNavigator API）""" if self.status == Nav2Status.NAVIGATING: logger.warning("导航正在进行中，请先停止当前导航") return False self.status = Nav2Status.IDLE self._result_status = None if yaw is None: current = self._get_current_pose() yaw = current[2] logger.info(f"发送导航目标: ({x:.3f}, {y:.3f}), yaw={math.degrees(yaw):.1f}°") self._cancel_event.clear() self._result_status = None self.status = Nav2Status.NAVIGATING if self._nav_thread and self._nav_thread.is_alive(): self.stop() self._nav_thread.join(timeout=3) self._nav_thread = threading.Thread( target=self._nav_thread_func, args=(x, y, yaw, timeout_sec), daemon=True ) self._nav_thread.start() if blocking: self._nav_thread.join(timeout=timeout_sec + 10) return self.status == Nav2Status.SUCCEEDED return True def _nav_thread_func(self, x: float, y: float, yaw: float, timeout_sec: float): """导航执行线程 — 使用官方 BasicNavigator API""" try: logger.info(f"Nav2 线程启动: x={x}, y={y}, yaw={yaw}, timeout={timeout_sec}s") # 在子线程中初始化 rclpy _init_ros() if not _rclpy.ok(): _rclpy.init() navigator = _BasicNavigator() # 等待 Nav2 激活（可选，如果 Nav2 已启动可跳过） # navigator.waitUntilNav2Active() # 构造目标位姿 goal_pose = _PoseStamped() goal_pose.header.frame_id = 'map' goal_pose.header.stamp = navigator.get_clock().now().to_msg() goal_pose.pose.position.x = x goal_pose.pose.position.y = y goal_pose.pose.position.z = 0.0 # 四元数（仅 z, w） qz = math.sin(yaw / 2.0) qw = math.cos(yaw / 2.0) goal_pose.pose.orientation.x = 0.0 goal_pose.pose.orientation.y = 0.0 goal_pose.pose.orientation.z = qz goal_pose.pose.orientation.w = qw # 发送导航目标 navigator.goToPose(goal_pose) logger.info("导航目标已发送，等待完成...") # 等待导航完成 start_time = time.time() while not navigator.isTaskComplete(): if self._cancel_event.is_set(): navigator.cancelTask() logger.info("导航被取消") self._result_status = "cancelled" break elapsed = time.time() - start_time if elapsed > timeout_sec: logger.warning(f"导航超时 ({timeout_sec}s)，取消任务") navigator.cancelTask() self._result_status = "failed" break # 获取反馈（可选） feedback = navigator.getFeedback() if feedback: remaining = _Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9 logger.debug(f"预计剩余时间: {remaining:.1f}s") time.sleep(0.5) # 获取结果 result = navigator.getResult() if result == _TaskResult.SUCCEEDED: logger.info(f"✅ Nav2 导航成功到达目标 ({x:.3f}, {y:.3f})") self._result_status = "succeeded" self.status = Nav2Status.SUCCEEDED elif result == _TaskResult.CANCELED: logger.info("导航被取消") self._result_status = "cancelled" self.status = Nav2Status.CANCELLED elif result == _TaskResult.FAILED: logger.warning("⚠️ Nav2 导航失败") self._result_status = "failed" self.status = Nav2Status.FAILED else: logger.warning(f"导航返回未知状态: {result}") self._result_status = "failed" self.status = Nav2Status.FAILED except Exception as e: logger.error(f"Nav2 导航异常: {e}", exc_info=True) self.status = Nav2Status.FAILED self._result_status = "failed" def navigate_through_poses(self, poses: List[Tuple[float, float, float]], timeout_per_pose: float = 120.0, blocking: bool = True) -> bool: """导航通过多个路径点""" if self.status == Nav2Status.NAVIGATING: logger.warning("导航正在进行中，请先停止") return False if not poses: return True logger.info(f"发送 {len(poses)} 个路径点的导航任务") self._cancel_event.clear() self.status = Nav2Status.NAVIGATING self._nav_thread = threading.Thread( target=self._nav_thread_func_multi, args=(poses, timeout_per_pose), daemon=True ) self._nav_thread.start() if blocking: total_timeout = len(poses) * timeout_per_pose + 30 self._nav_thread.join(timeout=total_timeout) return self.status == Nav2Status.SUCCEEDED return True def _nav_thread_func_multi(self, poses: List[Tuple[float, float, float]], timeout_per_pose: float): """多路径点导航线程""" try: _init_ros() if not _rclpy.ok(): _rclpy.init() navigator = _BasicNavigator() # 构造路径点列表 goal_poses = [] for x, y, yaw in poses: pose = _PoseStamped() pose.header.frame_id = 'map' pose.header.stamp = navigator.get_clock().now().to_msg() pose.pose.position.x = x pose.pose.position.y = y pose.pose.position.z = 0.0 qz = math.sin(yaw / 2.0) qw = math.cos(yaw / 2.0) pose.pose.orientation.x = 0.0 pose.pose.orientation.y = 0.0 pose.pose.orientation.z = qz pose.pose.orientation.w = qw goal_poses.append(pose) navigator.goThroughPoses(goal_poses) logger.info(f"已发送 {len(poses)} 个路径点，等待完成...") start_time = time.time() total_timeout = len(poses) * timeout_per_pose while not navigator.isTaskComplete(): if self._cancel_event.is_set(): navigator.cancelTask() self._result_status = "cancelled" break elapsed = time.time() - start_time if elapsed > total_timeout: navigator.cancelTask() self._result_status = "failed" break time.sleep(0.5) result = navigator.getResult() if result == _TaskResult.SUCCEEDED: logger.info(f"✅ Nav2 路径点导航成功完成 {len(poses)} 个点") self._result_status = "succeeded" self.status = Nav2Status.SUCCEEDED elif result == _TaskResult.CANCELED: self._result_status = "cancelled" self.status = Nav2Status.CANCELLED else: self._result_status = "failed" self.status = Nav2Status.FAILED except Exception as e: logger.error(f"Nav2 路径点导航异常: {e}", exc_info=True) self.status = Nav2Status.FAILED def stop(self): """取消当前导航""" if self.status == Nav2Status.NAVIGATING: self._cancel_event.set() self.status = Nav2Status.CANCELLED logger.info("导航已停止") def get_status(self) -> Dict: """获取导航状态""" current = self._get_current_pose() nav2_available = self._check_nav2_available() return { "status": self.status.value, "current_position": current, "nav2_available": nav2_available } def get_current_position(self) -> List[float]: """获取当前位置 [x, y, yaw]""" return self._get_current_pose()