shell處理csv文件，讀取csv文件中的IMU數據并以sensor_msgs/Imu格式發送

讀取csv文件中的IMU數據并以sensor_msgs/Imu格式發送 20210724

項目需要將所給的csv表格中IMU數據讀取并轉化為sensor_msgs/Imu格式，以便于后續使用數據跑定位算法。

csv表格：

讀取csv文件

code：

ifstream inFile("(csv文件絕對路徑)", ios::in);    //
vector<string> stamp,Yaw,Pitch,Roll,Ve,Vn,Vu;if (!inFile)
{cout << "打開CSV文件失敗！" << endl;exit(1);
}
string line;
while (getline(inFile, line)){string field;istringstream sin(line);getline(sin, field, ',');stamp.push_back(field.c_str()); getline(sin, field, ',');Yaw.push_back(field.c_str()); getline(sin, field, ',');Pitch.push_back(field.c_str()); getline(sin, field, ',');Roll.push_back(field.c_str()); getline(sin, field, ',');Ve.push_back(field.c_str()); getline(sin, field, ',');Vn.push_back(field.c_str()); getline(sin, field); Vu.push_back(field.c_str()); 
}

IMU數據轉換

由于csv中所給的數據并不是標準IMU格式，而是通過解算得到的處理過后的數據，所以需要通過逆變換反推出IMU的標準數據，得到gyro，accel，四元數等數據。

四元數

公式

$$\begin{array}{rl}{\mathbf{q}}_{\mathrm{B}}& =?\begin{array}{c}\cos (\psi /2)\\ 0\\ 0\\ \sin (\psi /2)\end{array}??\begin{array}{c}\cos (\theta /2)\\ 0\\ \sin (\theta /2)\\ 0\end{array}??\begin{array}{c}\cos (?/2)\\ \sin (?/2)\\ 0\\ 0\end{array}?\\ & =?\begin{array}{l}\cos (?/2)\cos (\theta /2)\cos (\psi /2)+\sin (?/2)\sin (\theta /2)\sin (\psi /2)\\ \sin (?/2)\cos (\theta /2)\cos (\psi /2)?\cos (?/2)\sin (\theta /2)\sin (\psi /2)\\ \cos (?/2)\sin (\theta /2)\cos (\psi /2)+\sin (?/2)\cos (\theta /2)\sin (\psi /2)\\ \cos (?/2)\cos (\theta /2)\sin (\psi /2)?\sin (?/2)\sin (\theta /2)\cos (\psi /2)\end{array}?\end{array}$$

code：

q0 = sin(roll/360.0f*M_PI) * cos(pitch/360.0f*M_PI) * cos(yaw/360.0f*M_PI) - cos(roll/360.0f*M_PI) * 			sin(pitch/360.0f*M_PI) * sin(yaw/360.0f*M_PI);
q1 = cos(roll/360.0f*M_PI) * sin(pitch/360.0f*M_PI) * cos(yaw/360.0f*M_PI) + sin(roll/360.0f*M_PI) * 			cos(pitch/360.0f*M_PI) * sin(yaw/360.0f*M_PI);
q2 = cos(roll/360.0f*M_PI) * cos(pitch/360.0f*M_PI) * sin(yaw/360.0f*M_PI) - sin(roll/360.0f*M_PI) * 			sin(pitch/360.0f*M_PI) * cos(yaw/360.0f*M_PI);
q3 = cos(roll/360.0f*M_PI) * cos(pitch/360.0f*M_PI) * cos(yaw/360.0f*M_PI) + sin(roll/360.0f*M_PI) * 			sin(pitch/360.0f*M_PI) * sin(yaw/360.0f*M_PI);

GYRO

公式

$$?\begin{array}{l}{g}_x\\ g_y\\ g_z\end{array}?={\mathrm{M}}_x?{\mathrm{M}}_y??\begin{array}{c}0\\ 0\\ \frac{dy}{dt}\end{array}?+{\mathrm{M}}_x??\begin{array}{c}0\\ \frac{dp}{dt}\\ 0\end{array}?+?\begin{array}{c}\frac{dr}{dt}\\ 0\\ 0\end{array}?$$

shell處理csv文件。$$=?\begin{array}{ccc}1& 0& 0\\ 0& \cos r& \sin r\\ 0& ?\sin r& \cos r\end{array}???\begin{array}{ccc}\cos p& 0& ?\sin p\\ 0& 1& 0\\ \sin p& 0& \cos p\end{array}???\begin{array}{c}0\\ 0\\ \frac{dy}{dt}\end{array}?+?\begin{array}{ccc}1& 0& 0\\ 0& \cos r& \sin r\\ 0& ?\sin r& \cos r\end{array}???\begin{array}{c}0\\ \frac{dp}{dt}\\ 0\end{array}?+?\begin{array}{c}\frac{dr}{dt}\\ 0\\ 0\end{array}?$$

$$=?\begin{array}{ccc}1& 0& ?\sin p\\ 0& \cos r& \cos p?\sin r\\ 0& ?\sin r& \cos p?\cos r\end{array}???\begin{array}{l}\frac{dr}{dt}\\ \frac{dp}{dt}\\ \frac{dy}{dt}\end{array}?$$

code

