初始化位置

agv_app/app.py

@@ -676,12 +676,34 @@ def api_mission_init_pose():
     """将 AMCL 初始位置设为 (0,0,0)，无需 RViz"""
     try:
         script = "/tmp/ros2_init_pose.sh"
+        yaml_content = (
+            "pose:\n"
+            "  header:\n"
+            "    stamp:\n"
+            "      sec: 0\n"
+            "      nanosec: 0\n"
+            "    frame_id: map\n"
+            "  pose:\n"
+            "    position:\n"
+            "      x: 0.0\n"
+            "      y: 0.0\n"
+            "      z: 0.0\n"
+            "    covariance: [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"
+            "    orientation:\n"
+            "      x: 0.0\n"
+            "      y: 0.0\n"
+            "      z: 0.0\n"
+            "      w: 1.0\n"
+        )
+        goal_file = "/tmp/nav2_goal_{}.yaml".format(os.getpid())
+        with open(goal_file, "w") as f:
+            f.write(yaml_content)
         lines = [
             "#!/bin/bash",
-            "export ROS_DOMAIN_ID=0",
+            "export ROS_DOMAIN_ID=1",
             "source /opt/ros/humble/setup.bash",
-            "source ~/agv_pro_ros2/install/setup.bash",
-            "python3 /tmp/publish_init_pose.py",
+            "source /home/elephant/agv_pro_ros2/install/setup.bash",
+            f'ros2 topic pub --once /initialpose geometry_msgs/PoseWithCovarianceStamped "$(cat {goal_file})"',
         ]
         with open(script, "w") as f:
             f.write("\n".join(lines) + "\n")
@@ -689,9 +711,9 @@ def api_mission_init_pose():
         result = subprocess.run(
             [script],
             capture_output=True, text=True, timeout=12,
-            env={**os.environ, "ROS_DOMAIN_ID": "0"}
+            env={**os.environ, "ROS_DOMAIN_ID": "1"}
         )
-        logger.info(f"init_pose: rc={result.returncode}")
+        logger.info(f"init_pose: rc={result.returncode}, stdout={result.stdout[:200]}, stderr={result.stderr[:200]}")
         return jsonify({"ok": True, "message": "初始位置已设为 (0,0,0)"})
     except Exception as e:
         logger.error(f"初始化位置失败: {e}")