目标:
利用IMU测得的加速度信息来计算位移,很简单假设是匀加速运动或是匀速运动都可以,主要是看问题的背景来具体去确定运动模型,这里我就取个简单的匀加速运动模型。
学习过程:
1、了解IMU的相关原理知识
2、阅读产品自带的SDK包
3、自己添加并修改相关代码
学习内容:
这里主要介绍我是如何添改的代码。因为想要直接获取位移需要时间和加速度,这里的出速度我设为0,其加速度的获取可以阅读产品自带的功能包中的源码,我在编写过程中主要是在时间的获取上出现了一点问题,需要获得IMU开启时的时间戳以及当前帧的时间戳。
在运行产品自带的源码时,可以看见每一帧都对应着一个 seq 和 stamp ,所以只要把 seq 为1的那一帧对应的时间戳提取出来作为起始时间即可,如图所示,同样的方法去提取当前帧的时间戳。
学习产出:
#include <ahrs_driver.h>
#include <Eigen/Eigen>
using namespace std;
void my_handle_imu_data(const sensor_msgs::Imu::ConstPtr& imu_data)
{
cout<<"accelaration X is :"<<imu_data->linear_acceleration.x<<endl;
cout<<"accelaration Y is :"<<imu_data->linear_acceleration.y<<endl;
static int start_time;
int now_time;
if(imu_data->header.seq == 50)
{
start_time = int(imu_data->header.stamp.toSec());
cout<<"when the seq is "<<imu_data->header.seq<<" "<<" the stamp is "<<start_time<<endl;
}
cout<<"the start time is :"<<start_time<<endl;
now_time = int(imu_data->header.stamp.toSec());
if(start_time == now_time)
{
cout<<"the shift on X is :"<< 0 <<endl;
cout<<"the shift on Y is :"<< 0 <<endl;
}
else if(now_time > start_time)
{
double X_Shift,Y_Shift;
int Dealt_Time;
Dealt_Time = now_time - start_time;
X_Shift = 0.5 * imu_data->linear_acceleration.x * Dealt_Time * Dealt_Time;
Y_Shift = 0.5 * imu_data->linear_acceleration.y * Dealt_Time * Dealt_Time;
cout<<"the shift on X is :"<<X_Shift<<endl;
cout<<"the shift on Y is :"<<Y_Shift<<endl;
cout<<"now time is :"<<now_time<<endl;
cout<<"start_time is :"<<start_time<<endl;
cout<<imu_data->header.seq<<endl;
//cout<<imu_data->header.stamp<<endl;
cout<<"*********************************************"<<endl;
}
}
int main(int argc, char *argv[])
{
ros::init(argc,argv,"cal_shift");
ros::NodeHandle nh;
ros::Subscriber sub_imu_data_ = nh.subscribe<sensor_msgs::Imu>("/mobile_base/sensors/imu_data",10,my_handle_imu_data);
ros::spin();
return 0;
}
代码很短,原理也很简单,主要是需要注意一下 start_time 变量前的 static 关键字,它可以保证该变量在程序结束时,再释放变量。同学可以试一下删除 static 之后变量的值是什么。
然后就是ros相关的知识,想要学习的话可以在这里学习:保姆级教程
【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程_哔哩哔哩_bilibili
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)