12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Switch to batch uploading for cloud based mapping and add retries (#1070)

master
Christoph Schütte 2018-04-18 14:02:43 +02:00 committed by Wally B. Feed
parent 23f6de46b0
commit 214606457c
25 changed files with 345 additions and 265 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
bazel/repositories.bzl
View File

@ -228,9 +228,9 @@ def cartographer_repositories():
_maybe(native.http_archive,
name = "com_github_googlecartographer_async_grpc",
strip_prefix = "async_grpc-654c75ebf553c2bdb624c87a690f5a238aeb651f",
strip_prefix = "async_grpc-c2c68f56904a595ab5ba24c1fb19b4b8e954fa15",
urls = [
"https://github.com/googlecartographer/async_grpc/archive/654c75ebf553c2bdb624c87a690f5a238aeb651f.tar.gz",
"https://github.com/googlecartographer/async_grpc/archive/c2c68f56904a595ab5ba24c1fb19b4b8e954fa15.tar.gz",
],
)

1
cartographer/cloud/internal/client/trajectory_builder_stub.h
View File

@ -23,7 +23,6 @@
#include "cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_imu_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_landmark_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_local_slam_result_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_odometry_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_rangefinder_data_handler.h"
#include "cartographer/cloud/internal/handlers/receive_local_slam_results_handler.h"

119
cartographer/cloud/internal/client_server_test.cc
View File

@ -59,11 +59,28 @@ class ClientServerTest : public ::testing::Test {
MAP_BUILDER_SERVER.num_event_threads = 1
MAP_BUILDER_SERVER.num_grpc_threads = 1
MAP_BUILDER_SERVER.uplink_server_address = ""
MAP_BUILDER_SERVER.server_address = "0.0.0.0:50051"
return MAP_BUILDER_SERVER)text";
auto map_builder_server_parameters =
mapping::test::ResolveLuaParameters(kMapBuilderServerLua);
map_builder_server_options_ =
CreateMapBuilderServerOptions(map_builder_server_parameters.get());
const std::string kUploadingMapBuilderServerLua = R"text(
include "map_builder_server.lua"
MAP_BUILDER.use_trajectory_builder_2d = true
MAP_BUILDER.pose_graph.optimize_every_n_nodes = 0
MAP_BUILDER_SERVER.num_event_threads = 1
MAP_BUILDER_SERVER.num_grpc_threads = 1
MAP_BUILDER_SERVER.uplink_server_address = "localhost:50051"
MAP_BUILDER_SERVER.server_address = "0.0.0.0:50052"
MAP_BUILDER_SERVER.upload_batch_size = 1
return MAP_BUILDER_SERVER)text";
auto uploading_map_builder_server_parameters =
mapping::test::ResolveLuaParameters(kUploadingMapBuilderServerLua);
uploading_map_builder_server_options_ = CreateMapBuilderServerOptions(
uploading_map_builder_server_parameters.get());
const std::string kTrajectoryBuilderLua = R"text(
include "trajectory_builder.lua"
TRAJECTORY_BUILDER.trajectory_builder_2d.use_imu_data = false
@ -73,12 +90,17 @@ class ClientServerTest : public ::testing::Test {
mapping::test::ResolveLuaParameters(kTrajectoryBuilderLua);
trajectory_builder_options_ = mapping::CreateTrajectoryBuilderOptions(
trajectory_builder_parameters.get());
number_of_insertion_results_ = 0;
local_slam_result_callback_ =
[this](
int, common::Time, transform::Rigid3d local_pose, sensor::RangeData,
std::unique_ptr<
const mapping::TrajectoryBuilderInterface::InsertionResult>) {
[this](int, common::Time, transform::Rigid3d local_pose,
sensor::RangeData,
std::unique_ptr<
const mapping::TrajectoryBuilderInterface::InsertionResult>
insertion_result) {
std::unique_lock<std::mutex> lock(local_slam_result_mutex_);
if (insertion_result) {
++number_of_insertion_results_;
}
local_slam_result_poses_.push_back(local_pose);
lock.unlock();
local_slam_result_condition_.notify_all();
@ -93,6 +115,14 @@ class ClientServerTest : public ::testing::Test {
EXPECT_TRUE(server_ != nullptr);
}
void InitializeRealUploadingServer() {
auto map_builder = common::make_unique<MapBuilder>(
uploading_map_builder_server_options_.map_builder_options());
uploading_server_ = common::make_unique<MapBuilderServer>(
uploading_map_builder_server_options_, std::move(map_builder));
EXPECT_TRUE(uploading_server_ != nullptr);
}
void InitializeServerWithMockMapBuilder() {
auto mock_map_builder = common::make_unique<MockMapBuilder>();
mock_map_builder_ = mock_map_builder.get();
@ -108,24 +138,43 @@ class ClientServerTest : public ::testing::Test {
EXPECT_TRUE(stub_ != nullptr);
}
void InitializeStubForUploadingServer() {
stub_for_uploading_server_ = common::make_unique<MapBuilderStub>(
uploading_map_builder_server_options_.server_address());
EXPECT_TRUE(stub_for_uploading_server_ != nullptr);
}
void WaitForLocalSlamResults(size_t size) {
std::unique_lock<std::mutex> lock(local_slam_result_mutex_);
local_slam_result_condition_.wait(
lock, [&] { return local_slam_result_poses_.size() >= size; });
}
void WaitForLocalSlamResultUploads(size_t size) {
std::unique_lock<std::mutex> lock(local_slam_result_upload_mutex_);
local_slam_result_upload_condition_.wait(lock, [&] {
return stub_->pose_graph()->GetTrajectoryNodePoses().size() >= size;
});
}
proto::MapBuilderServerOptions map_builder_server_options_;
proto::MapBuilderServerOptions uploading_map_builder_server_options_;
MockMapBuilder* mock_map_builder_;
std::unique_ptr<MockTrajectoryBuilder> mock_trajectory_builder_;
::cartographer::mapping::proto::TrajectoryBuilderOptions
trajectory_builder_options_;
std::unique_ptr<MapBuilderServer> server_;
std::unique_ptr<MapBuilderServer> uploading_server_;
std::unique_ptr<MapBuilderStub> stub_;
std::unique_ptr<MapBuilderStub> stub_for_uploading_server_;
TrajectoryBuilderInterface::LocalSlamResultCallback
local_slam_result_callback_;
std::condition_variable local_slam_result_condition_;
std::condition_variable local_slam_result_upload_condition_;
std::mutex local_slam_result_mutex_;
std::mutex local_slam_result_upload_mutex_;
std::vector<transform::Rigid3d> local_slam_result_poses_;
int number_of_insertion_results_;
};
TEST_F(ClientServerTest, StartAndStopServer) {
@ -281,6 +330,68 @@ TEST_F(ClientServerTest, GlobalSlam3D) {
server_->Shutdown();
}
TEST_F(ClientServerTest, StartAndStopUploadingServerAndServer) {
InitializeRealServer();
server_->Start();
InitializeRealUploadingServer();
uploading_server_->Start();
uploading_server_->Shutdown();
server_->Shutdown();
}
TEST_F(ClientServerTest, AddTrajectoryBuilderWithUploadingServer) {
InitializeRealServer();
server_->Start();
InitializeRealUploadingServer();
uploading_server_->Start();
InitializeStub();
InitializeStubForUploadingServer();
int trajectory_id = stub_for_uploading_server_->AddTrajectoryBuilder(
{kImuSensorId}, trajectory_builder_options_, nullptr);
EXPECT_FALSE(stub_for_uploading_server_->pose_graph()->IsTrajectoryFinished(
trajectory_id));
EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFinished(trajectory_id));
stub_for_uploading_server_->FinishTrajectory(trajectory_id);
EXPECT_TRUE(stub_for_uploading_server_->pose_graph()->IsTrajectoryFinished(
trajectory_id));
EXPECT_TRUE(stub_->pose_graph()->IsTrajectoryFinished(trajectory_id));
EXPECT_FALSE(stub_for_uploading_server_->pose_graph()->IsTrajectoryFrozen(
trajectory_id));
EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFrozen(trajectory_id));
uploading_server_->Shutdown();
server_->Shutdown();
}
TEST_F(ClientServerTest, LocalSlam2DWithUploadingServer) {
InitializeRealServer();
server_->Start();
InitializeStub();
InitializeRealUploadingServer();
uploading_server_->Start();
InitializeStubForUploadingServer();
int trajectory_id = stub_for_uploading_server_->AddTrajectoryBuilder(
{kRangeSensorId}, trajectory_builder_options_,
local_slam_result_callback_);
TrajectoryBuilderInterface* trajectory_stub =
stub_for_uploading_server_->GetTrajectoryBuilder(trajectory_id);
const auto measurements = mapping::test::GenerateFakeRangeMeasurements(
kTravelDistance, kDuration, kTimeStep);
for (const auto& measurement : measurements) {
trajectory_stub->AddSensorData(kRangeSensorId.id, measurement);
}
WaitForLocalSlamResults(measurements.size());
WaitForLocalSlamResultUploads(number_of_insertion_results_);
stub_for_uploading_server_->FinishTrajectory(trajectory_id);
EXPECT_EQ(local_slam_result_poses_.size(), measurements.size());
EXPECT_NEAR(kTravelDistance,
(local_slam_result_poses_.back().translation() -
local_slam_result_poses_.front().translation())
.norm(),
0.1 * kTravelDistance);
uploading_server_->Shutdown();
server_->Shutdown();
}
} // namespace
} // namespace cloud
} // namespace cartographer