Eigen::Vector3f deltaAngle;
Eigen::Vector3f gyro;
Eigen::Matrix3f tfmatrix;
yaw   = atof(Yaw[i].c_str());
pitch = atof(Pitch[i].c_str());
roll  = atof(Roll[i].c_str());
tfmatrix<<1,0,-sin(pitch/180.0f*M_PI),0,cos(roll/180.0f*M_PI),cos(pitch/180.0f*M_PI)*sin(roll/180.0f*M_PI),0,-sin(roll/180.0f*M_PI),cos(pitch/180.0f*M_PI)*cos(roll/180.0f*M_PI);
deltaAngle<<(roll-last_roll)/0.01f,(pitch-last_pitch)/0.01f,(yaw-last_yaw)/0.01f;
gyro=tfmatrix*deltaAngle;

Accel

公式：

$$?\begin{array}{l}{a}_x\\ a_y\\ a_z\end{array}?={\mathrm{M}}_x?{\mathrm{M}}_y?{\mathrm{M}}_z??\begin{array}{l}dvn\\ dve\\ g+dvu/dt\end{array}?$$

$$=?\begin{array}{ccc}1& 0& 0\\ 0& \cos r& \sin r\\ 0& ?\sin r& \cos r\end{array}???\begin{array}{ccc}\cos p& 0& ?\sin p\\ 0& 1& 0\\ \sin p& 0& \cos p\end{array}???\begin{array}{ccc}\cos y& \sin y& 0\\ ?\sin y& \cos y& 0\\ 0& 0& 1\end{array}???\begin{array}{l}dvn\\ dve\\ g+dvu/dt\end{array}?$$

$$=?\begin{array}{ccc}\cos p?\cos y& \cos p?\sin y& ?\sin p\\ \cos y?\sin p?\sin r?\cos r?\sin y& \cos r?\cos y+\sin p?\sin r?\sin y& \cos p?\sin r\\ \sin r?\sin y+\cos r?\cos y?\sin p& \cos r?\sin p?\sin y?\cos y?\sin r& \cos p?\cos r\end{array}???\begin{array}{l}dvn\\ dve\\ g+dvu/dt\end{array}?$$

$$=?\begin{array}{c}?\sin p\\ \cos p?\sin r\\ \cos p?\cos r\end{array}???\begin{array}{l}dvn\\ dve\\ g+dvu/dt\end{array}?$$

code:

Eigen::Vector3f deltaV;
Eigen::Vector3f accel;
Eigen::Matrix3f tfmatrix2;
vx = atof(Vn[i].c_str());
vy = atof(Ve[i].c_str());
vz = atof(Vu[i].c_str());tfmatrix2<<cos(pitch/180.0f*M_PI)*cos(yaw/180.0f*M_PI),cos(pitch/180.0f*M_PI)*sin(yaw/180.0f*M_PI),-sin(pitch/180.0f*M_PI),cos(yaw/180.0f*M_PI)*sin(pitch/180.0f*M_PI)*sin(roll/180.0f*M_PI)-											cos(roll/180.0f*M_PI)*sin(yaw/180.0f*M_PI),cos(roll/180.0f*M_PI)*cos(yaw/180.0f*M_PI)+sin(pitch/180.0f*M_PI)*sin(roll/180.0f*M_PI)*sin(yaw/180.0f*M_PI),cos(pitch/180.0f*M_PI)*sin(roll/180.0f*M_PI),sin(roll/180.0f*M_PI)*sin(yaw/180.0f*M_PI)+cos(roll/180.0f*M_PI)*cos(yaw/180.0f*M_PI)*sin(pitch/180.0f*M_PI),cos(roll/180.0f*M_PI)*sin(pitch/180.0f*M_PI)*sin(yaw/180.0f*M_PI)-cos(yaw/180.0f*M_PI)*sin(roll/180.0f*M_PI),cos(pitch/180.0f*M_PI)*cos(roll/180.0f*M_PI);
deltaV<<(vx-last_vx)/0.01f,(vy-last_vy)/0.01f,(vz-last_vz)/0.01f+9.8f;
accel=tfmatrix2*deltaV;

節點發布

ros::init(argc, argv, "imu2rosbag");
ros::NodeHandle nh;
ros::Publisher IMU_pub = nh.advertise<sensor_msgs::Imu>("IMU_data", 20);
ros::Rate loop_rate(10);
sensor_msgs::Imu imu_data;
imu_data.header.frame_id = "base_link";
imu_data.header.stamp = ros::Time(atoi(stamp[i].c_str()));imu_data.orientation.x = q0;
imu_data.orientation.y = q1;
imu_data.orientation.z = q2;
imu_data.orientation.w = q3;imu_data.linear_acceleration.x = accel(0);
imu_data.linear_acceleration.y = accel(1);
imu_data.linear_acceleration.z = accel(2);imu_data.angular_velocity.x = gyro(0);
imu_data.angular_velocity.y = gyro(1);
imu_data.angular_velocity.z = gyro(2);

用python讀取csv文件，ref: https://blog.csdn.net/weixin_39719127/article/details/110299079 （姿態解算）

? https://blog.csdn.net/chenlin41204050/article/details/78429437 （C++讀CSV文件）