学习笔记15--tf2 方式: 根据odom的 msg的四元数计算 偏转角

环境:ros-kincetc ubuntu16.04
假设:头文件\编译依赖以及相关的都已经配置好了.

背景是这样子的,需要把小车的在运动一段距离后,计算小车的yaw偏转角,为后面的操作做准备.编写了一个node,然后里面定义了一个topic 来监听 odom ;虽然可以获取该odom的四元数信息,但是该消息类型是nav_msgs::Odometry::ConstPtr& msg ,里面的四元数的角度不是实际需要的(或者说里面的值没有是直接表示宏观的角度的).下面通过tf2方式获取需要的偏转角并输出来:

		tf2::Quaternion q;//构建一个tf2版本的"四元数"
		tf2::convert(msg->pose.pose.orientation,q);//将topic监听得到的odom消息关于四元数的转换为tf2版本的"四元数"
		double roll,pitch,yam;//创建三个角度的变量
		tf2::Matrix3x3(q_orig).getRPY(roll,pitch,yam);//利用tf2的函数将"四元数"里面转换出三个方向的角度
		double y_angle=yam;//通过赋值方式就可以得到需要的绕z轴偏转角度了.

这是通过tf2方式获取绕z轴偏转角的方式,记录一下.