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

Nav2导航 + 初始化位置功能

Browse Source 
- nav2_navigator.py: 通过 ros2 action 与 Nav2 通信，修复 YAML 格式（pose 嵌套）
- mission_executor.py: 改用 Nav2 navigate_to_pose action 导航
- app.py: navigate/to 等 API 改用 Nav2Navigator；新增 /api/mission/init_pose
  API，通过子进程调用 rclpy 发布 /initialpose 到 (0,0,0)
- setting.html/js: 任务配置Tab加「初始化位置」按钮

注意：ROS2 daemon (domain=1) 与 Flask 在不同 domain，
Flask 进程内调用 rclpy 会破坏 daemon 状态，需通过子进程调用
This commit is contained in:
ywb
2026-05-16 14:22:10 +08:00
parent f2130acfaa
commit a9840c38ef
5 changed files with 91 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
agv_app/utils/mission_executor.py
+11 -10
View File
@@ -250,15 +250,16 @@ class MissionExecutor:
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}"
f" pose:\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}°")
@@ -266,7 +267,7 @@ class MissionExecutor:
# 发送 goal 并监听反馈和结果
cmd = (
f"ros2 action send_goal /navigate_to_pose "
f"navigation_action_msgs/NavigateToPose "
f"nav2_msgs/action/NavigateToPose "
f"'{pose_yaml}' "
f"--feedback"
)
agv_app/utils/nav2_navigator.py
+34 -30
View File
@@ -17,7 +17,7 @@ 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"
ROS2_SETUP_CMD = "export ROS_DOMAIN_ID=1 && source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash"
class Nav2Status(Enum):
@@ -174,21 +174,22 @@ class Nav2Navigator:
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}"
f" pose:\\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"nav2_msgs/action/NavigateToPose "
f"'{pose_yaml}' "
f"--feedback"
)
@@ -309,15 +310,16 @@ class Nav2Navigator:
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}"
f" pose:\\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)
@@ -354,22 +356,24 @@ class Nav2Navigator:
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}"
f" pose:\\n"
f" position:\\n"
f" x: {x}\\n"
f" y: {y}\
"
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"nav2_msgs/action/NavigateThroughPoses "
f"'{poses_yaml}' "
f"--feedback"
)