geometry_msgs::TransformStamped与geometry_msgs::PoseStamped消息互转

geometry_msgs::TransformStamped与geometry_msgs::PoseStamped消息互转

下面是一个将geometry_msgs::TransformStamped转换为geometry_msgs::PoseStamped的示例代码：

geometry_msgs::PoseStamped transformToPose(const geometry_msgs::TransformStamped& transformStamped)
{
    geometry_msgs::PoseStamped poseStamped;
    poseStamped.header = transformStamped.header;

    // Convert the translation and rotation from the transform to the pose
    poseStamped.pose.position.x = transformStamped.transform.translation.x;
    poseStamped.pose.position.y = transformStamped.transform.translation.y;
    poseStamped.pose.position.z = transformStamped.transform.translation.z;
    poseStamped.pose.orientation.x = transformStamped.transform.rotation.x;
    poseStamped.pose.orientation.y = transformStamped.transform.rotation.y;
    poseStamped.pose.orientation.z = transformStamped.transform.rotation.z;
    poseStamped.pose.orientation.w = transformStamped.transform.rotation.w;

    return poseStamped;
}

同样地，我们可以编写一个将geometry_msgs::PoseStamped转换为geometry_msgs::TransformStamped的示例代码：

geometry_msgs::TransformStamped poseToTransform(const geometry_msgs::PoseStamped& poseStamped)
{
    geometry_msgs::TransformStamped transformStamped;
    transformStamped.header = poseStamped.header;

    // Convert the pose's translation and rotation to the transform
    transformStamped.transform.translation.x = poseStamped.pose.position.x;
    transformStamped.transform.translation.y = poseStamped.pose.position.y;
    transformStamped.transform.translation.z = poseStamped.pose.position.z;
    transformStamped.transform.rotation.x = poseStamped.pose.orientation.x;
    transformStamped.transform.rotation.y = poseStamped.pose.orientation.y;
    transformStamped.transform.rotation.z = poseStamped.pose.orientation.z;
    transformStamped.transform.rotation.w = poseStamped.pose.orientation.w;

    return transformStamped;
}
在这两个示例中，我们使用了tf2库中的tf2_geometry_msgs头文件中的函数来完成转换。这些函数使用了tf2库中的transformations功能，其中包括了从四元数和欧拉角之间的相互转换、从变换矩阵到四元数之间的相互转换等。

geometry_msgs::PointStamped与geometry_msgs::PoseStamped互转

    geometry_msgs::PoseStamped pointStampedToPose(const geometry_msgs::PointStamped& PointStamped)
    {
        geometry_msgs::PoseStamped poseStamped;
        poseStamped.header = PointStamped.header;
        poseStamped.pose.position.x = PointStamped.point.x;
        poseStamped.pose.position.y = PointStamped.point.y;
        poseStamped.pose.position.z = PointStamped.point.z;
        poseStamped.pose.orientation.x = 0;
        poseStamped.pose.orientation.y = 0;
        poseStamped.pose.orientation.z = 0;
        poseStamped.pose.orientation.w = 1;

        return poseStamped;
    }