导航修复
This commit is contained in:
ywb
@@ -44,41 +44,23 @@ class AGVController:
|
||||
return -1, "", str(e)
|
||||
|
||||
def connect(self) -> bool:
|
||||
"""连接 AGV - 检查 ROS2 节点和 topic"""
|
||||
"""连接 AGV - 检查 ROS2 topic 是否可用"""
|
||||
try:
|
||||
# 检查 agv_pro_node 是否运行
|
||||
rc, out, err = self._run_ros2_cmd("ros2 node list")
|
||||
if rc != 0:
|
||||
logger.error(f"ROS2 节点列表获取失败: {err}")
|
||||
return False
|
||||
|
||||
if "/agv_pro_node" not in out:
|
||||
logger.error("agv_pro_node 未运行")
|
||||
return False
|
||||
|
||||
# 检查 /odom topic
|
||||
# 检查 /odom topic 是否存在（比 ros2 node list 更可靠）
|
||||
rc, out, err = self._run_ros2_cmd("ros2 topic list")
|
||||
if rc != 0:
|
||||
logger.error(f"ROS2 topic 列表获取失败: {err}")
|
||||
return False
|
||||
|
||||
if "/odom" not in out:
|
||||
logger.error("/odom topic 不存在")
|
||||
return False
|
||||
|
||||
# 尝试获取一次位置数据
|
||||
rc, out, err = self._run_ros2_cmd(
|
||||
"timeout 5 ros2 topic echo /odom --once 2>/dev/null",
|
||||
timeout=6
|
||||
)
|
||||
|
||||
if rc == 0 and out:
|
||||
self._connected = True
|
||||
self._ros2_available = True
|
||||
logger.info("AGV ROS2 连接成功")
|
||||
return True
|
||||
else:
|
||||
# /odom 可能暂时没数据，但节点存在也算连接成功
|
||||
self._connected = True
|
||||
self._ros2_available = True
|
||||
logger.info("AGV ROS2 连接成功 (节点存在，等待 odom 数据)")
|
||||
return True
|
||||
# /odom 存在即表示 AGV 节点在运行
|
||||
self._connected = True
|
||||
self._ros2_available = True
|
||||
logger.info("AGV ROS2 连接成功 (/odom topic 存在)")
|
||||
return True
|
||||
|
||||
except Exception as e:
|
||||
logger.error(f"AGV 连接失败: {e}")
|
||||
@@ -154,13 +136,13 @@ class AGVController:
|
||||
self._publish_cmd_vel(0, 0, 0)
|
||||
|
||||
def get_position(self) -> Optional[List[float]]:
|
||||
"""获取 AGV 当前位置 [x, y, yaw]"""
|
||||
"""获取 AGV 当前位置 [x, y, yaw]（使用 AMCL 全局定位，比 /odom 更准确）"""
|
||||
if not self.is_connected():
|
||||
return None
|
||||
try:
|
||||
# 从 /odom topic 获取位置
|
||||
# 优先从 /amcl_pose 获取全局定位位置（更准确）
|
||||
rc, out, err = self._run_ros2_cmd(
|
||||
"timeout 5 ros2 topic echo /odom --once 2>/dev/null",
|
||||
"timeout 5 ros2 topic echo /amcl_pose --once 2>/dev/null",
|
||||
timeout=6
|
||||
)
|
||||
if rc == 0 and out:
|
||||
|
||||
+106
-248
@@ -2,7 +2,7 @@
|
||||
Nav2 导航模块 - 通过 ROS2 Action Server 控制 AGV 导航
|
||||
使用 Nav2 的 navigate_to_pose 和 navigate_through_poses action
|
||||
|
||||
通过写入临时 shell 脚本再执行，避免 bash -c 单引号转义问题
|
||||
通过 stdin pipe 发送 goal YAML，避免 bash $() 替换破坏 YAML 格式
|
||||
"""
|
||||
import time
|
||||
import math
|
||||
@@ -10,6 +10,7 @@ import logging
|
||||
import threading
|
||||
import subprocess
|
||||
import os
|
||||
import re
|
||||
from typing import List, Tuple, Optional, Dict
|
||||
from enum import Enum
|
||||
|
||||
@@ -39,55 +40,12 @@ class Nav2Status(Enum):
|
||||
class Nav2Navigator:
|
||||
"""Nav2 导航器 — 通过 ros2 action /navigate_to_pose 与 Nav2 通信"""
|
||||
|
||||
NAVIGATE_SCRIPT = "/tmp/nav2_send_goal.sh"
|
||||
|
||||
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._build_script()
|
||||
|
||||
def _build_script(self):
|
||||
"""生成 Nav2 action 发送脚本（通过 heredoc 避免所有 bash 转义问题）"""
|
||||
script = self.NAVIGATE_SCRIPT
|
||||
content = [
|
||||
"#!/bin/bash",
|
||||
"source /opt/ros/humble/setup.bash",
|
||||
"source /home/elephant/agv_pro_ros2/install/setup.bash",
|
||||
"",
|
||||
"TARGET_X=${1:-0}",
|
||||
"TARGET_Y=${2:-0}",
|
||||
"TARGET_YAW=${3:-0}",
|
||||
"",
|
||||
"# 计算四元数",
|
||||
"YAW_QZ=$(python3 -c \"import math; print(math.sin($TARGET_YAW / 2.0))\")",
|
||||
"YAW_QW=$(python3 -c \"import math; print(math.cos($TARGET_YAW / 2.0))\")",
|
||||
"",
|
||||
"# 通过 stdin pipe 发送 goal",
|
||||
'cat << \'GOAL\' | ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose - --feedback',
|
||||
"pose:",
|
||||
" header:",
|
||||
" stamp:",
|
||||
" sec: 0",
|
||||
" nanosec: 0",
|
||||
" frame_id: map",
|
||||
" pose:",
|
||||
" position:",
|
||||
" x: $TARGET_X",
|
||||
" y: $TARGET_Y",
|
||||
" z: 0.0",
|
||||
" orientation:",
|
||||
" x: 0.0",
|
||||
" y: 0.0",
|
||||
" z: $YAW_QZ",
|
||||
" w: $YAW_QW",
|
||||
"GOAL",
|
||||
]
|
||||
with open(script, "w") as f:
|
||||
f.write("\n".join(content) + "\n")
|
||||
os.chmod(script, 0o755)
|
||||
|
||||
def _check_nav2_available(self) -> bool:
|
||||
"""检查 Nav2 action server 是否可用（带缓存 10 秒）"""
|
||||
@@ -95,7 +53,6 @@ class Nav2Navigator:
|
||||
if hasattr(self, '_nav2_check_time') and hasattr(self, '_nav2_check_result'):
|
||||
if time_mod.time() - self._nav2_check_time < 10:
|
||||
return self._nav2_check_result
|
||||
# 写入脚本文件执行，兼容 Flask subprocess 环境
|
||||
script = (
|
||||
"#!/bin/bash\n"
|
||||
"source /opt/ros/humble/setup.bash 2>/dev/null\n"
|
||||
@@ -106,24 +63,20 @@ class Nav2Navigator:
|
||||
script_file = "/tmp/nav2_check.sh"
|
||||
with open(script_file, "w") as f:
|
||||
f.write(script)
|
||||
import os
|
||||
os.chmod(script_file, 0o755)
|
||||
r = subprocess.run([script_file], capture_output=True, text=True, timeout=8)
|
||||
rc = r.returncode
|
||||
out = r.stdout
|
||||
err = r.stderr
|
||||
result = rc == 0 and ("/navigate_to_pose" in out or "navigate_to_pose" in out)
|
||||
logger.debug(f"_check_nav2_available: rc={rc}, out={out[:100]}, result={result}")
|
||||
result = r.returncode == 0 and ("/navigate_to_pose" in r.stdout or "navigate_to_pose" in r.stdout)
|
||||
logger.debug(f"_check_nav2_available: rc={r.returncode}, result={result}")
|
||||
self._nav2_check_result = result
|
||||
self._nav2_check_time = time_mod.time()
|
||||
return result
|
||||
|
||||
def _get_current_pose(self) -> List[float]:
|
||||
"""从 /odom 获取当前位置 [x, y, yaw]"""
|
||||
"""从 /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 re, yaml
|
||||
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]
|
||||
@@ -139,35 +92,22 @@ class Nav2Navigator:
|
||||
self._current_pose = [x, y, yaw]
|
||||
return self._current_pose
|
||||
except Exception as e:
|
||||
logger.debug(f"获取 odom 失败: {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:
|
||||
"""
|
||||
导航到目标坐标（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
|
||||
|
||||
# 重置状态，允许发起新导航
|
||||
self.status = Nav2Status.IDLE
|
||||
self._result_status = None
|
||||
|
||||
if not self._check_nav2_available():
|
||||
logger.error("Nav2 action server 不可用，请确保 navigation2_active.launch.py 已启动")
|
||||
logger.error("Nav2 action server 不可用")
|
||||
return False
|
||||
|
||||
if yaw is None:
|
||||
@@ -176,12 +116,10 @@ class Nav2Navigator:
|
||||
|
||||
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)
|
||||
@@ -199,24 +137,32 @@ class Nav2Navigator:
|
||||
|
||||
return True
|
||||
|
||||
def _parse_result_status(self, combined: str) -> Optional[str]:
|
||||
"""从 ros2 action send_goal 输出中解析导航结果状态"""
|
||||
result_idx = combined.find("Result:")
|
||||
if result_idx < 0:
|
||||
return None
|
||||
# 匹配 "status: STATUS_SUCCEEDED" 或 "status: SUCCEEDED" 等格式
|
||||
status_m = re.search(r'status\s*:\s*(\w+)', combined[result_idx:])
|
||||
if status_m:
|
||||
st = status_m.group(1).upper()
|
||||
if 'SUCCEEDED' in st:
|
||||
return "succeeded"
|
||||
elif 'FAILED' in st or 'ABORTED' in st:
|
||||
return "failed"
|
||||
elif 'CANCELED' in st or 'CANCELLED' in st:
|
||||
return "cancelled"
|
||||
logger.warning(f"未知的导航状态: {st}")
|
||||
return None
|
||||
|
||||
def _nav_thread_func(self, x: float, y: float, yaw: float, timeout_sec: float):
|
||||
"""导航执行线程 — 使用 heredoc 脚本避免 bash 转义问题"""
|
||||
"""导航执行线程 — 使用 stdin pipe 发送 goal YAML"""
|
||||
try:
|
||||
logger.info(f"Nav2 线程启动: x={x}, y={y}, yaw={yaw}, timeout={timeout_sec}s")
|
||||
start_time = time.time()
|
||||
elapsed = 0.0
|
||||
check_interval = 1.0
|
||||
result_received = False
|
||||
|
||||
# 使用 pipe 方式运行导航
|
||||
# 通过 subprocess.Popen + stdin heredoc 避免所有引号转义问题
|
||||
setup = "source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash && export ROS_DOMAIN_ID=1"
|
||||
qz = math.sin(yaw / 2.0)
|
||||
qw = math.cos(yaw / 2.0)
|
||||
|
||||
# 构建 goal YAML（用 bash heredoc 方式避免转义问题）
|
||||
# 写入临时文件
|
||||
import tempfile
|
||||
goal_yaml = (
|
||||
f"pose:\n"
|
||||
f" header:\n"
|
||||
@@ -234,23 +180,27 @@ class Nav2Navigator:
|
||||
f" y: 0.0\n"
|
||||
f" z: {qz}\n"
|
||||
f" w: {qw}\n"
|
||||
f"behavior_tree: ''\n"
|
||||
)
|
||||
|
||||
goal_file = "/tmp/nav2_goal_{}.yaml".format(os.getpid())
|
||||
# 写入临时文件，用 $(cat file) 方式传递（stdin pipe "-" 在 Humble 有 bug）
|
||||
import tempfile
|
||||
goal_file = "/tmp/nav2_goal_{}_{}.yaml".format(os.getpid(), int(time.time()))
|
||||
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\''
|
||||
)
|
||||
# 写一个临时脚本执行导航（避免引号嵌套地狱）
|
||||
script_file = "/tmp/nav2_navigate_{}_{}.sh".format(os.getpid(), int(time.time()))
|
||||
with open(script_file, "w") as f:
|
||||
f.write('#!/bin/bash\n')
|
||||
f.write('source /opt/ros/humble/setup.bash\n')
|
||||
f.write('source /home/elephant/agv_pro_ros2/install/setup.bash\n')
|
||||
f.write('export ROS_DOMAIN_ID=1\n')
|
||||
f.write('ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "$(cat {})" --feedback\n'.format(goal_file))
|
||||
os.chmod(script_file, 0o755)
|
||||
|
||||
process = subprocess.Popen(
|
||||
cmd, shell=True, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
|
||||
proc = subprocess.Popen(
|
||||
['bash', '-l', script_file],
|
||||
stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
|
||||
)
|
||||
|
||||
stdout_lines = []
|
||||
@@ -263,155 +213,72 @@ 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)
|
||||
t_out = threading.Thread(target=reader, args=(proc.stdout, stdout_lines))
|
||||
t_err = threading.Thread(target=reader, args=(proc.stderr, stderr_lines))
|
||||
t_out.start()
|
||||
t_err.start()
|
||||
|
||||
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()
|
||||
|
||||
# 用 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 检查一次结果）
|
||||
# 轮询等待 action 完成
|
||||
result_received = False
|
||||
for _ in range(60): # 最多等 120 秒
|
||||
max_checks = int(timeout_sec / 2)
|
||||
for _ in range(max_checks):
|
||||
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:
|
||||
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:
|
||||
time.sleep(check_interval)
|
||||
elapsed = time.time() - start_time
|
||||
|
||||
full_out = "".join(stdout_lines)
|
||||
full_err = "".join(stderr_lines)
|
||||
combined = full_out + full_err
|
||||
|
||||
# 查找 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:
|
||||
# 检查是否有 YAML 解析错误
|
||||
if "Traceback" in combined and "Goal accepted" not in combined:
|
||||
logger.error(f"ros2 action send_goal YAML 解析失败: {combined[:500]}")
|
||||
self._result_status = "failed"
|
||||
result_received = True
|
||||
break
|
||||
|
||||
# 善后
|
||||
# 检查是否未被接受
|
||||
if "Goal accepted" not in combined and elapsed > 15:
|
||||
logger.warning(f"导航目标 15 秒内未被接受，输出: {combined[:300]}")
|
||||
|
||||
# 解析 Result
|
||||
parsed = self._parse_result_status(combined)
|
||||
if parsed:
|
||||
self._result_status = parsed
|
||||
result_received = True
|
||||
if parsed == "succeeded":
|
||||
logger.info(f"✅ Nav2 导航成功到达目标 ({x:.3f}, {y:.3f})")
|
||||
elif parsed == "failed":
|
||||
logger.warning(f"⚠️ Nav2 导航失败")
|
||||
else:
|
||||
logger.info("Nav2 导航被取消")
|
||||
break
|
||||
|
||||
# 进程已结束但无 Result
|
||||
if proc.poll() is not None and not result_received:
|
||||
logger.error(f"导航进程提前退出，rc={proc.returncode}，输出: {combined[:300]}")
|
||||
self._result_status = "failed"
|
||||
result_received = True
|
||||
break
|
||||
|
||||
if self._cancel_event.is_set():
|
||||
break
|
||||
|
||||
# 清理
|
||||
stop_reading.set()
|
||||
if act_proc.poll() is None:
|
||||
act_proc.terminate()
|
||||
if proc.poll() is None:
|
||||
proc.terminate()
|
||||
try:
|
||||
act_proc.wait(timeout=3)
|
||||
proc.wait(timeout=3)
|
||||
except subprocess.TimeoutExpired:
|
||||
act_proc.kill()
|
||||
|
||||
out_thread.join(timeout=2)
|
||||
err_thread.join(timeout=2)
|
||||
proc.kill()
|
||||
t_out.join(timeout=2)
|
||||
t_err.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\''
|
||||
setup = "source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash && export ROS_DOMAIN_ID=1"
|
||||
cancel_cmd = f'bash -l -c \'{setup} && ros2 action cancel /navigate_to_pose 2>/dev/null || true\''
|
||||
subprocess.run(cancel_cmd, shell=True, timeout=3)
|
||||
self._result_status = "cancelled"
|
||||
|
||||
@@ -429,7 +296,7 @@ class Nav2Navigator:
|
||||
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）"""
|
||||
"""导航通过多个路径点"""
|
||||
if self.status == Nav2Status.NAVIGATING:
|
||||
logger.warning("导航正在进行中，请先停止")
|
||||
return False
|
||||
@@ -461,7 +328,6 @@ class Nav2Navigator:
|
||||
try:
|
||||
setup = "source /opt/ros/humble/setup.bash && source /home/elephant/agv_pro_ros2/install/setup.bash && export ROS_DOMAIN_ID=1"
|
||||
|
||||
# 构建 poses heredoc
|
||||
poses_lines = []
|
||||
for i, (x, y, yaw) in enumerate(poses):
|
||||
qz = math.sin(yaw / 2.0)
|
||||
@@ -493,6 +359,7 @@ class Nav2Navigator:
|
||||
)
|
||||
|
||||
stdout_lines = []
|
||||
stderr_lines = []
|
||||
stop_reading = threading.Event()
|
||||
|
||||
def reader(pipe, lines):
|
||||
@@ -502,14 +369,16 @@ class Nav2Navigator:
|
||||
lines.append(line)
|
||||
|
||||
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()
|
||||
|
||||
try:
|
||||
process.stdin.write(poses_yaml + "\n")
|
||||
process.stdin.flush()
|
||||
process.stdin.close()
|
||||
except Exception as e:
|
||||
logger.error(f"写入 poses heredoc 失败: {e}")
|
||||
logger.error(f"写入 poses YAML 失败: {e}")
|
||||
self.status = Nav2Status.FAILED
|
||||
return
|
||||
|
||||
@@ -520,29 +389,17 @@ class Nav2Navigator:
|
||||
full_err = "".join(stderr_lines)
|
||||
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(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
|
||||
parsed = self._parse_result_status(combined)
|
||||
if parsed:
|
||||
self._result_status = parsed
|
||||
result_received = True
|
||||
if parsed == "succeeded":
|
||||
logger.info(f"✅ Nav2 路径点导航成功完成 {len(poses)} 个点")
|
||||
break
|
||||
|
||||
if process.poll() is not None and not result_received:
|
||||
self._result_status = "failed"
|
||||
result_received = True
|
||||
break
|
||||
|
||||
stop_reading.set()
|
||||
@@ -550,6 +407,7 @@ class Nav2Navigator:
|
||||
process.terminate()
|
||||
process.wait(timeout=3)
|
||||
out_thread.join(timeout=2)
|
||||
err_thread.join(timeout=2)
|
||||
|
||||
if self._result_status == "succeeded":
|
||||
self.status = Nav2Status.SUCCEEDED
|
||||
@@ -583,4 +441,4 @@ class Nav2Navigator:
|
||||
|
||||
def get_current_position(self) -> List[float]:
|
||||
"""获取当前位置 [x, y, yaw]"""
|
||||
return self._get_current_pose()
|
||||
return self._get_current_pose()
|
||||
|
||||
Reference in New Issue
Block a user