#include<ros/ros.h>
#include<geometry_msgs/PoseWithCovarianceStamped.h> //amcl_pose
#include<tf/transform_datatypes.h> //转换函数头文件
#include <tf/tf.h>
class RobotMove
{
public:
RobotMove();
void poseCallBack(geometry_msgs::PoseWithCovarianceStampedConstPtr &pose);
private:
ros::NodeHandle n;
ros::Subscriber pose_sub; //订阅amcl位置
float pose_x,pose_y,pose_z;
};
//构造函数
RobotMove::RobotMove()
{
pose_sub=n.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/amcl_pose",10,&RobotMove::poseCallBack,this);
}
//位置回调函数
void RobotMove::poseCallBack(geometry_msgs::PoseWithCovarianceStampedConstPtr &pose)
{
//四元数转欧拉角
tf::Quaternion quat; //定义一个四元数
tf::quaternionMsgToTF(pose->pose.pose.orientation,quat); //取出方向存储于四元数
double roll,pitch,yaw; //定义存储r\p\y的容器
tf::Matrix3x3(quat).getRPY(roll,pitch,yaw); //进行转换
pose_x=roll;
pose_y=pitch;
pose_z=yaw;
//欧拉角转四元数,类型可以不同
geometry_msgs::Quaternion quaternion;//定义四元数
quaternion=tf::createQuaternionMsgFromRollPitchYaw(0,0,0); //欧拉角
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "robotMoveAction");
RobotMove robotmove;
ros::spin();
return 0;
}
ROS四元数、欧拉角互转
最新推荐文章于 2023-10-11 10:57:43 发布