上一节介绍了LandmarkData数据相关转换,到此阐述了在不带ros环境中,对cartographer所需要的几类传感器数据进行处理。
本节就将介绍在脱离ros环境后,我们仍可以引入和使用Rosbag,以对外部数据进行存包读包,方便我们对cartographer的建图和定位做离线模拟和调试。
目录
1:安装rosbag并引用
安装:
sudo apt-get install librosbag-dev
如果按照此系列第一篇博客【cartographer without ros】一、不带Ros的快速安装配置环境,此时应该已经安装了rosbag。
引用:
#include <rosbag/bag.h>
#include <rosbag/view.h>
2:初始化rosbag,并打开对应读写文件
rosbag::Bag bag_;
if(read)
{
bag_.open(bagName,rosbag::bagmode::Read);
}
else
{
bag_.open(bagName,rosbag::bagmode::Write);
}
3:将传感器数据写入rosbag
template void Write<cartographer::sensor::TimedPointCloudData>(
cartographer::sensor::TimedPointCloudData measurement);
template void Write<cartographer::sensor::OdometryData>(
cartographer::sensor::OdometryData measurement);
template void Write<cartographer::sensor::ImuData>(
cartographer::sensor::ImuData measurement);
template void Write<cartographer::sensor::LandmarkData>(
cartographer::sensor::LandmarkData measurement);
template <typename T>
void Write(T measurement)
{
std::string sensorname = typeid(measurement).name();
ros::Time time = RosFromCarto(CartoFromSys());
auto msg = ToRosMsg(measurement);
bag_.write(sensorname, time, msg);
}
4:在rosbag中读取传感器数据
void Read()
{
rosbag::View view(bag_);
for (rosbag::MessageInstance m : view)
{
if (m.getTopic() == typeid(cartographer::sensor::TimedPointCloudData).name())
{
sensor_msgs::LaserScanConstPtr rosMsg = m.instantiate<sensor_msgs::LaserScan>();
if (rosMsg != NULL)
{
cartographer::sensor::TimedPointCloudData pointcloud_data = ToCartoSensor(*rosMsg);
}
}
else if (m.getTopic() == typeid(cartographer::sensor::OdometryData).name())
{
nav_msgs::OdometryConstPtr rosMsg = m.instantiate<nav_msgs::Odometry>();
if (rosMsg != NULL)
{
cartographer::sensor::OdometryData odometry_data = ToCartoSensor(*rosMsg);
}
}
else if ( m.getTopic() == typeid(cartographer::sensor::ImuData).name())
{
sensor_msgs::ImuConstPtr rosMsg = m.instantiate<sensor_msgs::Imu>();
if (rosMsg != NULL)
{
cartographer::sensor::ImuData imu_data = ToCartoSensor(*rosMsg);
}
}
else if (m.getTopic() == typeid(cartographer::sensor::LandmarkData).name())
{
cartographer_ros_msgs::LandmarkListConstPtr rosMsg = m.instantiate<cartographer_ros_msgs::LandmarkList>();
if (rosMsg != NULL)
{
cartographer::sensor::LandmarkData landmark_data = ToCartoSensor(*rosMsg);
}
}
}
}
【完】
下一节会介绍在自己的工程中,引入调用cartographer,来实现建图,存储,定位等功能。