ros2机器人发布pose

在Robot Operating System (ROS) 2中，发布pose通常涉及将位置（x, y, z）和姿态（roll, pitch, yaw）数据作为一个消息结构发布到网络上，以便其他节点可以订阅并处理这些信息。ROS 2中的`geometry_msgs/PoseWithCovarianceStamped`或`tf2_ros/Broadcaster`等都是常用的工具。

1. 首先，你需要创建一个`PoseWithCovarianceStamped`消息实例，它包含了`PoseStamped`消息（包含位置和姿态），以及时间戳和框架ID：

#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
geometry_msgs::msg::PoseWithCovarianceStamped pose_msg;

2. 设置消息内容，比如：

   pose_msg.header.stamp = ros::Time::now(); // 设置当前的时间戳
   pose_msg.header.frame_id = "world"; // 指定坐标系，如"base_link"或"map"
   pose_msg.pose.position.x = x; // 定位点x坐标
   pose_msg.pose.position.y = y;
   pose_msg.pose.position.z = z;
   pose_msg.pose.orientation.x = roll;
   pose_msg.pose.orientation.y = pitch;
   pose_msg.pose.orientation.z = yaw;
   pose_msg.pose.orientation.w = 1.0; // 四元数的w分量，一般设置为1.0

3. 使用`Publisher`发布这个消息，例如：

   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped> publisher("pose_publisher", msg::msg::PoseWithCovarianceStamped::MSG_TYPE);
   publisher.publish(pose_msg);

4. 如果你想在tf2中广播，可以使用`tf2_ros::TransformBroadcaster`来同步变换数据：

#include <tf2_ros/broadcaster.h>
tf2_ros::TransformBroadcaster broadcaster;

// ... 同样设置header、frame_id等
broadcaster.sendTransform(
    pose_msg.header,
    tf2::Quaternion(roll, pitch, yaw, 1.0),
    pose_msg.header.stamp,
    "destination_frame", // 目标坐标系
    pose_msg.pose.position
);