geometry_msgs::PoseStamped 使用例子

以下是使用geometry_msgs::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;

  // 创建一个Publisher来发布PoseStamped消息
  ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pose_stamped_topic", 10);

  // 创建一个PoseStamped消息
  geometry_msgs::PoseStamped pose_stamped;

  // 设置PoseStamped消息的header部分
  pose_stamped.header.stamp = ros::Time::now();
  pose_stamped.header.frame_id = "base_link";

  // 设置PoseStamped消息的位姿信息
  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_pub.publish(pose_stamped);
    ros::spinOnce();
    rate.sleep();
  }

  return 0;
}

这个例子演示了如何创建一个PoseStamped消息，并将其发布到ROS topic中。在这个例子中，我们设置了PoseStamped消息的header部分，包括timestamp和frame_id。然后，我们设置了位姿信息，包括位置和姿态。最后，我们使用一个循环来定期发布PoseStamped消息。你可以根据自己的需求修改消息的内容和发布频率。