Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Read the final trajectory of cartographer_ros offline mapping from .pbstream file
#include "cartographer/io/proto_stream.h"
#include "cartographer/io/proto_stream_deserializer.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/time_conversion.h"
#include "cartographer_ros/split_string.h"
#include "geometry_msgs/TransformStamped.h"
#include <iostream>
#include <string>
#include <ros/ros.h>
namespace cartographer_ros {
namespace carto = ::cartographer;
void ExportPbstream(const std::string& pbstream_filename) {
carto::io::ProtoStreamReader reader(pbstream_filename);
carto::io::ProtoStreamDeserializer deserializer(&reader);
carto::mapping::proto::PoseGraph pose_graph_proto = deserializer.pose_graph();
for (size_t trajectory_id = 0; trajectory_id < pose_graph_proto.trajectory().size();
++trajectory_id) {
const carto::mapping::proto::Trajectory& trajectory_proto =
pose_graph_proto.trajectory(trajectory_id);
std::cerr << "size : " << trajectory_proto.node_size() << std::endl;
for (int i = 0; i < trajectory_proto.node_size(); ++i) {
const auto& node = trajectory_proto.node(i);
// node.pose() contains the pose...
}
}
}
}
int main(int argc, char** argv) {
// Initialize ROS
ros::init (argc, argv, "pbstream_reader");
ros::NodeHandle nh;
std::cerr << "initialize reader node." << std::endl;
if (argc < 2){
std::cerr << "no input directory" << std::endl;
} else {
std::cerr << "got input file" << std::endl;
cartographer_ros::ExportPbstream(argv[1]);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment