diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index 7367ef3..49b2c29 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -27,6 +27,7 @@ #include "cartographer/io/points_processor.h" #include "cartographer/io/points_processor_pipeline_builder.h" #include "cartographer/io/proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" #include "cartographer/mapping/proto/pose_graph.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/sensor/point_cloud.h" @@ -54,14 +55,6 @@ namespace { constexpr char kTfStaticTopic[] = "/tf_static"; namespace carto = ::cartographer; -carto::mapping::proto::PoseGraph LoadPoseGraph( - const std::string& pose_graph_filename) { - carto::mapping::proto::PoseGraph pose_graph_proto; - carto::io::ProtoStreamReader reader(pose_graph_filename); - CHECK(reader.ReadProto(&pose_graph_proto)); - return pose_graph_proto; -} - std::unique_ptr CreatePipelineBuilder( const std::vector& trajectories, @@ -147,7 +140,8 @@ AssetsWriter::AssetsWriter(const std::string& pose_graph_filename, const std::vector& bag_filenames, const std::string& output_file_prefix) : bag_filenames_(bag_filenames), - pose_graph_(LoadPoseGraph(pose_graph_filename)) { + pose_graph_( + carto::io::DeserializePoseGraphFromFile(pose_graph_filename)) { CHECK_EQ(pose_graph_.trajectory_size(), bag_filenames_.size()) << "Pose graphs contains " << pose_graph_.trajectory_size() << " trajectories while " << bag_filenames_.size() diff --git a/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc b/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc index 102c9e5..da37aa6 100644 --- a/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc +++ b/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc @@ -21,9 +21,8 @@ #include #include "cartographer/common/math.h" -#include "cartographer/io/proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" #include "cartographer/mapping/proto/pose_graph.pb.h" -#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/transform/transform_interpolation_buffer.h" #include "cartographer_ros/msg_conversion.h" #include "cartographer_ros/time_conversion.h" @@ -67,9 +66,8 @@ std::string QuantilesToString(std::vector* v) { void Run(const std::string& pbstream_filename, const std::string& bag_filename) { - cartographer::io::ProtoStreamReader reader(pbstream_filename); - cartographer::mapping::proto::PoseGraph pose_graph_proto; - CHECK(reader.ReadProto(&pose_graph_proto)); + cartographer::mapping::proto::PoseGraph pose_graph_proto = + cartographer::io::DeserializePoseGraphFromFile(pbstream_filename); const cartographer::mapping::proto::Trajectory& last_trajectory_proto = *pose_graph_proto.mutable_trajectory()->rbegin(); const cartographer::transform::TransformInterpolationBuffer diff --git a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc index 5841b4c..4774ac8 100644 --- a/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc +++ b/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc @@ -20,6 +20,7 @@ #include #include "cartographer/io/proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" #include "cartographer/io/submap_painter.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/submap_2d.h" @@ -50,31 +51,23 @@ namespace { void Run(const std::string& pbstream_filename, const std::string& map_topic, const std::string& map_frame_id, const double resolution) { ::cartographer::io::ProtoStreamReader reader(pbstream_filename); - - ::cartographer::mapping::proto::PoseGraph pose_graph; - CHECK(reader.ReadProto(&pose_graph)); - ::cartographer::mapping::proto::AllTrajectoryBuilderOptions - all_trajectory_builder_options; - CHECK(reader.ReadProto(&all_trajectory_builder_options)); + ::cartographer::io::ProtoStreamDeserializer deserializer(&reader); LOG(INFO) << "Loading submap slices from serialized data."; std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice> submap_slices; - for (;;) { - ::cartographer::mapping::proto::SerializedData proto; - if (!reader.ReadProto(&proto)) { - break; - } + ::cartographer::mapping::proto::SerializedData proto; + while (deserializer.ReadNextSerializedData(&proto)) { if (proto.has_submap()) { const auto& submap = proto.submap(); const ::cartographer::mapping::SubmapId id{ submap.submap_id().trajectory_id(), submap.submap_id().submap_index()}; const ::cartographer::transform::Rigid3d global_submap_pose = - ::cartographer::transform::ToRigid3( - pose_graph.trajectory(id.trajectory_id) - .submap(id.submap_index) - .pose()); + ::cartographer::transform::ToRigid3(deserializer.pose_graph() + .trajectory(id.trajectory_id) + .submap(id.submap_index) + .pose()); FillSubmapSlice(global_submap_pose, submap, &submap_slices[id]); } } diff --git a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc b/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc index 63be8fd..b75fcc0 100644 --- a/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc +++ b/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc @@ -18,6 +18,7 @@ #include #include "cartographer/io/proto_stream.h" +#include "cartographer/io/proto_stream_deserializer.h" #include "cartographer/io/submap_painter.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/submap_2d.h" @@ -42,21 +43,15 @@ namespace { void Run(const std::string& pbstream_filename, const std::string& map_filestem, const double resolution) { ::cartographer::io::ProtoStreamReader reader(pbstream_filename); + ::cartographer::io::ProtoStreamDeserializer deserializer(&reader); - ::cartographer::mapping::proto::PoseGraph pose_graph; - CHECK(reader.ReadProto(&pose_graph)); - ::cartographer::mapping::proto::AllTrajectoryBuilderOptions - all_trajectory_builder_options; - CHECK(reader.ReadProto(&all_trajectory_builder_options)); + const auto& pose_graph = deserializer.pose_graph(); LOG(INFO) << "Loading submap slices from serialized data."; std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice> submap_slices; - for (;;) { - ::cartographer::mapping::proto::SerializedData proto; - if (!reader.ReadProto(&proto)) { - break; - } + ::cartographer::mapping::proto::SerializedData proto; + while (deserializer.ReadNextSerializedData(&proto)) { if (proto.has_submap()) { const auto& submap = proto.submap(); const ::cartographer::mapping::SubmapId id{