FaulknerWu/smart-inspection
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: AGV位置读取失败 - ROS_DOMAIN_ID环境变量缺失、odom输出管道错误、ANSI转义序列未清理

Browse Source 
问题根因:
1. ros2 daemon默认运行在domain 0，AGV节点在domain 1，导致无法通信
2. _run_ros2_cmd通过daemon通信，daemon忽略了shell的ROS_DOMAIN_ID环境变量
3. 修复: 在bash -c内联设置export ROS_DOMAIN_ID=1
4. 修复: 所有ros2 topic echo命令添加--once参数确保只输出一次后退出
5. 修复: 清理ANSI转义序列(\x1b)和RTPS错误信息前缀，避免YAML解析失败
6. 修复: 添加source /opt/ros/humble/setup.bash确保ros2命令可用
7. 修复: get_position和get_battery统一使用_run_ros2_cmd而非独立subprocess调用
This commit is contained in:
ywb
2026-05-15 23:44:21 +08:00
parent 03a6e87730
commit 1dc439dc8e
1 changed files with 18 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_app/utils/agv_controller_ros2.py
+18 -25
View File
@@ -12,7 +12,7 @@ from typing import Tuple, Optional, List
logger = logging.getLogger(__name__)
# ROS2 环境设置
ROS2_SETUP_CMD = "export ROS_DOMAIN_ID=1 && source ~/agv_pro_ros2/install/setup.bash"
ROS2_SETUP_CMD = "export ROS_DOMAIN_ID=1 && source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash"
class AGVController:
@@ -28,7 +28,7 @@ class AGVController:
def _run_ros2_cmd(self, cmd: str, timeout: float = 5.0) -> tuple:
"""执行 ros2 命令"""
full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
full_cmd = f"bash -c 'export ROS_DOMAIN_ID=1 && source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash && {cmd}'"
try:
result = subprocess.run(
full_cmd,
@@ -64,7 +64,7 @@ class AGVController:
# 尝试获取一次位置数据
rc, out, err = self._run_ros2_cmd(
"timeout 5 ros2 topic echo /odom 2>timeout 10 ros2 topic echo /odom --once 2>/dev/null1 | head -1",
"timeout 5 ros2 topic echo /odom --once 2>/dev/null",
timeout=6
)
@@ -89,23 +89,10 @@ class AGVController:
def _publish_cmd_vel(self, linear_x: float = 0.0, linear_y: float = 0.0, angular_z: float = 0.0):
"""发布速度命令到 /cmd_vel"""
# 直接执行，避免引号嵌套问题
msg = f'{{"linear": {{"x": {linear_x}, "y": {linear_y}, "z": 0.0}}, "angular": {{"x": 0.0, "y": 0.0, "z": {angular_z}}}}}'
full_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \"{msg}\" --once'"
try:
result = subprocess.run(
full_cmd,
shell=True,
capture_output=True,
text=True,
timeout=5
)
if result.returncode != 0:
logger.warning(f"发布 cmd_vel 失败: {result.stderr.strip()}")
except subprocess.TimeoutExpired:
logger.warning("发布 cmd_vel 超时")
except Exception as e:
logger.warning(f"发布 cmd_vel 失败: {e}")
rc, out, err = self._run_ros2_cmd(f'ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{msg}" --once')
if rc != 0:
logger.warning(f"发布 cmd_vel 失败: {err}")
def move_forward(self, speed: float = 0.5, duration: float = None):
"""前进"""
@@ -173,14 +160,19 @@ class AGVController:
try:
# 从 /odom topic 获取位置
rc, out, err = self._run_ros2_cmd(
"timeout 5 ros2 topic echo /odom 2>timeout 10 ros2 topic echo /odom --once 2>/dev/null1 | head -1",
"timeout 5 ros2 topic echo /odom --once 2>/dev/null",
timeout=6
)
if rc == 0 and out:
# 解析 odom 消息 (YAML 格式)
# ros2 topic echo 输出可能含多个 --- 分隔的文档，只取第一个
import yaml
yaml_str = out.split('---')[0]
# 注意：RTPS 错误信息混在 stderr 通过管道进入 stdout，需要去掉
import re, yaml
# 去掉 ANSI 转义序列
clean = re.sub(r'\x1b\[[0-9;]*[a-zA-Z]', '', out)
# 去掉时间戳前缀行（形如 "2026-05-15 23:39:38.824 [RTPS_TRANSPORT_SHM Error] ..."）
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", {})
@@ -209,13 +201,14 @@ class AGVController:
try:
# 从 /voltage topic 获取电压
rc, out, err = self._run_ros2_cmd(
"timeout 5 ros2 topic echo /voltage 2>timeout 10 ros2 topic echo /voltage --once 2>/dev/null1 | head -1",
"timeout 5 ros2 topic echo /voltage --once 2>/dev/null",
timeout=6
)
if rc == 0 and out:
# 解析电压消息（ros2 topic echo 可能输出多文档 YAML）
import yaml
yaml_str = out.split('---')[0]
import re, yaml
clean = re.sub(r'\x1b\[[0-9;]*[a-zA-Z]', '', out)
yaml_str = clean.split('---')[0].strip()
data = yaml.safe_load(yaml_str)
if data:
self._voltage = data.get("data", 0.0)