PoseStamped()的使用方法与示例

PoseStamped() 是 ROS 中的一个消息类型，用于表示一个带有时间戳的位姿信息。它包含一个 Header 和一个 Pose 成员变量。

Header 用于存储时间戳和坐标系的信息，它定义了以下几个成员变量：

- uint32 seq：序列号，用于标识消息的顺序

- time stamp：时间戳，表示消息的时间

- string frame_id：坐标系的名称，表示该位姿相对于哪个坐标系进行描述

Pose 用于存储位姿信息，它是一个 PoseWithCovariance 类型的消息，包含了位置信息和姿态信息。

PoseStamped() 的常用方法有：

1. 初始化一个 PoseStamped 对象

可以通过 PoseStamped() 构造函数来初始化一个空的 PoseStamped 对象。

2. 设置 Header 的信息

可以通过调用 pose_stamped.header.seq = seq 来设置序列号，pose_stamped.header.stamp = stamp 来设置时间戳，pose_stamped.header.frame_id = frame_id 来设置坐标系的名称。

3. 设置 Pose 的信息

可以通过调用 pose_stamped.pose.position.x = x 来设置位置的 x 坐标，pose_stamped.pose.position.y = y 来设置位置的 y 坐标，pose_stamped.pose.position.z = z 来设置位置的 z 坐标，pose_stamped.pose.orientation.x = qx 来设置姿态的 x 分量，pose_stamped.pose.orientation.y = qy 来设置姿态的 y 分量，pose_stamped.pose.orientation.z = qz 来设置姿态的 z 分量，pose_stamped.pose.orientation.w = qw 来设置姿态的 w 分量。

下面是一个使用 PoseStamped() 的示例：

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>

int main(int argc, char** argv)
{
    // 初始化 ROS 节点
    ros::init(argc, argv, "pose_stamped_example");
    ros::NodeHandle nh;

    // 创建一个发布者，用于发布 PoseStamped 消息
    ros::Publisher pose_stamped_pub = nh.advertise<geometry_msgs::PoseStamped>("pose_stamped", 10);

    // 创建一个 PoseStamped 对象，并设置其中的 Header 和 Pose 信息
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header.seq = 1;
    pose_stamped.header.stamp = ros::Time::now();
    pose_stamped.header.frame_id = "base_link";
    pose_stamped.pose.position.x = 1.0;
    pose_stamped.pose.position.y = 2.0;
    pose_stamped.pose.position.z = 3.0;
    pose_stamped.pose.orientation.x = 0.0;
    pose_stamped.pose.orientation.y = 0.0;
    pose_stamped.pose.orientation.z = 0.0;
    pose_stamped.pose.orientation.w = 1.0;

    // 循环发布 PoseStamped 消息
    ros::Rate rate(1.0);
    while (ros::ok())
    {
        pose_stamped_pub.publish(pose_stamped);
        ROS_INFO("Published PoseStamped message");

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

以上代码创建了一个发布者，用于发布一个带有时间戳和坐标系的位姿信息。在主循环中，不断发布这个位姿信息，并打印出相关信息。可以通过 rostopic 命令来查看发布的消息内容：

$ rostopic echo pose_stamped

可以看到输出的消息中包含了时间戳、坐标系和位姿信息。