可导航
This commit is contained in:
ywb
@@ -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,
|
||||
|
||||
+162
-49
@@ -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("导航已停止")
|
||||
|
||||
Reference in New Issue
Block a user