Nav2 导航集成：新增 nav2_navigator.py，mission_executor 改用 Nav2 action 导航
- 新增 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 时降级用
This commit is contained in:
ywb
@@ -5,16 +5,21 @@ import os
|
||||
import json
|
||||
import time
|
||||
import logging
|
||||
import subprocess
|
||||
import math
|
||||
from typing import List, Dict, Optional
|
||||
from enum import Enum
|
||||
|
||||
from .arm_client import ArmClient
|
||||
from .agv_controller import AGVController
|
||||
from .agv_controller_ros2 import AGVController
|
||||
from .qr_scanner import QRScanner
|
||||
from .image_uploader import ImageUploader
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
# ROS2 环境设置（与 agv_controller_ros2.py 保持一致）
|
||||
ROS2_SETUP_CMD = "export ROS_DOMAIN_ID=1 && source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash"
|
||||
|
||||
|
||||
class TaskStatus(Enum):
|
||||
PENDING = "pending"
|
||||
@@ -122,12 +127,16 @@ class MissionExecutor:
|
||||
"poses": []
|
||||
}
|
||||
|
||||
# 1. AGV 移动到点位
|
||||
# 1. AGV 移动到点位（Nav2 导航）
|
||||
coords = point.get("coords", {})
|
||||
x, y = coords.get("x", 0), coords.get("y", 0)
|
||||
logger.info(f"AGV 移动到 ({x}, {y})")
|
||||
# TODO: 调用导航移动到目标点
|
||||
time.sleep(1) # 模拟移动
|
||||
rz = coords.get("rz", 0.0) # 目标朝向
|
||||
logger.info(f"AGV Nav2 导航到 ({x}, {y}), yaw={rz}")
|
||||
|
||||
nav_result = self._nav2_go_to_point(x, y, rz)
|
||||
if not nav_result:
|
||||
logger.warning(f"AGV 导航到点位 {point_name} 失败，跳过")
|
||||
return {"point_name": point_name, "status": "failed", "error": "导航失败"}
|
||||
|
||||
# 2. 执行该点位的所有姿态
|
||||
poses = point.get("poses", [])
|
||||
@@ -206,6 +215,151 @@ class MissionExecutor:
|
||||
self.uploader.upload(photo_path, serial, pose_idx, photo_type)
|
||||
logger.info(f"上传完成: {serial} {photo_type}")
|
||||
|
||||
# ========== Nav2 导航 ==========
|
||||
|
||||
def _nav2_check_available(self) -> bool:
|
||||
"""检查 Nav2 action server 是否可用"""
|
||||
try:
|
||||
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
|
||||
except:
|
||||
return False
|
||||
|
||||
def _nav2_send_goal(self, x: float, y: float, yaw: float, timeout_sec: float = 120.0) -> bool:
|
||||
"""
|
||||
通过 ros2 action 发送 navigate_to_pose 目标并等待结果
|
||||
|
||||
Returns:
|
||||
是否成功到达
|
||||
"""
|
||||
# 检查 Nav2 可用性
|
||||
if not self._nav2_check_available():
|
||||
logger.error("Nav2 action server 不可用，请确保 navigation2_active.launch.py 已启动")
|
||||
return False
|
||||
|
||||
# 构建四元数
|
||||
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"Nav2 发送导航目标: ({x:.3f}, {y:.3f}), yaw={math.degrees(yaw):.1f}°")
|
||||
|
||||
# 发送 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}'"
|
||||
|
||||
try:
|
||||
process = subprocess.Popen(
|
||||
full_cmd, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
|
||||
)
|
||||
|
||||
succeeded = False
|
||||
check_interval = 1.0
|
||||
elapsed = 0.0
|
||||
|
||||
while elapsed < timeout_sec:
|
||||
import select
|
||||
reads = [process.stdout]
|
||||
ready, _, _ = select.select(reads, [], [], check_interval)
|
||||
elapsed += check_interval
|
||||
|
||||
if process.stdout in ready:
|
||||
line = process.stdout.readline()
|
||||
if not line:
|
||||
break
|
||||
line_stripped = line.strip()
|
||||
|
||||
if "succeeded" in line_stripped.lower():
|
||||
logger.info("✅ Nav2 导航成功到达目标")
|
||||
succeeded = True
|
||||
break
|
||||
elif "failed" in line_stripped.lower() or "aborted" in line_stripped.lower():
|
||||
logger.warning(f"⚠️ Nav2 导航失败: {line_stripped}")
|
||||
break
|
||||
elif "canceled" in line_stripped.lower() or "cancelled" in line_stripped.lower():
|
||||
logger.info("Nav2 导航被取消")
|
||||
break
|
||||
|
||||
if process.poll() is not None:
|
||||
# 进程结束但没读到 succeeded
|
||||
break
|
||||
|
||||
# 确保进程结束
|
||||
if process.poll() is None:
|
||||
process.terminate()
|
||||
try:
|
||||
process.wait(timeout=3)
|
||||
except subprocess.TimeoutExpired:
|
||||
process.kill()
|
||||
|
||||
return succeeded
|
||||
|
||||
except Exception as e:
|
||||
logger.error(f"Nav2 导航异常: {e}")
|
||||
return False
|
||||
|
||||
def _nav2_go_to_point(self, x: float, y: float, yaw: float = 0.0,
|
||||
timeout_sec: float = 120.0) -> bool:
|
||||
"""
|
||||
AGV 通过 Nav2 导航到目标点位
|
||||
|
||||
Args:
|
||||
x, y: 目标世界坐标（米）
|
||||
yaw: 目标朝向（弧度）
|
||||
timeout_sec: 超时时间
|
||||
|
||||
Returns:
|
||||
是否成功到达
|
||||
"""
|
||||
return self._nav2_send_goal(x, y, yaw, timeout_sec)
|
||||
|
||||
def _nav2_cancel(self):
|
||||
"""取消当前 Nav2 导航"""
|
||||
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'"
|
||||
try:
|
||||
subprocess.run(cancel_cmd, shell=True, timeout=3)
|
||||
logger.info("Nav2 导航已取消")
|
||||
except:
|
||||
pass
|
||||
|
||||
def _run_ros2_cmd(self, cmd: str, timeout: float = 5.0) -> tuple:
|
||||
"""执行 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 get_status(self) -> dict:
|
||||
|
||||
Reference in New Issue
Block a user