给一个角度来进行时间的插值

描述： //已知开始时刻的旋转角度,和起始时间 和结束时刻的旋转角度,和结束时刻的时间 //判断当前旋转 是否旋转过半,选择与起始位置还是终止位置进行差值计算，从而进行插值计算时间 输入： 输出：

#include <iostream>
#include <cmath>
using namespace std;


//已知开始时刻的旋转角度,和起始时间  和结束时刻的旋转角度,和结束时刻的时间
//判断当前旋转 是否旋转过半,选择与起始位置还是终止位置进行差值计算，从而进行插值计算时间

float gettime(float startOri, float start_time, float endOri, float endOri_time, float ori)
{
    //开始时刻的旋转角度,和起始时间
    //float startOri, start_time;

    //结束时刻的旋转角度,和结束时刻的时间
    //float endOri, endOri_time;

    //当前时刻的旋转角度
    //float ori;
    
    //判断是否旋转过半
    bool halfPassed = false;

    if (!halfPassed) {
      //旋转没过半,就进入这里
      //确保-pi/2 < ori - startOri < 3*pi/2
      if (ori < startOri - M_PI / 2) {
        ori += 2 * M_PI;
      } else if (ori > startOri + M_PI * 3 / 2) {
        ori -= 2 * M_PI;
      }

      if (ori - startOri > M_PI) {
        halfPassed = true;
      }
    } else { 
      //旋转过半进入这里
      ori += 2 * M_PI;

      //确保-3*pi/2 < ori - endOri < pi/2
      if (ori < endOri - M_PI * 3 / 2) {
        ori += 2 * M_PI;
      } else if (ori > endOri + M_PI / 2) {
        ori -= 2 * M_PI;
      } 
    }
    
    //当前时刻插值的时间
    float ori_time = start_time + (endOri_time - start_time )*(ori - startOri) / (endOri - startOri);

    return ori_time;
    
}

int main()
{
    float startOri = 0.523; //M_PI/6
    float start_time = 10;
    float endOri = 5.235; //M_PI*(5/3)
    float endOri_time = 100;
    float ori = 2;
    float ori2 = 2 + 2 * M_PI;
    float ori3 = 2 - 4 * M_PI;
    float ori_time1 = gettime(startOri, start_time, endOri, endOri_time, ori);
    cout << "插值得到的时间1:" << ori_time1 << endl;
    float ori_time2 = gettime(startOri, start_time, endOri, endOri_time, ori);
    cout << "插值得到的时间2:" << ori_time2 << endl;
    float ori_time3 = gettime(startOri, start_time, endOri, endOri_time, ori);
    cout << "插值得到的时间3:" << ori_time3 << endl;
}

View Code

 

 

ROS中里程计的 quaternion四元数和RPY欧拉角转换

ROS-TF的使用（常用功能）：https://blog.csdn.net/liuzubing/article/details/81014240

#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的旋转角度计算四元数，用于平面小车。返回四元数

View Code