版权声明:https://blog.csdn.net/CAIYUNFREEDOM https://blog.csdn.net/CAIYUNFREEDOM/article/details/91047909
http://docs.ros.org/jade/api/tf2/html/classtf2_1_1Quaternion.html
Quaternion tf2::Quaternion::slerp | ( | const Quaternion & | q, |
const tf2Scalar & | t | ||
) | const [inline] |
Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion.
Parameters:
q | The other quaternion to interpolate with |
t | The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q. Slerp interpolates assuming constant velocity. |
Definition at line 314 of file Quaternion.h.
example:
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf2/LinearMath/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <tf2/utils.h>
#include <turtlesim/Spawn.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#define epsilo 0.000001
using namespace std;
using namespace tf2;
#define r2d 57.29577951308232
#define d2r 0.017453292519943295
void seeq(Quaternion q)
{
printf("[%f, %f, %f,%f]\n", q.getX(),q.getY(), q.getZ(), q.getW());
}
void test() {
Quaternion q,qb,qc;
float r, p, y;
float du = 270;
q.setRPY(0, 0, du*d2r);//
printf("绕Z旋转270度 ");
seeq(q);
y = tf2::getYaw(q);
printf("%f \n", y);
du = -90;
qb.setRPY(0, 0, du*d2r);
printf("绕Z旋转-90度 ");
seeq(q);
y = tf2::getYaw(q);
printf("%f \n", y);
printf("end-----四元数每个元素都取反后和自身等价-----\n---四元数插值示例-----\n");
q.setRPY(0,0,100*d2r);//100 du
printf("q 100 du ");
seeq(q);
qb.setRPY(0,0,-120*d2r);//-100 du
printf("qb -120 du");
seeq(qb);
qc=q.slerp(qb,0);// qc = q
printf("插值系数为0,得到qc = q ");
seeq(qc);
qc=q.slerp(qb,0.3);// qc = q
y = tf2::getYaw(qc);
printf("插值系数为0.3,得到qc ");
seeq(qc);
printf("插值系数为0.3,得到qc的yaw :%f du\n", y*r2d);
qc=q.slerp(qb,0.8);// qc = q
y = tf2::getYaw(qc);
printf("插值系数为0.8,得到qc ");
seeq(qc);
printf("插值系数为0.8,得到qc的yaw :%f du\n", y*r2d);
qc=q.slerp(qb,1);// qc = q
printf("插值系数为1,得到qc=qb ");
seeq(qc);
}
int main(int argc, char** argv) {
ros::init(argc, argv, "markerListener");
ros::NodeHandle node("~");
test();
return 0;
}