260418

接下来就要让我们动态位置的旗帜和我们的小车这两者进行对接了。

至于为什么要对接嘛...因为我们的 /flag_pose 是使用的Gazebo 世界位姿真值，而我们小车控制还是使用的 /odom，如果这个 /odom 是里程计积分量，不是世界真值，那目标一旦多次挪动，两个坐标系就会越来越对不上。

也就会出现在我们移动了旗帜后，第一次如果角度不大的话，往往小车可以正常过去，但是第二次和多几次后，小车就会往看似和旗帜方向无关的方向行驶一段距离后就停下了，不会按照预期来跟随旗帜。

我们先给 src/kibot_one_sim/models/kibot_one_base/model.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>20</update_frequency>
    </plugin>

然后，同样的，在 src/kibot_one_sim/config/ros_gz_bridge.yaml 中加入这个桥接的话题：

- ros_topic_name: "/robot_pose"
  gz_topic_name: "/model/kibot_one_base/pose"
  ros_type_name: "geometry_msgs/msg/PoseStamped"
  gz_type_name: "gz.msgs.Pose"
  direction: GZ_TO_ROS
  frame_id: "world"

在 src/kibot_one_control/kibot_one_control/follow_controller.py 下加入我们的这个参数、变量和订阅者：

class FollowController(Node):
    def __init__(self) -> None:
        # ...
        params = [
            # ...
            ('robot_pose_topic', 'robot_pose'),
            # ...
        ]
        # ...
        self.has_robot_pose = False
        # ...
        self.create_subscription(
            msg_type=PoseStamped,
            topic=cast(str, self.get_parameter('robot_pose_topic').value),
            callback=self._robot_pose_callback,
            qos_profile=10,
        )
        # ...

这里我们引入了 self.has_robot_pose 这个变量，用途是为了作为开关，来避免两个不同来源同时写 robot_x / robot_y / robot_yaw。

因为我们当前的 follow_controller 有两个输入源，分别是 /odom和 /robot_pose，它们都会更新我们提到的上述三个状态。

因此，如果不加 self.has_robot_pose 这个变量，则就会出现输入源混乱的情况。

紧接着，修改我们的 _odom_callback 这个旧的 odom 回调：

    def _odom_callback(self, msg: Odometry) -> None:
        if self.has_robot_pose:
            return

        orientation = msg.pose.pose.orientation
        self.robot_x = msg.pose.pose.position.x
        self.robot_y = msg.pose.pose.position.y
        siny_cosp = 2.0 * (orientation.w * orientation.z + orientation.x * orientation.y)
        cosy_cosp = 1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z)
        self.robot_yaw = math.atan2(siny_cosp, cosy_cosp)

然后新增 _robot_pose_callback 这个回调，作为我们 robot_pose_topic 这个话题的回调：

    def _robot_pose_callback(self, msg: PoseStamped) -> None:
        self.has_robot_pose = True
        self.robot_x = msg.pose.position.x
        self.robot_y = msg.pose.position.y

        orientation = msg.pose.orientation
        siny_cosp = 2.0 * (orientation.w * orientation.z + orientation.x * orientation.y)
        cosy_cosp = 1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z)
        self.robot_yaw = math.atan2(siny_cosp, cosy_cosp)

我们会发现我们的这一段代码出现了两次：

        siny_cosp = 2.0 * (orientation.w * orientation.z + orientation.x * orientation.y)
        cosy_cosp = 1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z)
        self.robot_yaw = math.atan2(siny_cosp, cosy_cosp)

为了确保一致性，我们可以将这个代码整理为一个辅助函数：

    @staticmethod
    def _yaw_from_quaternion(orientation) -> float:
        siny_cosp = 2.0 * (orientation.w * orientation.z + orientation.x * orientation.y)
        cosy_cosp = 1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z)
        return math.atan2(siny_cosp, cosy_cosp)

现在我们的这两个回调就长这样了：

    def _odom_callback(self, msg: Odometry) -> None:
        if self.has_robot_pose:
            return

        self.robot_x = msg.pose.pose.position.x
        self.robot_y = msg.pose.pose.position.y
        self.robot_yaw = self._yaw_from_quaternion(msg.pose.pose.orientation)

    def _robot_pose_callback(self, msg: PoseStamped) -> None:
        self.has_robot_pose = True
        self.robot_x = msg.pose.position.x
        self.robot_y = msg.pose.position.y
        self.robot_yaw = self._yaw_from_quaternion(msg.pose.orientation)

相对来说简化了一点。

接下来就是构建，然后启动看看效果了：