赞
踩
在移动小车导航中,ros系统通常使用四元数表示里程计orientation信息,但是很多时候需要rpy表示更加直观方便,因此在这里记录一下转换方法。
- #include "tf/transform_datatypes.h"//转换函数头文件
- #include <nav_msgs/Odometry.h>//里程计信息格式
-
-
- /****************四元数转RPY欧拉角,以odomsub的回调函数为例*****************/
-
- void odomCallback(const nav_msgs::Odometry &odom) {
-
-
- tf::Quaternion quat;
- tf::quaternionMsgToTF(odom.pose.pose.orientation, quat);
-
-
- double roll, pitch, yaw;//定义存储r\p\y的容器
- tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//进行转换
-
- }
-
-
- /****************RPY欧拉角转四元数*****************/
- tf::createQuaternionMsgFromRollPitchYaw(double r, double p, double y);//返回四元数
-
-
- tf::createQuaternionMsgFromYaw(double y);//只通过y即绕z的旋转角度计算四元数,用于平面小车。返回四元数
![](https://csdnimg.cn/release/blogv2/dist/pc/img/newCodeMoreBlack.png)
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。