用C++写一段在ros里面把PoseStamped里的orientation四元素转换成欧拉角的代码

Certainly! Here's a sample code in C++ to convert the orientation quaternion of a `PoseStamped` message to Euler angles in ROS:

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

void poseStampedCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    // Extract the orientation quaternion from the PoseStamped message
    tf2::Quaternion quaternion;
    tf2::fromMsg(msg->pose.orientation, quaternion);

    // Convert quaternion to Euler angles
    tf2::Matrix3x3 matrix(quaternion);
    double roll, pitch, yaw;
    matrix.getRPY(roll, pitch, yaw);

    // Print the Euler angles
    ROS_INFO("Euler angles: Roll = %f, Pitch = %f, Yaw = %f", roll, pitch, yaw);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "pose_stamped_subscriber");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe("/pose_stamped_topic", 1000, poseStampedCallback);

    ros::spin();

    return 0;
}

In this code, we create a subscriber to a topic named `/pose_stamped_topic`, which receives `PoseStamped` messages. Inside the `poseStampedCallback` function, we extract the orientation quaternion from the received message using the `fromMsg` method of the `tf2::Quaternion` class. Then, we create a `Matrix3x3` object from the quaternion and use the `getRPY` method to obtain the Euler angles.

Remember to replace `/pose_stamped_topic` with the actual topic name where your `PoseStamped` messages are being published.

Please make sure to compile this code with the appropriate ROS package dependencies and run it within a ROS environment.