上一节介绍了IMU数据相关转换。
本节就开始介绍cartographer中的LandmarkData数据和Ros中的cartographer_ros_msgs::LandmarkList数据相互转换,方便引入Rosbag进行储存和读取。
目录
1:LandmarkData数据类型
【landmark_data.h】中查看数据结构:
struct LandmarkObservation {
std::string id;
transform::Rigid3d landmark_to_tracking_transform;
double translation_weight;
double rotation_weight;
};
struct LandmarkData {
common::Time time;
std::vector<LandmarkObservation> landmark_observations;
};
2:cartographer数据转换Ros数据
cartographer_ros_msgs::LandmarkList ToRosMsg(cartographer::sensor::LandmarkData landmark_data)
{
cartographer_ros_msgs::LandmarkList landmarkList;
cartographer_ros_msgs::LandmarkEntry landmarkEntry;
landmarkList.header.frame_id = "base_link";
landmarkList.header.stamp = common::RosFromCarto(landmark_data.time);
landmarkList.landmarks.clear();
for (auto var : landmark_data.landmark_observations)
{
landmarkEntry.id = var.id;
landmarkEntry.tracking_from_landmark_transform.position.x = var.landmark_to_tracking_transform.translation().x();
landmarkEntry.tracking_from_landmark_transform.position.y = var.landmark_to_tracking_transform.translation().y();
landmarkEntry.tracking_from_landmark_transform.position.z = 0;
landmarkEntry.tracking_from_landmark_transform.orientation.x = var.landmark_to_tracking_transform.rotation().x();
landmarkEntry.tracking_from_landmark_transform.orientation.y = var.landmark_to_tracking_transform.rotation().y();
landmarkEntry.tracking_from_landmark_transform.orientation.z = var.landmark_to_tracking_transform.rotation().z();
landmarkEntry.tracking_from_landmark_transform.orientation.w = var.landmark_to_tracking_transform.rotation().w();
landmarkEntry.translation_weight = var.translation_weight;
landmarkEntry.rotation_weight = var.rotation_weight;
landmarkList.landmarks.push_back(landmarkEntry);
}
return landmarkList;
}
3:Ros数据转换cartographer数据
cartographer::sensor::LandmarkData ToCartoSensor(cartographer_ros_msgs::LandmarkList landmark_list)
{
cartographer::sensor::LandmarkData landmark_data;
landmark_data.time = FromRos(landmark_list.header.stamp);
for (const LandmarkEntry& entry : landmark_list.landmarks) {
landmark_data.landmark_observations.push_back(
{entry.id, ToRigid3d(entry.tracking_from_landmark_transform),
entry.translation_weight, entry.rotation_weight});
}
return landmark_data;
}
【完】
下一节会介绍Rosbag的引入和使用,即使脱离了ros环境依然能够进行存包读包等操作,方便支持离线建图定位等功能,以及对原始数据进行分析