12
cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.cc
View File

@ -45,15 +45,13 @@ void AddFixedFramePoseDataHandler::OnRequest(
// 'MapBuilderContext'.
if (GetUnsynchronizedContext<MapBuilderContextInterface>()
->local_trajectory_uploader()) {
auto data_request =
common::make_unique<proto::AddFixedFramePoseDataRequest>();
CreateAddFixedFramePoseDataRequest(
request.sensor_metadata().sensor_id(),
request.sensor_metadata().trajectory_id(),
request.fixed_frame_pose_data(), data_request.get());
auto sensor_data = common::make_unique<proto::SensorData>();
*sensor_data->mutable_sensor_metadata() = request.sensor_metadata();
*sensor_data->mutable_fixed_frame_pose_data() =
request.fixed_frame_pose_data();
GetUnsynchronizedContext<MapBuilderContextInterface>()
->local_trajectory_uploader()
->EnqueueDataRequest(std::move(data_request));
->EnqueueSensorData(std::move(sensor_data));
}
}

8
cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler_test.cc
View File

@ -74,9 +74,13 @@ TEST_F(AddFixedFramePoseDataHandlerTest, WithMockLocalSlamUploader) {
DoEnqueueSensorData(
Eq(request.sensor_metadata().trajectory_id()),
Pointee(Truly(testing::BuildDataPredicateEquals(request)))));
proto::SensorData sensor_data;
*sensor_data.mutable_sensor_metadata() = request.sensor_metadata();
*sensor_data.mutable_fixed_frame_pose_data() =
request.fixed_frame_pose_data();
EXPECT_CALL(*mock_local_trajectory_uploader_,
DoEnqueueDataRequest(Pointee(
Truly(testing::BuildProtoPredicateEquals(&request)))));
DoEnqueueSensorData(Pointee(
Truly(testing::BuildProtoPredicateEquals(&sensor_data)))));
test_server_->SendWrite(request);
test_server_->SendWritesDone();
test_server_->SendFinish();

