Clean up use of MapBuilderInterface. (#1776)

Moves options next to the interface like we do for other interfaces.
Adds a factory function to remove the need for direct use of MapBuilder.

Signed-off-by: Wolfgang Hess <whess@lyft.com>
master
Wolfgang Hess 2020-11-06 11:31:49 +01:00 committed by GitHub
parent 38dcf65d8f
commit b1ce24d3f8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
11 changed files with 64 additions and 31 deletions

View File

@ -29,12 +29,13 @@
#include "cartographer/mapping/internal/testing/mock_trajectory_builder.h" #include "cartographer/mapping/internal/testing/mock_trajectory_builder.h"
#include "cartographer/mapping/internal/testing/test_helpers.h" #include "cartographer/mapping/internal/testing/test_helpers.h"
#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/local_slam_result_data.h"
#include "cartographer/mapping/map_builder.h"
#include "glog/logging.h" #include "glog/logging.h"
#include "gmock/gmock.h" #include "gmock/gmock.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
using ::cartographer::io::testing::ProtoReaderFromStrings; using ::cartographer::io::testing::ProtoReaderFromStrings;
using ::cartographer::mapping::MapBuilder; using ::cartographer::mapping::CreateMapBuilder;
using ::cartographer::mapping::MapBuilderInterface; using ::cartographer::mapping::MapBuilderInterface;
using ::cartographer::mapping::PoseGraphInterface; using ::cartographer::mapping::PoseGraphInterface;
using ::cartographer::mapping::TrajectoryBuilderInterface; using ::cartographer::mapping::TrajectoryBuilderInterface;
@ -139,15 +140,15 @@ class ClientServerTestBase : public T {
} }
void InitializeRealServer() { void InitializeRealServer() {
auto map_builder = absl::make_unique<MapBuilder>( auto map_builder =
map_builder_server_options_.map_builder_options()); CreateMapBuilder(map_builder_server_options_.map_builder_options());
server_ = absl::make_unique<MapBuilderServer>(map_builder_server_options_, server_ = absl::make_unique<MapBuilderServer>(map_builder_server_options_,
std::move(map_builder)); std::move(map_builder));
EXPECT_TRUE(server_ != nullptr); EXPECT_TRUE(server_ != nullptr);
} }
void InitializeRealUploadingServer() { void InitializeRealUploadingServer() {
auto map_builder = absl::make_unique<MapBuilder>( auto map_builder = CreateMapBuilder(
uploading_map_builder_server_options_.map_builder_options()); uploading_map_builder_server_options_.map_builder_options());
uploading_server_ = absl::make_unique<MapBuilderServer>( uploading_server_ = absl::make_unique<MapBuilderServer>(
uploading_map_builder_server_options_, std::move(map_builder)); uploading_map_builder_server_options_, std::move(map_builder));

View File

@ -29,7 +29,6 @@
#include "cartographer/mapping/3d/submap_3d.h" #include "cartographer/mapping/3d/submap_3d.h"
#include "cartographer/mapping/internal/submap_controller.h" #include "cartographer/mapping/internal/submap_controller.h"
#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/local_slam_result_data.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer/metrics/family_factory.h" #include "cartographer/metrics/family_factory.h"
#include "cartographer/sensor/internal/dispatchable.h" #include "cartographer/sensor/internal/dispatchable.h"

View File

@ -50,7 +50,7 @@ void Run(const std::string& configuration_directory,
proto::MapBuilderServerOptions map_builder_server_options = proto::MapBuilderServerOptions map_builder_server_options =
LoadMapBuilderServerOptions(configuration_directory, LoadMapBuilderServerOptions(configuration_directory,
configuration_basename); configuration_basename);
auto map_builder = absl::make_unique<mapping::MapBuilder>( auto map_builder = mapping::CreateMapBuilder(
map_builder_server_options.map_builder_options()); map_builder_server_options.map_builder_options());
std::unique_ptr<MapBuilderServerInterface> map_builder_server = std::unique_ptr<MapBuilderServerInterface> map_builder_server =
CreateMapBuilderServer(map_builder_server_options, CreateMapBuilderServer(map_builder_server_options,

View File

@ -18,7 +18,7 @@
#include "absl/memory/memory.h" #include "absl/memory/memory.h"
#include "cartographer/common/configuration_file_resolver.h" #include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/mapping/map_builder.h" #include "cartographer/mapping/map_builder_interface.h"
namespace cartographer { namespace cartographer {
namespace cloud { namespace cloud {

View File

@ -20,7 +20,7 @@
#include "cartographer/common/config.h" #include "cartographer/common/config.h"
#include "cartographer/common/configuration_file_resolver.h" #include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/map_builder.h" #include "cartographer/mapping/map_builder_interface.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
namespace cartographer { namespace cartographer {

View File

@ -28,7 +28,6 @@
#include "cartographer/mapping/id.h" #include "cartographer/mapping/id.h"
#include "cartographer/mapping/internal/3d/pose_graph_3d.h" #include "cartographer/mapping/internal/3d/pose_graph_3d.h"
#include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h" #include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/probability_values.h" #include "cartographer/mapping/probability_values.h"

View File

@ -74,24 +74,6 @@ void MaybeAddPureLocalizationTrimmer(
} // namespace } // namespace
proto::MapBuilderOptions CreateMapBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::MapBuilderOptions options;
options.set_use_trajectory_builder_2d(
parameter_dictionary->GetBool("use_trajectory_builder_2d"));
options.set_use_trajectory_builder_3d(
parameter_dictionary->GetBool("use_trajectory_builder_3d"));
options.set_num_background_threads(
parameter_dictionary->GetNonNegativeInt("num_background_threads"));
options.set_collate_by_trajectory(
parameter_dictionary->GetBool("collate_by_trajectory"));
*options.mutable_pose_graph_options() = CreatePoseGraphOptions(
parameter_dictionary->GetDictionary("pose_graph").get());
CHECK_NE(options.use_trajectory_builder_2d(),
options.use_trajectory_builder_3d());
return options;
}
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options) MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
: options_(options), thread_pool_(options.num_background_threads()) { : options_(options), thread_pool_(options.num_background_threads()) {
CHECK(options.use_trajectory_builder_2d() ^ CHECK(options.use_trajectory_builder_2d() ^
@ -414,5 +396,10 @@ std::map<int, int> MapBuilder::LoadStateFromFile(
return LoadState(&stream, load_frozen_state); return LoadState(&stream, load_frozen_state);
} }
std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
const proto::MapBuilderOptions& options) {
return absl::make_unique<MapBuilder>(options);
}
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

View File

@ -28,9 +28,6 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
proto::MapBuilderOptions CreateMapBuilderOptions(
common::LuaParameterDictionary *const parameter_dictionary);
// Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps) // Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps)
// and a PoseGraph for loop closure. // and a PoseGraph for loop closure.
class MapBuilder : public MapBuilderInterface { class MapBuilder : public MapBuilderInterface {
@ -98,6 +95,9 @@ class MapBuilder : public MapBuilderInterface {
all_trajectory_builder_options_; all_trajectory_builder_options_;
}; };
std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
const proto::MapBuilderOptions& options);
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer

View File

@ -0,0 +1,43 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/pose_graph.h"
namespace cartographer {
namespace mapping {
proto::MapBuilderOptions CreateMapBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::MapBuilderOptions options;
options.set_use_trajectory_builder_2d(
parameter_dictionary->GetBool("use_trajectory_builder_2d"));
options.set_use_trajectory_builder_3d(
parameter_dictionary->GetBool("use_trajectory_builder_3d"));
options.set_num_background_threads(
parameter_dictionary->GetNonNegativeInt("num_background_threads"));
options.set_collate_by_trajectory(
parameter_dictionary->GetBool("collate_by_trajectory"));
*options.mutable_pose_graph_options() = CreatePoseGraphOptions(
parameter_dictionary->GetDictionary("pose_graph").get());
CHECK_NE(options.use_trajectory_builder_2d(),
options.use_trajectory_builder_3d());
return options;
}
} // namespace mapping
} // namespace cartographer

View File

@ -27,6 +27,7 @@
#include "cartographer/io/proto_stream_interface.h" #include "cartographer/io/proto_stream_interface.h"
#include "cartographer/mapping/id.h" #include "cartographer/mapping/id.h"
#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/proto/map_builder_options.pb.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
@ -35,6 +36,9 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
proto::MapBuilderOptions CreateMapBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary);
// This interface is used for both library and RPC implementations. // This interface is used for both library and RPC implementations.
// Implementations wire up the complete SLAM stack. // Implementations wire up the complete SLAM stack.
class MapBuilderInterface { class MapBuilderInterface {

View File

@ -64,7 +64,7 @@ class MapBuilderTestBase : public T {
} }
void BuildMapBuilder() { void BuildMapBuilder() {
map_builder_ = absl::make_unique<MapBuilder>(map_builder_options_); map_builder_ = CreateMapBuilder(map_builder_options_);
} }
void SetOptionsTo3D() { void SetOptionsTo3D() {