Extract assets writer class from static method (#827)
Extracted class Assets_Writer from RunAssetsWriterPipeline. The idea is to increase the re-usability and flexibility of the assets_writer: In next PR, the assets_writer will allow registering external points_processers to the points processing pipeline. This requires having a class instead of a static method to allow for different states.master
Martin Schwörer
Wally B. Feed
parent
1f55b18eb6
commit
65b069c94f
|
|
@ -54,17 +54,12 @@ namespace {
|
|||
constexpr char kTfStaticTopic[] = "/tf_static";
|
||||
namespace carto = ::cartographer;
|
||||
|
||||
std::tuple<carto::mapping::proto::PoseGraph,
|
||||
carto::mapping::proto::AllTrajectoryBuilderOptions>
|
||||
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<carto::io::PointsProcessorPipelineBuilder>
|
||||
|
|
@ -90,7 +85,7 @@ CreatePipelineBuilder(
|
|||
return builder;
|
||||
}
|
||||
|
||||
std::unique_ptr<carto::common::LuaParameterDictionary> CreateLuaDictionary(
|
||||
std::unique_ptr<carto::common::LuaParameterDictionary> LoadLuaDictionary(
|
||||
const std::string& configuration_directory,
|
||||
const std::string& configuration_basename) {
|
||||
auto file_resolver =
|
||||
|
|
@ -148,18 +143,50 @@ std::unique_ptr<carto::io::PointsBatch> HandleMessage(
|
|||
return points_batch;
|
||||
}
|
||||
|
||||
void RunPipeline(
|
||||
const std::vector<std::unique_ptr<carto::io::PointsProcessor>>& pipeline,
|
||||
const std::vector<std::string>& 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<std::string>& 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<std::unique_ptr<carto::io::PointsProcessor>> 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<std::string>& 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<carto::mapping::proto::Trajectory> 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<std::unique_ptr<carto::io::PointsProcessor>> 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
|
||||
|
|
|
|||
|
|
@ -17,20 +17,35 @@
|
|||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#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<std::string>& 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<std::string>& 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<std::string> 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
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue