Ros编程-tf::getYaw返回-nan的解决办法

首先看一下函数的使用方式：

static inline double getYaw(const geometry_msgs::Quaternion& msg_q){
Quaternion bt_q;
quaternionMsgToTF(msg_q, bt_q);
return getYaw(bt_q);
}

使用时可以直接代入

nav::msgs::Odometry.pose.pose.orientation

再看一下Odometry的构造：

struct Odometry_
{
  typedef Odometry_ Type;

  Odometry_()
    : header()
    , child_frame_id()
    , pose()
    , twist()  {
    }
  Odometry_(const ContainerAllocator& _alloc)
    : header(_alloc)
    , child_frame_id(_alloc)
    , pose(_alloc)
    , twist(_alloc)  {
  (void)_alloc;
    }



   typedef  ::std_msgs::Header_  _header_type;
  _header_type header;

   typedef std::basic_string, typename ContainerAllocator::template rebind::other >  _child_frame_id_type;
  _child_frame_id_type child_frame_id;

   typedef  ::geometry_msgs::PoseWithCovariance_  _pose_type;
  _pose_type pose;

   typedef  ::geometry_msgs::TwistWithCovariance_  _twist_type;
  _twist_type twist;





  typedef boost::shared_ptr< ::nav_msgs::Odometry_ > Ptr;
  typedef boost::shared_ptr< ::nav_msgs::Odometry_ const> ConstPtr;

}; 

他包含了几个部分，header, child_framd_id , pose , twist , 具体查看一下pose.pose.orientation的定义：

 

struct Quaternion_
{
  typedef Quaternion_ Type;

  Quaternion_()
    : x(0.0)
    , y(0.0)
    , z(0.0)
    , w(0.0)  {
    }
  Quaternion_(const ContainerAllocator& _alloc)
    : x(0.0)
    , y(0.0)
    , z(0.0)
    , w(0.0)  {
  (void)_alloc;
    }



   typedef double _x_type;
  _x_type x;

   typedef double _y_type;
  _y_type y;

   typedef double _z_type;
  _z_type z;

   typedef double _w_type;
  _w_type w;





  typedef boost::shared_ptr< ::geometry_msgs::Quaternion_ > Ptr;
  typedef boost::shared_ptr< ::geometry_msgs::Quaternion_ const> ConstPtr;

};

默认的初始化的时候orientation的数值默认都是0, 所以如果直接把orientation传入到getYaw函数中时，会报错，把orientation.w赋值成1,函数就不会报错了。