上一节介绍了cartographer中的OdometryData数据相关转换。
本节将介绍陀螺仪Imu数据转换为cartographer中的ImuData数据。还有cartographer中的ImuData数据和Ros的sensor_msgs::Imu数据相互转换,方便引入Rosbag进行储存和读取。
目录
1:ImuData数据类型
【imu_data.h】中查看数据结构:
//时间
common::Time time;
//线加速度
Eigen::Vector3d linear_acceleration;
//角速度
Eigen::Vector3d angular_velocity;
2:Imu数据转换cartographer数据
cartographer::sensor::ImuData GetImuData()
{
cartographer::sensor::ImuData imu_data;
//计算
//线加速度(含重力)
acc[0] = *((float *)&handle_buf[22]);
acc[1] = *((float *)&handle_buf[26]);
acc[2] = *((float *)&handle_buf[30]);
//角速度(陀螺仪I的输出)
gyo[0] = *((float *)&handle_buf[82]);
gyo[1] = *((float *)&handle_buf[86]);
gyo[2] = *((float *)&handle_buf[90]);
//欧拉角
eular[0] = *((float *)&handle_buf[146]);
eular[1] = *((float *)&handle_buf[150]);
eular[2] = *((float *)&handle_buf[154]);
Eigen::Vector3d linear_acceleration(((double)acc[0] * (-9.8)), ((double)acc[1] * (-9.8)), ((double)acc[2] * (-9.8)));
Eigen::Vector3d angular_velocity(((double)gyo[0] * 3.1415926 / 180), ((double)gyo[1] * 3.1415926 / 180), ((double)gyo[2]) * 3.1415926 / 180);
Eigen::Vector3d imu_angle(((double)eular[0]), ((double)eular[1]), ((double)eular[2]));
imu_data_.time = cartographer::common::NowTime();
imu_data_.linear_acceleration = linear_acceleration;
imu_data_.angular_velocity = angular_velocity;
return imu;
}
3:Imu数据转换Ros数据
【cartographer_ros】五: 发布和订阅陀螺仪Imu信息
4:cartographer数据转换Ros数据
sensor_msgs::Imu ToRosMsg(cartographer::sensor::ImuData imu_data)
{
sensor_msgs::Imu imu;
imu.header.stamp = TimeRosFromCarto(imu_data.time);
imu.header.frame_id = "imu";
imu.angular_velocity.x = imu_data.angular_velocity.x();
imu.angular_velocity.y = imu_data.angular_velocity.y();
imu.angular_velocity.z = imu_data.angular_velocity.z();
imu.linear_acceleration.x = imu_data.linear_acceleration.x();
imu.linear_acceleration.y = imu_data.linear_acceleration.y();
imu.linear_acceleration.z = imu_data.linear_acceleration.z();
return imu;
}
5:Ros数据转换cartographer数据
cartographer::sensor::ImuData ToCartoSensor(sensor_msgs::Imu imu)
{
cartographer::sensor::ImuData imu_data;
imu_data.time = TimeCartoFromRos(imu.header.stamp);
imu_data.angular_velocity = Eigen::Vector3d(imu.angular_velocity.x, imu.angular_velocity.y, imu.angular_velocity.z);
imu_data.linear_acceleration = Eigen::Vector3d(imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z);
return imu_data;
}
【完】
下一节会介绍路标Landmark数据在cartographer中的发布。