目的：

將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論壇