ywb
2bb8dff4e2
- 新增 agv_app/utils/nav2_navigator.py：Nav2Navigator 类，通过 ros2 action /navigate_to_pose 和 /navigate_through_poses 与 Nav2 通信，支持路径点导航 - app.py：navigate/to, navigate/path, navigate/status 三个 API 改用 Nav2Navigator - mission_executor.py：_execute_point 中调用 _nav2_go_to_point() 替代原来的 time.sleep 模拟移动，Nav2 负责从当前点到目标点的自主导航 - 原有的 map_navigator.py（A* + Pure Pursuit 自实现）保留不动，供无 Nav2 时降级用
461 lines
17 KiB
Python
461 lines
17 KiB
Python
"""
|
||
Nav2 导航模块 - 通过 ROS2 Action Server 控制 AGV 导航
|
||
使用 Nav2 的 navigate_to_pose 和 navigate_through_poses action
|
||
|
||
依赖：ROS2 humble, nav2_bringup 已启动
|
||
不依赖：自己实现 A*、路径跟踪
|
||
"""
|
||
|
||
import time
|
||
import math
|
||
import logging
|
||
import threading
|
||
import subprocess
|
||
from typing import List, Tuple, Optional, Dict
|
||
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"
|
||
|
||
|
||
class Nav2Status(Enum):
|
||
IDLE = "idle"
|
||
NAVIGATING = "navigating"
|
||
SUCCEEDED = "succeeded"
|
||
FAILED = "failed"
|
||
CANCELLED = "cancelled"
|
||
UNKNOWN = "unknown"
|
||
|
||
|
||
class Nav2Navigator:
|
||
"""Nav2 导航器 — 通过 ROS2 Action Server 发送导航目标"""
|
||
|
||
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]
|
||
|
||
def _run_ros2_cmd(self, cmd: str, timeout: float = 5.0) -> Tuple[int, str, str]:
|
||
"""执行 ros2 命令"""
|
||
full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {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"
|
||
except Exception as e:
|
||
return -1, "", str(e)
|
||
|
||
def _check_nav2_available(self) -> bool:
|
||
"""检查 Nav2 action server 是否可用"""
|
||
rc, out, err = self._run_ros2_cmd("ros2 action list")
|
||
if rc != 0:
|
||
return False
|
||
return "/navigate_to_pose" in out or "navigate_to_pose" in out
|
||
|
||
def _get_current_pose(self) -> List[float]:
|
||
"""从 /odom 获取当前位置 [x, y, yaw]"""
|
||
try:
|
||
rc, out, err = self._run_ros2_cmd(
|
||
"timeout 5 ros2 topic echo /odom --once 2>/dev/null",
|
||
timeout=6
|
||
)
|
||
if rc == 0 and out:
|
||
import re, 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"获取 odom 失败: {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:
|
||
"""
|
||
导航到目标坐标（Nav2 navigate_to_pose action）
|
||
|
||
Args:
|
||
x, y: 目标位置（世界坐标，米）
|
||
yaw: 目标朝向（弧度），None 则保持当前朝向
|
||
timeout_sec: 超时时间（秒）
|
||
blocking: 是否阻塞等待导航完成
|
||
|
||
Returns:
|
||
blocking=True: 是否成功到达
|
||
blocking=False: 是否成功启动导航
|
||
"""
|
||
if self.status == Nav2Status.NAVIGATING:
|
||
logger.warning("导航正在进行中，请先停止当前导航")
|
||
return False
|
||
|
||
# 检查 Nav2 是否可用
|
||
if not self._check_nav2_available():
|
||
logger.error("Nav2 action server 不可用，请确保 navigation2_active.launch.py 已启动")
|
||
return False
|
||
|
||
# 构建 pose 消息（geometry_msgs/Pose）
|
||
if yaw is None:
|
||
current = self._get_current_pose()
|
||
yaw = current[2]
|
||
|
||
# 计算四元数（只有 yaw）
|
||
qz = math.sin(yaw / 2.0)
|
||
qw = math.cos(yaw / 2.0)
|
||
|
||
pose_yaml = (
|
||
f"pose:\\n"
|
||
f" header:\\n"
|
||
f" stamp:\\n"
|
||
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}"
|
||
)
|
||
|
||
logger.info(f"发送导航目标: ({x:.3f}, {y:.3f}), yaw={math.degrees(yaw):.1f}°")
|
||
|
||
# 启动导航线程
|
||
self._cancel_event.clear()
|
||
self._result_status = None
|
||
self.status = Nav2Status.NAVIGATING
|
||
|
||
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):
|
||
"""导航执行线程"""
|
||
try:
|
||
# 计算四元数
|
||
qz = math.sin(yaw / 2.0)
|
||
qw = math.cos(yaw / 2.0)
|
||
|
||
# 使用 ros2 action send_goal 发送导航目标
|
||
# Nav2 的 navigate_to_pose action 接受 geometry_msgs/PoseStamped
|
||
pose_yaml = (
|
||
f"pose:\\n"
|
||
f" header:\\n"
|
||
f" stamp:\\n"
|
||
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}"
|
||
)
|
||
|
||
# 发送 goal，保持连接获取结果
|
||
cmd = (
|
||
f"ros2 action send_goal /navigate_to_pose "
|
||
f"navigation_action_msgs/NavigateToPose "
|
||
f"'{pose_yaml}' "
|
||
f"--feedback"
|
||
)
|
||
|
||
full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
|
||
|
||
# 用 popen 持续读取反馈，直到完成或取消
|
||
process = subprocess.Popen(
|
||
full_cmd, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
|
||
)
|
||
|
||
feedback_received = False
|
||
result_received = False
|
||
cancel_check_interval = 1.0 # 每秒检查一次取消标志
|
||
|
||
while not self._cancel_event.is_set():
|
||
# 非阻塞读取 stdout
|
||
import select
|
||
reads = [process.stdout]
|
||
ready, _, _ = select.select(reads, [], [], cancel_check_interval)
|
||
|
||
if process.stdout in ready:
|
||
line = process.stdout.readline()
|
||
if not line:
|
||
break
|
||
line = line.strip()
|
||
|
||
# 解析反馈
|
||
if "feedback" in line.lower() or "distance_remaining" in line:
|
||
feedback_received = True
|
||
logger.debug(f"Nav2 feedback: {line[:100]}")
|
||
# 解析结果
|
||
if "succeeded" in line.lower():
|
||
self._result_status = "succeeded"
|
||
result_received = True
|
||
logger.info("✅ Nav2 导航成功到达目标")
|
||
break
|
||
elif "failed" in line.lower() or "aborted" in line.lower():
|
||
self._result_status = "failed"
|
||
result_received = True
|
||
logger.warning(f"⚠️ Nav2 导航失败: {line}")
|
||
break
|
||
elif "canceled" in line.lower() or "cancelled" in line.lower():
|
||
self._result_status = "cancelled"
|
||
result_received = True
|
||
logger.info("Nav2 导航被取消")
|
||
break
|
||
|
||
# 检查进程是否结束
|
||
if process.poll() is not None:
|
||
# 进程已结束但没读到结果
|
||
if not result_received:
|
||
self._result_status = "failed"
|
||
break
|
||
|
||
# 处理取消或超时
|
||
if self._cancel_event.is_set() and not result_received:
|
||
logger.info("取消 Nav2 导航...")
|
||
# 发送取消命令
|
||
cancel_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 action cancel /navigate_to_pose'"
|
||
subprocess.run(cancel_cmd, shell=True, timeout=3)
|
||
self._result_status = "cancelled"
|
||
|
||
# 确保进程结束
|
||
if process.poll() is None:
|
||
process.terminate()
|
||
try:
|
||
process.wait(timeout=3)
|
||
except subprocess.TimeoutExpired:
|
||
process.kill()
|
||
|
||
# 设置最终状态
|
||
if self._result_status == "succeeded":
|
||
self.status = Nav2Status.SUCCEEDED
|
||
elif self._result_status == "cancelled":
|
||
self.status = Nav2Status.CANCELLED
|
||
else:
|
||
self.status = Nav2Status.FAILED
|
||
|
||
except Exception as e:
|
||
logger.error(f"Nav2 导航异常: {e}")
|
||
self.status = Nav2Status.FAILED
|
||
|
||
def navigate_through_poses(self, poses: List[Tuple[float, float, float]],
|
||
timeout_per_pose: float = 120.0,
|
||
blocking: bool = True) -> bool:
|
||
"""
|
||
导航通过多个路径点（Nav2 navigate_through_poses action）
|
||
|
||
Args:
|
||
poses: [(x, y, yaw), ...] 路径点列表
|
||
timeout_per_pose: 每个点位的超时（秒）
|
||
blocking: 是否阻塞等待完成
|
||
|
||
Returns:
|
||
是否全部成功
|
||
"""
|
||
if self.status == Nav2Status.NAVIGATING:
|
||
logger.warning("导航正在进行中，请先停止")
|
||
return False
|
||
|
||
if not self._check_nav2_available():
|
||
logger.error("Nav2 action server 不可用")
|
||
return False
|
||
|
||
if not poses:
|
||
return True
|
||
|
||
# 构建 poses 列表
|
||
poses_yaml_parts = []
|
||
for i, (x, y, yaw) in enumerate(poses):
|
||
qz = math.sin(yaw / 2.0)
|
||
qw = math.cos(yaw / 2.0)
|
||
poses_yaml_parts.append(
|
||
f" - pose:\\n"
|
||
f" header:\\n"
|
||
f" stamp:\\n"
|
||
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}"
|
||
)
|
||
|
||
poses_yaml = "poses:\n" + "\n".join(poses_yaml_parts)
|
||
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:
|
||
poses_yaml_parts = []
|
||
for i, (x, y, yaw) in enumerate(poses):
|
||
qz = math.sin(yaw / 2.0)
|
||
qw = math.cos(yaw / 2.0)
|
||
poses_yaml_parts.append(
|
||
f" - pose:\\n"
|
||
f" header:\\n"
|
||
f" stamp:\\n"
|
||
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}"
|
||
)
|
||
|
||
poses_yaml = "poses:\n" + "\n".join(poses_yaml_parts)
|
||
|
||
cmd = (
|
||
f"ros2 action send_goal /navigate_through_poses "
|
||
f"navigation_action_msgs/NavigateThroughPoses "
|
||
f"'{poses_yaml}' "
|
||
f"--feedback"
|
||
)
|
||
|
||
full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
|
||
|
||
process = subprocess.Popen(
|
||
full_cmd, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
|
||
)
|
||
|
||
result_received = False
|
||
|
||
while not self._cancel_event.is_set():
|
||
import select
|
||
reads = [process.stdout]
|
||
ready, _, _ = select.select(reads, [], [], 1.0)
|
||
|
||
if process.stdout in ready:
|
||
line = process.stdout.readline()
|
||
if not line:
|
||
break
|
||
line = line.strip()
|
||
|
||
if "succeeded" in line.lower():
|
||
logger.info(f"✅ Nav2 路径点导航成功完成 {len(poses)} 个点")
|
||
self._result_status = "succeeded"
|
||
result_received = True
|
||
break
|
||
elif "failed" in line.lower() or "aborted" in line.lower():
|
||
logger.warning(f"⚠️ Nav2 路径点导航失败: {line}")
|
||
self._result_status = "failed"
|
||
result_received = True
|
||
break
|
||
elif "canceled" in line.lower() or "cancelled" in line.lower():
|
||
self._result_status = "cancelled"
|
||
result_received = True
|
||
break
|
||
|
||
if process.poll() is not None:
|
||
if not result_received:
|
||
self._result_status = "failed"
|
||
break
|
||
|
||
if self._cancel_event.is_set() and not result_received:
|
||
logger.info("取消路径点导航...")
|
||
cancel_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 action cancel /navigate_through_poses'"
|
||
subprocess.run(cancel_cmd, shell=True, timeout=3)
|
||
self._result_status = "cancelled"
|
||
|
||
if process.poll() is None:
|
||
process.terminate()
|
||
try:
|
||
process.wait(timeout=3)
|
||
except subprocess.TimeoutExpired:
|
||
process.kill()
|
||
|
||
if self._result_status == "succeeded":
|
||
self.status = Nav2Status.SUCCEEDED
|
||
elif self._result_status == "cancelled":
|
||
self.status = Nav2Status.CANCELLED
|
||
else:
|
||
self.status = Nav2Status.FAILED
|
||
|
||
except Exception as e:
|
||
logger.error(f"Nav2 路径点导航异常: {e}")
|
||
self.status = Nav2Status.FAILED
|
||
|
||
def stop(self):
|
||
"""取消当前导航"""
|
||
if self.status == Nav2Status.NAVIGATING:
|
||
self._cancel_event.set()
|
||
# 发送取消命令
|
||
cancel_cmd = f"bash -c '{ROS2_SETUP_CMD} && 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("导航已停止")
|
||
|
||
def get_status(self) -> Dict:
|
||
"""获取导航状态"""
|
||
current = self._get_current_pose()
|
||
return {
|
||
"status": self.status.value,
|
||
"current_position": current,
|
||
"nav2_available": self._check_nav2_available()
|
||
}
|
||
|
||
def get_current_position(self) -> List[float]:
|
||
"""获取当前位置 [x, y, yaw]"""
|
||
return self._get_current_pose() |