1.将点云消息和geometry_msgs消息写入文件:
std::stringstream buffer; std::time_t t = std::time(nullptr); std::tm tm = *std::localtime(&t); buffer << "/home/robot/data/PointCloud." << std::put_time(&tm, "%Y-%m-%d-%H-%M-%S"); std::string file_name = buffer.str(); int size = raw_points_.size(); int size_int = sizeof(int); mkdir("/home/robot/data", S_IRWXU | S_IRWXG | S_IRWXO); std::ofstream ofs(file_name, std::ios::binary | std::ios::ate);//std::ios::ate从文件尾部加入 ofs.write((char *)&size, size_int);//写入int代表点云数 ofs.write((char *)&raw_points_[0], sizeof(RawPoint) * size); ofs.close(); LOG_INFO("Save point cloud to:%s", file_name.c_str());
//std::stringstream 的clear()函数不清空缓存区,所以使用buffer.str("")清空。 buffer.str(""); buffer << "/home/robot/data/RTK." << std::put_time(&tm, "%Y-%m-%d-%H-%M-%S"); file_name.clear();
file_name=buffer.str(); size=1;//这里先假定pose大小为1,后期可调为实际缓存的大小。 std::ofstream ofs_gps(file_name,std::ios::binary|std::ios::ate); ofs_gps.write((char *)&size,size_int);//把数据大小以int写入文件 ofs_gps.write((char *)&gps_,sizeof(geometry_msgs::Pose)*size);//sizeof测量的是数据结构的大小 ofs_gps.close(); LOG_INFO("Save GPS to:%s",file_name.c_str());
2.不使用cmake来编译包含ros头文件的程序,使用该程序测试下保存的数据是否有问题
#include <iostream> #include <string> #include <vector> #include <fstream> #include <ros/ros.h> #include <geometry_msgs/Pose.h> int main(){ std::vector<geometry_msgs::Pose> gps_; std::ifstream ifs ("/home/robot/data/RTK.2020-08-27-18-47-15", std::ios::binary | std::ios::in); if (ifs.is_open()) { size_t size = 0; ifs.read(reinterpret_cast<char *>(&size), sizeof(int)); std::cout<<size<<std::endl; gps_.resize(size);//调整容器的大小以装文件中的消息。 ifs.read(reinterpret_cast<char *>(&gps_[0]), sizeof(geometry_msgs::Pose) * size); ifs.close(); for (int i=0;i<gps_.size();i++){ std::cout<<"position:"<<gps_[i].position.x<<std::endl
<<gps_[i].position.y<<std::endl
<<gps_[i].position.z<<std::endl
<<"orientation:"<<std::endl
<<gps_[i].orientation.x<<std::endl
<<gps_[i].orientation.y<<std::endl
<<gps_[i].orientation.z<<std::endl
<<gps_[i].orientation.w<<std::endl; } } return 0; }
编译方式(告知g++库的位置即可):
g++ -std=c++11 readTimeStamp.cpp -o read -I/opt/ros/kinetic/include -L/opt/ros/kinetic/lib
ros消息写入文件和用g++编译roscpp程序从而读取消息文件
原文:https://www.cnblogs.com/gangyin/p/13573298.html