cartographer输出姿态角（ROS中四元数转欧拉角）

目的：

将cartographer输出位姿（/tf）中四元数转换成欧拉角（姿态角）输出（C++语言）

实现如下：

首先在cartographer_ros/node.h中添加发布器声明

::ros::Publisher rpy_publisher;

cartographer_ros/node.cc中

//添加库
#include "tf/transform_datatypes.h"

void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
  carto::common::MutexLocker lock(&mutex_);
  
  //publish pose（添加）
  // 告诉 master 我们将要在 rpy话题上发布消息类型为geometry_msgs::PointStamped  的消息。
  //第二个参数是发布序列的大小。如果我们发布的消息的频率太高，
  //缓冲区中的消息在大于 1000 个的时候就会开始丢弃先前发布的消息。
  rpy_publisher= node_handle_.advertise<geometry_msgs::PointStamped>("rpy",1000); 
  
  for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {

    } else {

    }

    if (trajectory_state.published_to_tracking != nullptr) {
      
      //publish pose（添加）
      //创建一个geometry_msgs::PointStamped类型的消息rpy
      geometry_msgs::PointStamped rpy;
      
      if (options_.provide_odom_frame) {
    
      } else {
        stamped_transform.header.frame_id = options_.map_frame;
        stamped_transform.child_frame_id = options_.published_frame;
        stamped_transform.transform = ToGeometryMsgTransform(
            tracking_to_map * (*trajectory_state.published_to_tracking));
        tf_broadcaster_.sendTransform(stamped_transform);
	
	//publish pose(添加)
	//tf::quaternionMsgToTF函数取/tf中四元数消息转换为四元数
	tf::Quaternion quat;
	tf::quaternionMsgToTF(stamped_transform.transform.rotation, quat);
	
	//tf::Matrix3x3().getRPY()函数将四元数转换为rpy(分别为绕xyz)
	double roll, pitch, yaw;
	tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
	
	//为rpy消息赋值
	rpy.header.stamp = stamped_transform.header.stamp;
	rpy.point.x = roll;
	rpy.point.y = pitch;
	rpy.point.z = yaw;
	//发布器发布
    rpy_publisher.publish(rpy);	
	
      }
    }

要点：

• ROS发布器使用

• 找到需要的消息类型，实际上geometry_msgs::PointStamped有三个值为位置值，此处拿来存储姿态角

• 两个转换函数tf::quaternionMsgToTF()和tf::Matrix3x3().getRPY();

参考：

ROS论坛

cabinx博客

最后

以上就是机智大门为你收集整理的cartographer输出姿态角（ROS中四元数转欧拉角）的全部内容，希望文章能够帮你解决cartographer输出姿态角（ROS中四元数转欧拉角）所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。