9
cartographer/cloud/internal/handlers/add_imu_data_handler.cc
View File

@ -43,13 +43,12 @@ void AddImuDataHandler::OnRequest(const proto::AddImuDataRequest &request) {
// 'MapBuilderContext'.
if (GetUnsynchronizedContext<MapBuilderContextInterface>()
->local_trajectory_uploader()) {
auto data_request = common::make_unique<proto::AddImuDataRequest>();
CreateAddImuDataRequest(request.sensor_metadata().sensor_id(),
request.sensor_metadata().trajectory_id(),
request.imu_data(), data_request.get());
auto sensor_data = common::make_unique<proto::SensorData>();
*sensor_data->mutable_sensor_metadata() = request.sensor_metadata();
*sensor_data->mutable_imu_data() = request.imu_data();
GetUnsynchronizedContext<MapBuilderContextInterface>()
->local_trajectory_uploader()
->EnqueueDataRequest(std::move(data_request));
->EnqueueSensorData(std::move(sensor_data));
}
}

7
cartographer/cloud/internal/handlers/add_imu_data_handler_test.cc
View File

@ -71,9 +71,12 @@ TEST_F(AddImuDataHandlerTest, WithMockLocalSlamUploader) {
DoEnqueueSensorData(
Eq(request.sensor_metadata().trajectory_id()),
Pointee(Truly(testing::BuildDataPredicateEquals(request)))));
proto::SensorData sensor_data;
*sensor_data.mutable_sensor_metadata() = request.sensor_metadata();
*sensor_data.mutable_imu_data() = request.imu_data();
EXPECT_CALL(*mock_local_trajectory_uploader_,
DoEnqueueDataRequest(Pointee(
Truly(testing::BuildProtoPredicateEquals(&request)))));
DoEnqueueSensorData(Pointee(
Truly(testing::BuildProtoPredicateEquals(&sensor_data)))));
test_server_->SendWrite(request);
test_server_->SendWritesDone();
test_server_->SendFinish();

9
cartographer/cloud/internal/handlers/add_landmark_data_handler.cc
View File

@ -44,13 +44,12 @@ void AddLandmarkDataHandler::OnRequest(
// 'MapBuilderContext'.
if (GetUnsynchronizedContext<MapBuilderContextInterface>()
->local_trajectory_uploader()) {
auto data_request = common::make_unique<proto::AddLandmarkDataRequest>();
CreateAddLandmarkDataRequest(request.sensor_metadata().sensor_id(),
request.sensor_metadata().trajectory_id(),
request.landmark_data(), data_request.get());
auto sensor_data = common::make_unique<proto::SensorData>();
*sensor_data->mutable_sensor_metadata() = request.sensor_metadata();
*sensor_data->mutable_landmark_data() = request.landmark_data();
GetUnsynchronizedContext<MapBuilderContextInterface>()
->local_trajectory_uploader()
->EnqueueDataRequest(std::move(data_request));
->EnqueueSensorData(std::move(sensor_data));
}
}

7
cartographer/cloud/internal/handlers/add_landmark_data_handler_test.cc
View File

@ -78,9 +78,12 @@ TEST_F(AddLandmarkDataHandlerTest, WithMockLocalSlamUploader) {
DoEnqueueSensorData(
Eq(request.sensor_metadata().trajectory_id()),
Pointee(Truly(testing::BuildDataPredicateEquals(request)))));
proto::SensorData sensor_data;
*sensor_data.mutable_sensor_metadata() = request.sensor_metadata();
*sensor_data.mutable_landmark_data() = request.landmark_data();
EXPECT_CALL(*mock_local_trajectory_uploader_,
DoEnqueueDataRequest(Pointee(
Truly(testing::BuildProtoPredicateEquals(&request)))));
DoEnqueueSensorData(Pointee(
Truly(testing::BuildProtoPredicateEquals(&sensor_data)))));
test_server_->SendWrite(request);
test_server_->SendWritesDone();
test_server_->SendFinish();

45
cartographer/cloud/internal/handlers/add_local_slam_result_data_handler.cc
View File

