diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index 406aaa0..66d5d6f 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -54,17 +54,12 @@ namespace { constexpr char kTfStaticTopic[] = "/tf_static"; namespace carto = ::cartographer; -std::tuple -LoadPoseGraph(const std::string& pose_graph_filename) { +carto::mapping::proto::PoseGraph LoadPoseGraph( + const std::string& pose_graph_filename) { carto::mapping::proto::PoseGraph pose_graph_proto; - carto::mapping::proto::AllTrajectoryBuilderOptions - all_trajectory_builder_options; - carto::io::ProtoStreamReader reader(pose_graph_filename); CHECK(reader.ReadProto(&pose_graph_proto)); - CHECK(reader.ReadProto(&all_trajectory_builder_options)); - return std::make_tuple(pose_graph_proto, all_trajectory_builder_options); + return pose_graph_proto; } std::unique_ptr @@ -90,7 +85,7 @@ CreatePipelineBuilder( return builder; } -std::unique_ptr CreateLuaDictionary( +std::unique_ptr LoadLuaDictionary( const std::string& configuration_directory, const std::string& configuration_basename) { auto file_resolver = @@ -148,18 +143,50 @@ std::unique_ptr HandleMessage( return points_batch; } -void RunPipeline( - const std::vector>& pipeline, - const std::vector& bag_filenames, - const carto::mapping::proto::PoseGraph& pose_graph_proto, - const std::string tracking_frame, const std::string& urdf_filename, - const bool use_bag_transforms) { +} // namespace + +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)) { + CHECK_EQ(pose_graph_.trajectory_size(), bag_filenames_.size()) + << "Pose graphs contains " << pose_graph_.trajectory_size() + << " trajectories while " << bag_filenames_.size() + << " bags were provided. This tool requires one bag for each " + "trajectory in the same order as the correponding trajectories in the " + "pose graph proto."; + + // This vector must outlive the pipeline. + all_trajectories_ = std::vector<::cartographer::mapping::proto::Trajectory>( + pose_graph_.trajectory().begin(), pose_graph_.trajectory().end()); + + const std::string file_prefix = !output_file_prefix.empty() + ? output_file_prefix + : bag_filenames_.front() + "_"; + point_pipeline_builder_ = + CreatePipelineBuilder(all_trajectories_, file_prefix); +} + +void AssetsWriter::Run(const std::string& configuration_directory, + const std::string& configuration_basename, + const std::string& urdf_filename, + const bool use_bag_transforms) { + const auto lua_parameter_dictionary = + LoadLuaDictionary(configuration_directory, configuration_basename); + + std::vector> pipeline = + point_pipeline_builder_->CreatePipeline( + lua_parameter_dictionary->GetDictionary("pipeline").get()); + const std::string tracking_frame = + lua_parameter_dictionary->GetString("tracking_frame"); + do { - for (size_t trajectory_id = 0; trajectory_id < bag_filenames.size(); + for (size_t trajectory_id = 0; trajectory_id < bag_filenames_.size(); ++trajectory_id) { const carto::mapping::proto::Trajectory& trajectory_proto = - pose_graph_proto.trajectory(trajectory_id); - const std::string& bag_filename = bag_filenames[trajectory_id]; + pose_graph_.trajectory(trajectory_id); + const std::string& bag_filename = bag_filenames_[trajectory_id]; LOG(INFO) << "Processing " << bag_filename << "..."; if (trajectory_proto.node_size() == 0) { continue; @@ -236,47 +263,4 @@ void RunPipeline( carto::io::PointsProcessor::FlushResult::kRestartStream); } -} // namespace - -void RunAssetsWriterPipeline(const std::string& pose_graph_filename, - const std::vector& bag_filenames, - const std::string& configuration_directory, - const std::string& configuration_basename, - const std::string& urdf_filename, - const std::string& output_file_prefix, - const bool use_bag_transforms) { - carto::mapping::proto::PoseGraph pose_graph_proto; - carto::mapping::proto::AllTrajectoryBuilderOptions - all_trajectory_builder_options; - std::tie(pose_graph_proto, all_trajectory_builder_options) = - LoadPoseGraph(pose_graph_filename); - - CHECK_EQ(pose_graph_proto.trajectory_size(), bag_filenames.size()) - << "Pose graphs contains " << pose_graph_proto.trajectory_size() - << " trajectories while " << bag_filenames.size() - << " bags were provided. This tool requires one bag for each " - "trajectory in the same order as the correponding trajectories in the " - "pose graph proto."; - - // This vector must outlive the pipeline. - std::vector all_trajectories( - pose_graph_proto.trajectory().begin(), - pose_graph_proto.trajectory().end()); - const std::string& file_prefix = !output_file_prefix.empty() - ? output_file_prefix - : bag_filenames.front() + "_"; - auto pipeline_builder = CreatePipelineBuilder(all_trajectories, file_prefix); - - auto lua_parameter_dictionary = - CreateLuaDictionary(configuration_directory, configuration_basename); - std::vector> pipeline = - pipeline_builder->CreatePipeline( - lua_parameter_dictionary->GetDictionary("pipeline").get()); - const std::string& tracking_frame = - lua_parameter_dictionary->GetString("tracking_frame"); - - RunPipeline(pipeline, bag_filenames, pose_graph_proto, tracking_frame, - urdf_filename, use_bag_transforms); -} - } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/assets_writer.h b/cartographer_ros/cartographer_ros/assets_writer.h index 8dbeed6..13ef7cb 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.h +++ b/cartographer_ros/cartographer_ros/assets_writer.h @@ -17,20 +17,35 @@ #include #include +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/io/points_processor_pipeline_builder.h" +#include "cartographer/mapping/proto/pose_graph.pb.h" +#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" + #ifndef CARTOGRAPHER_ROS_ASSETS_WRITER_H_ #define CARTOGRAPHER_ROS_ASSETS_WRITER_H_ namespace cartographer_ros { -// Configures a point processing pipeline and pushes the points from the bag -// through it. -void RunAssetsWriterPipeline(const std::string& pose_graph_filename, - const std::vector& bag_filenames, - const std::string& configuration_directory, - const std::string& configuration_basename, - const std::string& urdf_filename, - const std::string& output_file_prefix, - bool use_bag_transforms); +class AssetsWriter { + public: + AssetsWriter(const std::string& pose_graph_filename, + const std::vector& bag_filenames, + const std::string& output_file_prefix); + + // Configures a points processing pipeline and pushes the points from the + // bag through the pipeline. + void Run(const std::string& configuration_directory, + const std::string& configuration_basename, + const std::string& urdf_filename, bool use_bag_transforms); + + private: + std::vector bag_filenames_; + std::vector<::cartographer::mapping::proto::Trajectory> all_trajectories_; + ::cartographer::mapping::proto::PoseGraph pose_graph_; + std::unique_ptr<::cartographer::io::PointsProcessorPipelineBuilder> + point_pipeline_builder_; +}; } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index e17fa6f..d931abf 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -54,9 +54,11 @@ int main(int argc, char** argv) { CHECK(!FLAGS_pose_graph_filename.empty()) << "-pose_graph_filename is missing."; - ::cartographer_ros::RunAssetsWriterPipeline( + ::cartographer_ros::AssetsWriter asset_writer( FLAGS_pose_graph_filename, cartographer_ros::SplitString(FLAGS_bag_filenames, ','), - FLAGS_configuration_directory, FLAGS_configuration_basename, - FLAGS_urdf_filename, FLAGS_output_file_prefix, FLAGS_use_bag_transforms); + FLAGS_output_file_prefix); + + asset_writer.Run(FLAGS_configuration_directory, FLAGS_configuration_basename, + FLAGS_urdf_filename, FLAGS_use_bag_transforms); }