260416

目前为止我们已经完成了我们跟踪目的的前两个步骤，接下来来到我们的第三个步骤——实现实时旗帜跟随 + 避障。

因为我们要读取 Gazebo 传回的 pose 信息了，因此我们需要先配置一下我们的环境 ros_gz_bridge 相关依赖信息。

首先是将它添加到我们 IDE 的 python 分析路径中：

    "python.analysis.extraPaths": [
        "/opt/ros/jazzy/lib/python3.12/site-packages",
        "/usr/lib/python3/dist-packages",
        "/opt/ros/jazzy/opt/gz_msgs_vendor/lib/python",
        "${workspaceFolder}/install/kibot_one_interface/lib/python3.12/site-packages",
        "${workspaceFolder}/install/kibot_one_control/lib/python3.12/site-packages"
    ]

这里我们将 /opt/ros/jazzy/opt/gz_msgs_vendor/lib/python 给添加到了我们的路径中。

然后是 ~/.bash_profile.jazzy，需要额外写入一行 PYTHON 路径：

export PYTHONPATH=/usr/lib/python3/dist-packages:/opt/ros/jazzy/opt/gz_msgs_vendor/lib/python:$PYTHONPATH

接下来我们就可以开始进行开发了。

我们首先要实现的是，旗帜实时发布自己的位姿。

我们先在我们旗帜的 sdf 里引入这个插件：

    <plugin
      filename="gz-sim-pose-publisher-system"
      name="gz::sim::systems::PosePublisher">
      <publish_model_pose>true</publish_model_pose>
      <publish_link_pose>false</publish_link_pose>
      <use_pose_vector_msg>false</use_pose_vector_msg>
      <static_publisher>false</static_publisher>
      <update_frequency>10</update_frequency>
    </plugin>

这个插件是用来发布模型的位姿的一个插件，我们设置为发布整个模型的位姿，不发布其中 link 的位姿。

同时，我们关闭了 use_pose_vector_msg，这个影响其实不大，因为我们实际发布的就只有 model 一个对象的消息，而非所有 link，是否打包对我们来说没有多少影响。

然后在 src/kibot_one_sim/config/ros_gz_bridge.yaml，我们新增了 /flag_pose 这一 ros2 话题来对接 gz 的 /model/follow_flag/pose，以此来获取旗帜的位置。

- ros_topic_name: "/flag_pose"
  gz_topic_name: "/model/follow_flag/pose"
  ros_type_name: "geometry_msgs/msg/PoseStamped"
  gz_type_name: "gz.msgs.Pose"
  direction: GZ_TO_ROS
  frame_id: "odom"

启动！然后我们运行看看能不能获取到这个话题的内容：

(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotOne$ ros2 topic echo /flag_pose --once
header:
  stamp:
    sec: 0
    nanosec: 100000000
  frame_id: kibot_one_world
pose:
  position:
    x: 2.5
    y: 1.0
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
---
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotOne$ ros2 topic echo /flag_pose --once
header:
  stamp:
    sec: 1
    nanosec: 0
  frame_id: kibot_one_world
pose:
  position:
    x: 2.5
    y: 1.0
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
---

可以看到效果符合预期。

值得注意的是，我们只有当 Gazebo 处于正在模拟中的状态才能收到这个 topic 消息，否则会一直卡死。

我们在 Gazebo 中移动一下旗帜的位置看看：

(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotOne$ ros2 topic echo /flag_pose --once
header:
  stamp:
    sec: 2
    nanosec: 200000000
  frame_id: kibot_one_world
pose:
  position:
    x: 2.5
    y: -2.9728758335113525
    z: 0.0
  orientation:
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
---

可以看到我们移动后，pose正确反映到了我们的这个话题中。

就这样，我们成功实时监控了旗帜的位置并发布到了话题中。

以上。