@ -1,45 +0,0 @@
/*
* Copyright 2018 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/cloud/internal/handlers/add_local_slam_result_data_handler.h"
#include "async_grpc/rpc_handler.h"
#include "cartographer/cloud/internal/map_builder_context_interface.h"
#include "cartographer/cloud/proto/map_builder_service.pb.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/mapping/local_slam_result_data.h"
#include "cartographer/mapping/trajectory_node.h"
#include "cartographer/sensor/internal/dispatchable.h"
#include "google/protobuf/empty.pb.h"
namespace cartographer {
namespace cloud {
namespace handlers {
void AddLocalSlamResultDataHandler::OnRequest(
const proto::AddLocalSlamResultDataRequest& request) {
GetContext<MapBuilderContextInterface>()->EnqueueLocalSlamResultData(
request.sensor_metadata().trajectory_id(),
request.sensor_metadata().sensor_id(), request.local_slam_result_data());
}
void AddLocalSlamResultDataHandler::OnReadsDone() {
Send(common::make_unique<google::protobuf::Empty>());
}
} // namespace handlers
} // namespace cloud
} // namespace cartographer

9
cartographer/cloud/internal/handlers/add_odometry_data_handler.cc
View File

@ -44,13 +44,12 @@ void AddOdometryDataHandler::OnRequest(
// 'MapBuilderContext'.
if (GetUnsynchronizedContext<MapBuilderContextInterface>()
->local_trajectory_uploader()) {
auto data_request = common::make_unique<proto::AddOdometryDataRequest>();
CreateAddOdometryDataRequest(request.sensor_metadata().sensor_id(),
request.sensor_metadata().trajectory_id(),
request.odometry_data(), data_request.get());
auto sensor_data = common::make_unique<proto::SensorData>();
*sensor_data->mutable_sensor_metadata() = request.sensor_metadata();
*sensor_data->mutable_odometry_data() = request.odometry_data();
GetUnsynchronizedContext<MapBuilderContextInterface>()
->local_trajectory_uploader()
->EnqueueDataRequest(std::move(data_request));
->EnqueueSensorData(std::move(sensor_data));
}
}

7
cartographer/cloud/internal/handlers/add_odometry_data_handler_test.cc
View File

@ -73,9 +73,12 @@ TEST_F(AddOdometryDataHandlerTest, WithMockLocalSlamUploader) {
DoEnqueueSensorData(
Eq(request.sensor_metadata().trajectory_id()),
Pointee(Truly(testing::BuildDataPredicateEquals(request)))));
proto::SensorData sensor_data;
*sensor_data.mutable_sensor_metadata() = request.sensor_metadata();
*sensor_data.mutable_odometry_data() = request.odometry_data();
EXPECT_CALL(*mock_local_trajectory_uploader_,
DoEnqueueDataRequest(Pointee(
Truly(testing::BuildProtoPredicateEquals(&request)))));
DoEnqueueSensorData(Pointee(
Truly(testing::BuildProtoPredicateEquals(&sensor_data)))));
test_server_->SendWrite(request);
test_server_->SendWritesDone();
test_server_->SendFinish();

91
cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.cc Normal file
View File

@ -0,0 +1,91 @@
/*
* Copyright 2018 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/cloud/internal/handlers/add_sensor_data_batch_handler.h"
#include "async_grpc/rpc_handler.h"
#include "cartographer/cloud/internal/map_builder_context_interface.h"
#include "cartographer/cloud/proto/map_builder_service.pb.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/mapping/local_slam_result_data.h"
#include "cartographer/sensor/internal/dispatchable.h"
#include "cartographer/sensor/timed_point_cloud_data.h"
#include "google/protobuf/empty.pb.h"
namespace cartographer {
namespace cloud {
namespace handlers {
void AddSensorDataBatchHandler::OnRequest(
const proto::AddSensorDataBatchRequest& request) {
for (const proto::SensorData& sensor_data : request.sensor_data()) {
switch (sensor_data.sensor_data_case()) {
case proto::SensorData::kOdometryData:
GetUnsynchronizedContext<MapBuilderContextInterface>()
->EnqueueSensorData(
sensor_data.sensor_metadata().trajectory_id(),
sensor::MakeDispatchable(
sensor_data.sensor_metadata().sensor_id(),
sensor::FromProto(sensor_data.odometry_data())));
break;
case proto::SensorData::kImuData:
GetUnsynchronizedContext<MapBuilderContextInterface>()
->EnqueueSensorData(sensor_data.sensor_metadata().trajectory_id(),
sensor::MakeDispatchable(
sensor_data.sensor_metadata().sensor_id(),
sensor::FromProto(sensor_data.imu_data())));
break;
case proto::SensorData::kTimedPointCloudData:
GetUnsynchronizedContext<MapBuilderContextInterface>()
->EnqueueSensorData(
sensor_data.sensor_metadata().trajectory_id(),
sensor::MakeDispatchable(
sensor_data.sensor_metadata().sensor_id(),
sensor::FromProto(sensor_data.timed_point_cloud_data())));
break;
case proto::SensorData::kFixedFramePoseData:
GetUnsynchronizedContext<MapBuilderContextInterface>()
->EnqueueSensorData(
sensor_data.sensor_metadata().trajectory_id(),
sensor::MakeDispatchable(
sensor_data.sensor_metadata().sensor_id(),
sensor::FromProto(sensor_data.fixed_frame_pose_data())));
break;
case proto::SensorData::kLandmarkData:
GetUnsynchronizedContext<MapBuilderContextInterface>()
->EnqueueSensorData(
sensor_data.sensor_metadata().trajectory_id(),
sensor::MakeDispatchable(
sensor_data.sensor_metadata().sensor_id(),
sensor::FromProto(sensor_data.landmark_data())));
break;
case proto::SensorData::kLocalSlamResultData:
GetContext<MapBuilderContextInterface>()->EnqueueLocalSlamResultData(
sensor_data.sensor_metadata().trajectory_id(),
sensor_data.sensor_metadata().sensor_id(),
sensor_data.local_slam_result_data());
break;
default:
LOG(FATAL) << "Unknown sensor data type: "
<< sensor_data.sensor_data_case();
}
}
Send(common::make_unique<google::protobuf::Empty>());
}
} // namespace handlers
} // namespace cloud
} // namespace cartographer

18
cartographer/cloud/internal/handlers/add_local_slam_result_data_handler.h → cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_LOCAL_SLAM_RESULT_DATA_HANDLER_H
#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_LOCAL_SLAM_RESULT_DATA_HANDLER_H
#ifndef CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SENSOR_DATA_BATCH_HANDLER_H
#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SENSOR_DATA_BATCH_HANDLER_H
#include "async_grpc/rpc_handler.h"
#include "cartographer/cloud/proto/map_builder_service.pb.h"
@ -26,20 +26,18 @@ namespace cloud {
namespace handlers {
DEFINE_HANDLER_SIGNATURE(
AddLocalSlamResultDataSignature,
async_grpc::Stream<proto::AddLocalSlamResultDataRequest>,
AddSensorDataBatchSignature, proto::AddSensorDataBatchRequest,
google::protobuf::Empty,
"/cartographer.cloud.proto.MapBuilderService/AddLocalSlamResultData")
"/cartographer.cloud.proto.MapBuilderService/AddSensorDataBatch")
class AddLocalSlamResultDataHandler
: public async_grpc::RpcHandler<AddLocalSlamResultDataSignature> {
class AddSensorDataBatchHandler
: public async_grpc::RpcHandler<AddSensorDataBatchSignature> {
public:
void OnRequest(const proto::AddLocalSlamResultDataRequest& request) override;
void OnReadsDone() override;
void OnRequest(const proto::AddSensorDataBatchRequest& request) override;
};
} // namespace handlers
} // namespace cloud
} // namespace cartographer
#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_LOCAL_SLAM_RESULT_DATA_HANDLER_H
#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_ADD_SENSOR_DATA_BATCH_HANDLER_H

183
cartographer/cloud/internal/local_trajectory_uploader.cc
View File

@ -20,15 +20,10 @@
#include <thread>
#include "async_grpc/client.h"
#include "cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_imu_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_landmark_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_local_slam_result_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_odometry_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.h"
#include "cartographer/cloud/internal/handlers/add_trajectory_handler.h"
#include "cartographer/cloud/internal/handlers/finish_trajectory_handler.h"
#include "cartographer/cloud/internal/sensor/serialization.h"
#include "cartographer/cloud/proto/map_builder_service.pb.h"
#include "cartographer/common/blocking_queue.h"
#include "cartographer/common/make_unique.h"
#include "glog/logging.h"
@ -45,8 +40,9 @@ const common::Duration kPopTimeout = common::FromMilliseconds(100);
class LocalTrajectoryUploader : public LocalTrajectoryUploaderInterface {
public:
LocalTrajectoryUploader(const std::string &uplink_server_address);
~LocalTrajectoryUploader();
LocalTrajectoryUploader(const std::string &uplink_server_address,
int batch_size);
~LocalTrajectoryUploader() {}
// Starts the upload thread.
void Start() final;
@ -59,8 +55,7 @@ class LocalTrajectoryUploader : public LocalTrajectoryUploaderInterface {
int local_trajectory_id, const std::set<SensorId> &expected_sensor_ids,
const mapping::proto::TrajectoryBuilderOptions &trajectory_options) final;
void FinishTrajectory(int local_trajectory_id) final;
void EnqueueDataRequest(
std::unique_ptr<google::protobuf::Message> data_request) final;
void EnqueueSensorData(std::unique_ptr<proto::SensorData> sensor_data) final;
SensorId GetLocalSlamResultSensorId(int local_trajectory_id) const final {
return SensorId{SensorId::SensorType::LOCAL_SLAM_RESULT,
@ -70,35 +65,20 @@ class LocalTrajectoryUploader : public LocalTrajectoryUploaderInterface {
private:
void ProcessSendQueue();
void TranslateTrajectoryId(proto::SensorMetadata *sensor_metadata);
void ProcessFixedFramePoseDataMessage(
proto::AddFixedFramePoseDataRequest *data_request);
void ProcessImuDataMessage(proto::AddImuDataRequest *data_request);
void ProcessLocalSlamResultDataMessage(
proto::AddLocalSlamResultDataRequest *data_request);
void ProcessOdometryDataMessage(proto::AddOdometryDataRequest *data_request);
void ProcessLandmarkDataMessage(proto::AddLandmarkDataRequest *data_request);
std::shared_ptr<::grpc::Channel> client_channel_;
int batch_size_;
std::map<int, int> local_to_cloud_trajectory_id_map_;
common::BlockingQueue<std::unique_ptr<google::protobuf::Message>> send_queue_;
common::BlockingQueue<std::unique_ptr<proto::SensorData>> send_queue_;
bool shutting_down_ = false;
std::unique_ptr<std::thread> upload_thread_;
std::unique_ptr<async_grpc::Client<handlers::AddFixedFramePoseDataSignature>>
add_fixed_frame_pose_client_;
std::unique_ptr<async_grpc::Client<handlers::AddImuDataSignature>>
add_imu_client_;
std::unique_ptr<async_grpc::Client<handlers::AddLocalSlamResultDataSignature>>
add_local_slam_result_client_;
std::unique_ptr<async_grpc::Client<handlers::AddOdometryDataSignature>>
add_odometry_client_;
std::unique_ptr<async_grpc::Client<handlers::AddLandmarkDataSignature>>
add_landmark_client_;
};
LocalTrajectoryUploader::LocalTrajectoryUploader(
const std::string &uplink_server_address)
const std::string &uplink_server_address, int batch_size)
: client_channel_(::grpc::CreateChannel(
uplink_server_address, ::grpc::InsecureChannelCredentials())) {
uplink_server_address, ::grpc::InsecureChannelCredentials())),
batch_size_(batch_size) {
std::chrono::system_clock::time_point deadline(
std::chrono::system_clock::now() +
std::chrono::seconds(kConnectionTimeoutInSecond));
@ -108,29 +88,6 @@ LocalTrajectoryUploader::LocalTrajectoryUploader(
}
}
LocalTrajectoryUploader::~LocalTrajectoryUploader() {
if (add_imu_client_) {
CHECK(add_imu_client_->StreamWritesDone());
CHECK(add_imu_client_->StreamFinish().ok());
}
if (add_odometry_client_) {
CHECK(add_odometry_client_->StreamWritesDone());
CHECK(add_odometry_client_->StreamFinish().ok());
}
if (add_fixed_frame_pose_client_) {
CHECK(add_fixed_frame_pose_client_->StreamWritesDone());
CHECK(add_fixed_frame_pose_client_->StreamFinish().ok());
}
if (add_local_slam_result_client_) {
CHECK(add_local_slam_result_client_->StreamWritesDone());
CHECK(add_local_slam_result_client_->StreamFinish().ok());
}
if (add_landmark_client_) {
CHECK(add_landmark_client_->StreamWritesDone());
CHECK(add_landmark_client_->StreamFinish().ok());
}
}
void LocalTrajectoryUploader::Start() {
CHECK(!shutting_down_);
CHECK(!upload_thread_);
@ -147,30 +104,31 @@ void LocalTrajectoryUploader::Shutdown() {
void LocalTrajectoryUploader::ProcessSendQueue() {
LOG(INFO) << "Starting uploader thread.";
proto::AddSensorDataBatchRequest batch_request;
while (!shutting_down_) {
auto data_message = send_queue_.PopWithTimeout(kPopTimeout);
if (data_message) {
if (auto *fixed_frame_pose_data =
dynamic_cast<proto::AddFixedFramePoseDataRequest *>(
data_message.get())) {
ProcessFixedFramePoseDataMessage(fixed_frame_pose_data);
} else if (auto *imu_data = dynamic_cast<proto::AddImuDataRequest *>(
data_message.get())) {
ProcessImuDataMessage(imu_data);
} else if (auto *odometry_data =
dynamic_cast<proto::AddOdometryDataRequest *>(
data_message.get())) {
ProcessOdometryDataMessage(odometry_data);
} else if (auto *local_slam_result_data =
dynamic_cast<proto::AddLocalSlamResultDataRequest *>(
data_message.get())) {
ProcessLocalSlamResultDataMessage(local_slam_result_data);
} else if (auto *landmark_data =
dynamic_cast<proto::AddLandmarkDataRequest *>(
data_message.get())) {
ProcessLandmarkDataMessage(landmark_data);
} else {
LOG(FATAL) << "Unknown message type: " << data_message->GetTypeName();
auto sensor_data = send_queue_.PopWithTimeout(kPopTimeout);
if (sensor_data) {
proto::SensorData *added_sensor_data = batch_request.add_sensor_data();
*added_sensor_data = *sensor_data;
TranslateTrajectoryId(added_sensor_data->mutable_sensor_metadata());
// A submap also holds a trajectory id that must be translated to uplink's
// trajectory id.
if (added_sensor_data->has_local_slam_result_data()) {
for (mapping::proto::Submap &mutable_submap :
*added_sensor_data->mutable_local_slam_result_data()
->mutable_submaps()) {
mutable_submap.mutable_submap_id()->set_trajectory_id(
added_sensor_data->sensor_metadata().trajectory_id());
}
}
if (batch_request.sensor_data_size() == batch_size_) {
async_grpc::Client<handlers::AddSensorDataBatchSignature> client(
client_channel_, async_grpc::CreateUnlimitedConstantDelayStrategy(
common::FromSeconds(1)));
CHECK(client.Write(batch_request));
batch_request.clear_sensor_data();
}
}
}
@ -183,68 +141,6 @@ void LocalTrajectoryUploader::TranslateTrajectoryId(
sensor_metadata->set_trajectory_id(cloud_trajectory_id);
}
void LocalTrajectoryUploader::ProcessFixedFramePoseDataMessage(
proto::AddFixedFramePoseDataRequest *data_request) {
if (!add_fixed_frame_pose_client_) {
add_fixed_frame_pose_client_ = make_unique<
async_grpc::Client<handlers::AddFixedFramePoseDataSignature>>(
client_channel_);
}
TranslateTrajectoryId(data_request->mutable_sensor_metadata());
CHECK(add_fixed_frame_pose_client_->Write(*data_request));
}
void LocalTrajectoryUploader::ProcessImuDataMessage(
proto::AddImuDataRequest *data_request) {
if (!add_imu_client_) {
add_imu_client_ =
make_unique<async_grpc::Client<handlers::AddImuDataSignature>>(
client_channel_);
}
TranslateTrajectoryId(data_request->mutable_sensor_metadata());
CHECK(add_imu_client_->Write(*data_request));
}
void LocalTrajectoryUploader::ProcessOdometryDataMessage(
proto::AddOdometryDataRequest *data_request) {
if (!add_odometry_client_) {
add_odometry_client_ =
make_unique<async_grpc::Client<handlers::AddOdometryDataSignature>>(
client_channel_);
}
TranslateTrajectoryId(data_request->mutable_sensor_metadata());
CHECK(add_odometry_client_->Write(*data_request));
}
void LocalTrajectoryUploader::ProcessLandmarkDataMessage(
proto::AddLandmarkDataRequest *data_request) {
if (!add_landmark_client_) {
add_landmark_client_ =
make_unique<async_grpc::Client<handlers::AddLandmarkDataSignature>>(
client_channel_);
}
TranslateTrajectoryId(data_request->mutable_sensor_metadata());
CHECK(add_landmark_client_->Write(*data_request));
}
void LocalTrajectoryUploader::ProcessLocalSlamResultDataMessage(
proto::AddLocalSlamResultDataRequest *data_request) {
if (!add_local_slam_result_client_) {
add_local_slam_result_client_ = make_unique<
async_grpc::Client<handlers::AddLocalSlamResultDataSignature>>(
client_channel_);
}
TranslateTrajectoryId(data_request->mutable_sensor_metadata());
// A submap also holds a trajectory id that must be translated to uplink's
// trajectory id.
for (mapping::proto::Submap &mutable_submap :
*data_request->mutable_local_slam_result_data()->mutable_submaps()) {
mutable_submap.mutable_submap_id()->set_trajectory_id(
data_request->sensor_metadata().trajectory_id());
}
CHECK(add_local_slam_result_client_->Write(*data_request));
}
void LocalTrajectoryUploader::AddTrajectory(
int local_trajectory_id, const std::set<SensorId> &expected_sensor_ids,
const mapping::proto::TrajectoryBuilderOptions &trajectory_options) {
@ -276,16 +172,17 @@ void LocalTrajectoryUploader::FinishTrajectory(int local_trajectory_id) {
CHECK(client.Write(request));
}
void LocalTrajectoryUploader::EnqueueDataRequest(
std::unique_ptr<google::protobuf::Message> data_request) {
send_queue_.Push(std::move(data_request));
void LocalTrajectoryUploader::EnqueueSensorData(
std::unique_ptr<proto::SensorData> sensor_data) {
send_queue_.Push(std::move(sensor_data));
}
} // namespace
std::unique_ptr<LocalTrajectoryUploaderInterface> CreateLocalTrajectoryUploader(
const std::string &uplink_server_address) {
return make_unique<LocalTrajectoryUploader>(uplink_server_address);
const std::string &uplink_server_address, int batch_size) {
return make_unique<LocalTrajectoryUploader>(uplink_server_address,
batch_size);
}
} // namespace cloud

7
cartographer/cloud/internal/local_trajectory_uploader.h
View File

@ -21,6 +21,7 @@
#include <set>
#include <string>
#include "cartographer/cloud/proto/map_builder_service.pb.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
@ -41,8 +42,8 @@ class LocalTrajectoryUploaderInterface {
virtual void Shutdown() = 0;
// Enqueue an Add*DataRequest message to be uploaded.
virtual void EnqueueDataRequest(
std::unique_ptr<google::protobuf::Message> data_request) = 0;
virtual void EnqueueSensorData(
std::unique_ptr<proto::SensorData> sensor_data) = 0;
virtual void AddTrajectory(
int local_trajectory_id, const std::set<SensorId>& expected_sensor_ids,
const mapping::proto::TrajectoryBuilderOptions& trajectory_options) = 0;
@ -54,7 +55,7 @@ class LocalTrajectoryUploaderInterface {
// Returns LocalTrajectoryUploader with the actual implementation.
std::unique_ptr<LocalTrajectoryUploaderInterface> CreateLocalTrajectoryUploader(
const std::string& uplink_server_address);
const std::string& uplink_server_address, int batch_size);
} // namespace cloud
} // namespace cartographer

23
cartographer/cloud/internal/map_builder_server.cc
View File

@ -19,9 +19,9 @@
#include "cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_imu_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_landmark_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_local_slam_result_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_odometry_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_rangefinder_data_handler.h"
#include "cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.h"
#include "cartographer/cloud/internal/handlers/add_trajectory_handler.h"
#include "cartographer/cloud/internal/handlers/finish_trajectory_handler.h"
#include "cartographer/cloud/internal/handlers/get_all_submap_poses.h"
@ -60,7 +60,8 @@ MapBuilderServer::MapBuilderServer(
map_builder_server_options.num_event_threads());
if (!map_builder_server_options.uplink_server_address().empty()) {
local_trajectory_uploader_ = CreateLocalTrajectoryUploader(
map_builder_server_options.uplink_server_address());
map_builder_server_options.uplink_server_address(),
map_builder_server_options.upload_batch_size());
}
server_builder.RegisterHandler<handlers::AddTrajectoryHandler>();
server_builder.RegisterHandler<handlers::AddOdometryDataHandler>();
@ -68,7 +69,7 @@ MapBuilderServer::MapBuilderServer(
server_builder.RegisterHandler<handlers::AddRangefinderDataHandler>();
server_builder.RegisterHandler<handlers::AddFixedFramePoseDataHandler>();
server_builder.RegisterHandler<handlers::AddLandmarkDataHandler>();
server_builder.RegisterHandler<handlers::AddLocalSlamResultDataHandler>();
server_builder.RegisterHandler<handlers::AddSensorDataBatchHandler>();
server_builder.RegisterHandler<handlers::FinishTrajectoryHandler>();
server_builder.RegisterHandler<handlers::ReceiveLocalSlamResultsHandler>();
server_builder.RegisterHandler<handlers::GetSubmapHandler>();
@ -122,6 +123,11 @@ void MapBuilderServer::Shutdown() {
grpc_server_->Shutdown();
if (slam_thread_) {
slam_thread_->join();
slam_thread_.reset();
}
if (local_trajectory_uploader_) {
local_trajectory_uploader_->Shutdown();
local_trajectory_uploader_.reset();
}
}
@ -158,22 +164,21 @@ void MapBuilderServer::OnLocalSlamResult(
if (insertion_result &&
grpc_server_->GetUnsynchronizedContext<MapBuilderContextInterface>()
->local_trajectory_uploader()) {
auto data_request =
common::make_unique<proto::AddLocalSlamResultDataRequest>();
auto sensor_data = common::make_unique<proto::SensorData>();
auto sensor_id =
grpc_server_->GetUnsynchronizedContext<MapBuilderContextInterface>()
->local_trajectory_uploader()
->GetLocalSlamResultSensorId(trajectory_id);
CreateAddLocalSlamResultDataRequest(sensor_id.id, trajectory_id, time,
starting_submap_index_,
*insertion_result, data_request.get());
CreateSensorDataForLocalSlamResult(sensor_id.id, trajectory_id, time,
starting_submap_index_,
*insertion_result, sensor_data.get());
// TODO(cschuet): Make this more robust.
if (insertion_result->insertion_submaps.front()->finished()) {
++starting_submap_index_;
}
grpc_server_->GetUnsynchronizedContext<MapBuilderContextInterface>()
->local_trajectory_uploader()
->EnqueueDataRequest(std::move(data_request));
->EnqueueSensorData(std::move(sensor_data));
}
common::MutexLocker locker(&local_slam_subscriptions_lock_);

4
cartographer/cloud/internal/sensor/serialization.cc
View File

@ -70,12 +70,12 @@ void CreateAddLandmarkDataRequest(
*proto->mutable_landmark_data() = landmark_data;
}
void CreateAddLocalSlamResultDataRequest(
void CreateSensorDataForLocalSlamResult(
const std::string& sensor_id, int trajectory_id, common::Time time,
int starting_submap_index,
const mapping::TrajectoryBuilderInterface::InsertionResult&
insertion_result,
proto::AddLocalSlamResultDataRequest* proto) {
proto::SensorData* proto) {
CreateSensorMetadata(sensor_id, trajectory_id,
proto->mutable_sensor_metadata());
proto->mutable_local_slam_result_data()->set_timestamp(

4
cartographer/cloud/internal/sensor/serialization.h
View File

@ -51,12 +51,12 @@ void CreateAddLandmarkDataRequest(
const std::string& sensor_id, int trajectory_id,
const sensor::proto::LandmarkData& landmark_data,
proto::AddLandmarkDataRequest* proto);
void CreateAddLocalSlamResultDataRequest(
void CreateSensorDataForLocalSlamResult(
const std::string& sensor_id, int trajectory_id, common::Time time,
int starting_submap_index,
const mapping::TrajectoryBuilderInterface::InsertionResult&
insertion_result,
proto::AddLocalSlamResultDataRequest* proto);
proto::SensorData* proto);
proto::SensorId ToProto(
const mapping::TrajectoryBuilderInterface::SensorId& sensor_id);

8
cartographer/cloud/internal/testing/mock_local_trajectory_uploader.h
View File

@ -28,10 +28,10 @@ namespace testing {
class MockLocalTrajectoryUploader : public LocalTrajectoryUploaderInterface {
public:
MOCK_METHOD1(DoEnqueueDataRequest, void(google::protobuf::Message *));
void EnqueueDataRequest(
std::unique_ptr<google::protobuf::Message> data_request) override {
DoEnqueueDataRequest(data_request.get());
MOCK_METHOD1(DoEnqueueSensorData, void(proto::SensorData *));
void EnqueueSensorData(
std::unique_ptr<proto::SensorData> data_request) override {
DoEnqueueSensorData(data_request.get());
}
MOCK_METHOD0(Start, void());
MOCK_METHOD0(Shutdown, void());

2
cartographer/cloud/map_builder_server_options.cc
View File

@ -37,6 +37,8 @@ proto::MapBuilderServerOptions CreateMapBuilderServerOptions(
lua_parameter_dictionary->GetDictionary("map_builder").get());
map_builder_server_options.set_uplink_server_address(
lua_parameter_dictionary->GetString("uplink_server_address"));
map_builder_server_options.set_upload_batch_size(
lua_parameter_dictionary->GetInt("upload_batch_size"));
return map_builder_server_options;
}

1
cartographer/cloud/proto/map_builder_server_options.proto
View File

@ -24,4 +24,5 @@ message MapBuilderServerOptions {
int32 num_event_threads = 3;
cartographer.mapping.proto.MapBuilderOptions map_builder_options = 4;
string uplink_server_address = 5;
int32 upload_batch_size = 6;
}

29
cartographer/cloud/proto/map_builder_service.proto
View File

@ -49,10 +49,26 @@ message SensorMetadata {
string sensor_id = 2;
}
message SensorData {
SensorMetadata sensor_metadata = 1;
oneof sensor_data {
cartographer.sensor.proto.OdometryData odometry_data = 2;
cartographer.sensor.proto.ImuData imu_data = 3;
cartographer.sensor.proto.TimedPointCloudData timed_point_cloud_data = 4;
cartographer.sensor.proto.FixedFramePoseData fixed_frame_pose_data = 5;
cartographer.sensor.proto.LandmarkData landmark_data = 6;
cartographer.mapping.proto.LocalSlamResultData local_slam_result_data = 7;
}
}
message AddTrajectoryResponse {
int32 trajectory_id = 1;
}
message AddSensorDataBatchRequest {
repeated SensorData sensor_data = 1;
}
message AddOdometryDataRequest {
SensorMetadata sensor_metadata = 1;
cartographer.sensor.proto.OdometryData odometry_data = 2;
@ -156,11 +172,6 @@ message GetConstraintsResponse {
repeated cartographer.mapping.proto.PoseGraph.Constraint constraints = 1;
}
message AddLocalSlamResultDataRequest {
SensorMetadata sensor_metadata = 1;
cartographer.mapping.proto.LocalSlamResultData local_slam_result_data = 2;
}
message WriteStateResponse {
oneof state_chunk {
cartographer.mapping.proto.PoseGraph pose_graph = 1;
@ -190,6 +201,10 @@ service MapBuilderService {
// Starts a new trajectory and returns its index.
rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse);
// Adds a batch of sensor data to a trajectory.
rpc AddSensorDataBatch(AddSensorDataBatchRequest)
returns (google.protobuf.Empty);
// Adds odometry data from the sensor with id 'sensor_metadata.sensor_id' to
// the trajectory corresponding to 'sensor_metadata.trajectory_id'.
rpc AddOdometryData(stream AddOdometryDataRequest)
@ -210,10 +225,6 @@ service MapBuilderService {
rpc AddLandmarkData(stream AddLandmarkDataRequest)
returns (google.protobuf.Empty);
// Adds a local SLAM result.
rpc AddLocalSlamResultData(stream AddLocalSlamResultDataRequest)
returns (google.protobuf.Empty);
// Requests the server to send a stream of local SLAM results for the given
// 'trajectory_id'.
rpc ReceiveLocalSlamResults(ReceiveLocalSlamResultsRequest)

1
configuration_files/map_builder_server.lua
View File

@ -20,4 +20,5 @@ MAP_BUILDER_SERVER = {
num_grpc_threads = 4,
server_address = "0.0.0.0:50051",
uplink_server_address = "",
upload_batch_size = 100,
}

2
scripts/install_async_grpc.sh
View File

@ -19,7 +19,7 @@ set -o verbose
git clone https://github.com/googlecartographer/async_grpc
cd async_grpc
git checkout 654c75ebf553c2bdb624c87a690f5a238aeb651f
git checkout c2c68f56904a595ab5ba24c1fb19b4b8e954fa15
mkdir build
cd build
cmake -G Ninja \