Implement sensor data forwarding. (#818)

master
Christoph Schütte 2018-01-15 15:31:33 +01:00 committed by Wally B. Feed
parent 903e6432b9
commit c053fc7a2f
13 changed files with 214 additions and 39 deletions

View File

@ -21,6 +21,7 @@
#include "cartographer_grpc/framework/rpc_handler.h" #include "cartographer_grpc/framework/rpc_handler.h"
#include "cartographer_grpc/map_builder_server.h" #include "cartographer_grpc/map_builder_server.h"
#include "cartographer_grpc/proto/map_builder_service.pb.h" #include "cartographer_grpc/proto/map_builder_service.pb.h"
#include "cartographer_grpc/sensor/serialization.h"
#include "google/protobuf/empty.pb.h" #include "google/protobuf/empty.pb.h"
namespace cartographer_grpc { namespace cartographer_grpc {
@ -40,6 +41,22 @@ class AddFixedFramePoseDataHandler
request.sensor_metadata().trajectory_id(), request.sensor_metadata().trajectory_id(),
request.sensor_metadata().sensor_id(), request.sensor_metadata().sensor_id(),
cartographer::sensor::FromProto(request.fixed_frame_pose_data())); cartographer::sensor::FromProto(request.fixed_frame_pose_data()));
// The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe.
// Therefore it suffices to get an unsynchronized reference to the
// 'MapBuilderContext'.
if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
->local_trajectory_uploader()) {
auto data_request = cartographer::common::make_unique<
proto::AddFixedFramePoseDataRequest>();
mapping::CreateAddFixedFramePoseDataRequest(
request.sensor_metadata().sensor_id(),
request.sensor_metadata().trajectory_id(),
request.fixed_frame_pose_data(), data_request.get());
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
->local_trajectory_uploader()
->EnqueueDataRequest(std::move(data_request));
}
} }
void OnReadsDone() override { void OnReadsDone() override {

View File

@ -21,6 +21,7 @@
#include "cartographer_grpc/framework/rpc_handler.h" #include "cartographer_grpc/framework/rpc_handler.h"
#include "cartographer_grpc/map_builder_server.h" #include "cartographer_grpc/map_builder_server.h"
#include "cartographer_grpc/proto/map_builder_service.pb.h" #include "cartographer_grpc/proto/map_builder_service.pb.h"
#include "cartographer_grpc/sensor/serialization.h"
#include "google/protobuf/empty.pb.h" #include "google/protobuf/empty.pb.h"
namespace cartographer_grpc { namespace cartographer_grpc {
@ -39,6 +40,22 @@ class AddImuDataHandler
request.sensor_metadata().trajectory_id(), request.sensor_metadata().trajectory_id(),
request.sensor_metadata().sensor_id(), request.sensor_metadata().sensor_id(),
cartographer::sensor::FromProto(request.imu_data())); cartographer::sensor::FromProto(request.imu_data()));
// The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe.
// Therefore it suffices to get an unsynchronized reference to the
// 'MapBuilderContext'.
if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
->local_trajectory_uploader()) {
auto data_request =
cartographer::common::make_unique<proto::AddImuDataRequest>();
mapping::CreateAddImuDataRequest(
request.sensor_metadata().sensor_id(),
request.sensor_metadata().trajectory_id(), request.imu_data(),
data_request.get());
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
->local_trajectory_uploader()
->EnqueueDataRequest(std::move(data_request));
}
} }
void OnReadsDone() override { void OnReadsDone() override {

View File

@ -40,6 +40,22 @@ class AddOdometryDataHandler
request.sensor_metadata().trajectory_id(), request.sensor_metadata().trajectory_id(),
request.sensor_metadata().sensor_id(), request.sensor_metadata().sensor_id(),
cartographer::sensor::FromProto(request.odometry_data())); cartographer::sensor::FromProto(request.odometry_data()));
// The 'BlockingQueue' in 'LocalTrajectoryUploader' is thread-safe.
// Therefore it suffices to get an unsynchronized reference to the
// 'MapBuilderContext'.
if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
->local_trajectory_uploader()) {
auto data_request =
cartographer::common::make_unique<proto::AddOdometryDataRequest>();
mapping::CreateAddOdometryDataRequest(
request.sensor_metadata().sensor_id(),
request.sensor_metadata().trajectory_id(), request.odometry_data(),
data_request.get());
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
->local_trajectory_uploader()
->EnqueueDataRequest(std::move(data_request));
}
} }
void OnReadsDone() override { void OnReadsDone() override {

View File

@ -43,7 +43,7 @@ class AddTrajectoryHandler
request.trajectory_builder_options(), request.trajectory_builder_options(),
local_slam_result_callback); local_slam_result_callback);
if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>() if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
->data_uploader()) { ->local_trajectory_uploader()) {
auto trajectory_builder_options = request.trajectory_builder_options(); auto trajectory_builder_options = request.trajectory_builder_options();
// Clear the trajectory builder options to convey to the cloud // Clear the trajectory builder options to convey to the cloud
@ -53,7 +53,7 @@ class AddTrajectoryHandler
trajectory_builder_options.clear_trajectory_builder_3d_options(); trajectory_builder_options.clear_trajectory_builder_3d_options();
GetContext<MapBuilderServer::MapBuilderContext>() GetContext<MapBuilderServer::MapBuilderContext>()
->data_uploader() ->local_trajectory_uploader()
->AddTrajectory(trajectory_id, expected_sensor_ids, ->AddTrajectory(trajectory_id, expected_sensor_ids,
trajectory_builder_options); trajectory_builder_options);
} }

View File

@ -37,9 +37,9 @@ class FinishTrajectoryHandler
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>() GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
->NotifyFinishTrajectory(request.trajectory_id()); ->NotifyFinishTrajectory(request.trajectory_id());
if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>() if (GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
->data_uploader()) { ->local_trajectory_uploader()) {
GetContext<MapBuilderServer::MapBuilderContext>() GetContext<MapBuilderServer::MapBuilderContext>()
->data_uploader() ->local_trajectory_uploader()
->FinishTrajectory(request.trajectory_id()); ->FinishTrajectory(request.trajectory_id());
} }
Send(cartographer::common::make_unique<google::protobuf::Empty>()); Send(cartographer::common::make_unique<google::protobuf::Empty>());

View File

@ -14,18 +14,19 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer_grpc/data_uploader.h" #include "cartographer_grpc/local_trajectory_uploader.h"
#include "glog/logging.h" #include "glog/logging.h"
namespace cartographer_grpc { namespace cartographer_grpc {
DataUploader::DataUploader(const std::string& server_address) LocalTrajectoryUploader::LocalTrajectoryUploader(
const std::string& server_address)
: client_channel_(grpc::CreateChannel(server_address, : client_channel_(grpc::CreateChannel(server_address,
grpc::InsecureChannelCredentials())), grpc::InsecureChannelCredentials())),
service_stub_(proto::MapBuilderService::NewStub(client_channel_)) {} service_stub_(proto::MapBuilderService::NewStub(client_channel_)) {}
void DataUploader::AddTrajectory( void LocalTrajectoryUploader::AddTrajectory(
int local_trajectory_id, int local_trajectory_id,
const std::unordered_set<std::string>& expected_sensor_ids, const std::unordered_set<std::string>& expected_sensor_ids,
const cartographer::mapping::proto::TrajectoryBuilderOptions& const cartographer::mapping::proto::TrajectoryBuilderOptions&
@ -45,7 +46,7 @@ void DataUploader::AddTrajectory(
result.trajectory_id(); result.trajectory_id();
} }
void DataUploader::FinishTrajectory(int local_trajectory_id) { void LocalTrajectoryUploader::FinishTrajectory(int local_trajectory_id) {
CHECK_EQ(local_to_cloud_trajectory_id_map_.count(local_trajectory_id), 1); CHECK_EQ(local_to_cloud_trajectory_id_map_.count(local_trajectory_id), 1);
int cloud_trajectory_id = int cloud_trajectory_id =
local_to_cloud_trajectory_id_map_[local_trajectory_id]; local_to_cloud_trajectory_id_map_[local_trajectory_id];
@ -58,4 +59,9 @@ void DataUploader::FinishTrajectory(int local_trajectory_id) {
CHECK(status.ok()); CHECK(status.ok());
} }
void LocalTrajectoryUploader::EnqueueDataRequest(
std::unique_ptr<google::protobuf::Message> data_request) {
send_queue_.Push(std::move(data_request));
}
} // namespace cartographer_grpc } // namespace cartographer_grpc

View File

@ -14,35 +14,41 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_GRPC_DATA_UPLOADER_H #ifndef CARTOGRAPHER_GRPC_LOCAL_TRAJECTORY_UPLOADER_H
#define CARTOGRAPHER_GRPC_DATA_UPLOADER_H #define CARTOGRAPHER_GRPC_LOCAL_TRAJECTORY_UPLOADER_H
#include <map> #include <map>
#include <string> #include <string>
#include <unordered_set> #include <unordered_set>
#include "cartographer/common/blocking_queue.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h" #include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
#include "grpc++/grpc++.h" #include "grpc++/grpc++.h"
namespace cartographer_grpc { namespace cartographer_grpc {
class DataUploader { class LocalTrajectoryUploader {
public: public:
DataUploader(const std::string& server_address); LocalTrajectoryUploader(const std::string& server_address);
void AddTrajectory( void AddTrajectory(
int local_trajectory_id, int local_trajectory_id,
const std::unordered_set<std::string>& expected_sensor_ids, const std::unordered_set<std::string>& expected_sensor_ids,
const cartographer::mapping::proto::TrajectoryBuilderOptions& const cartographer::mapping::proto::TrajectoryBuilderOptions&
trajectory_options); trajectory_options);
void FinishTrajectory(int local_trajectory_id); void FinishTrajectory(int local_trajectory_id);
void EnqueueDataRequest(
std::unique_ptr<google::protobuf::Message> data_request);
private: private:
std::shared_ptr<grpc::Channel> client_channel_; std::shared_ptr<grpc::Channel> client_channel_;
std::unique_ptr<proto::MapBuilderService::Stub> service_stub_; std::unique_ptr<proto::MapBuilderService::Stub> service_stub_;
std::map<int, int> local_to_cloud_trajectory_id_map_; std::map<int, int> local_to_cloud_trajectory_id_map_;
cartographer::common::BlockingQueue<
std::unique_ptr<google::protobuf::Message>>
send_queue_;
}; };
} // namespace cartographer_grpc } // namespace cartographer_grpc
#endif // CARTOGRAPHER_GRPC_DATA_UPLOADER_H #endif // CARTOGRAPHER_GRPC_LOCAL_TRAJECTORY_UPLOADER_H

View File

@ -208,7 +208,8 @@ MapBuilderServer::MapBuilderServer(
server_builder.SetNumEventThreads( server_builder.SetNumEventThreads(
map_builder_server_options.num_event_threads()); map_builder_server_options.num_event_threads());
if (!map_builder_server_options.uplink_server_address().empty()) { if (!map_builder_server_options.uplink_server_address().empty()) {
data_uploader_ = cartographer::common::make_unique<DataUploader>( local_trajectory_uploader_ =
cartographer::common::make_unique<LocalTrajectoryUploader>(
map_builder_server_options.uplink_server_address()); map_builder_server_options.uplink_server_address());
} }
server_builder.RegisterHandler<handlers::AddTrajectoryHandler, server_builder.RegisterHandler<handlers::AddTrajectoryHandler,

View File

@ -23,9 +23,9 @@
#include "cartographer/mapping/map_builder.h" #include "cartographer/mapping/map_builder.h"
#include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer/sensor/dispatchable.h" #include "cartographer/sensor/dispatchable.h"
#include "cartographer_grpc/data_uploader.h"
#include "cartographer_grpc/framework/execution_context.h" #include "cartographer_grpc/framework/execution_context.h"
#include "cartographer_grpc/framework/server.h" #include "cartographer_grpc/framework/server.h"
#include "cartographer_grpc/local_trajectory_uploader.h"
#include "cartographer_grpc/proto/map_builder_server_options.pb.h" #include "cartographer_grpc/proto/map_builder_server_options.pb.h"
namespace cartographer_grpc { namespace cartographer_grpc {
@ -70,8 +70,8 @@ class MapBuilderServer {
ProcessLocalSlamResultData( ProcessLocalSlamResultData(
const std::string& sensor_id, cartographer::common::Time time, const std::string& sensor_id, cartographer::common::Time time,
const cartographer::mapping::proto::LocalSlamResultData& proto); const cartographer::mapping::proto::LocalSlamResultData& proto);
DataUploader* data_uploader() { LocalTrajectoryUploader* local_trajectory_uploader() {
return map_builder_server_->data_uploader_.get(); return map_builder_server_->local_trajectory_uploader_.get();
} }
template <typename DataType> template <typename DataType>
@ -154,7 +154,7 @@ class MapBuilderServer {
int current_subscription_index_ = 0; int current_subscription_index_ = 0;
std::map<int /* trajectory ID */, LocalSlamResultHandlerSubscriptions> std::map<int /* trajectory ID */, LocalSlamResultHandlerSubscriptions>
local_slam_subscriptions_ GUARDED_BY(local_slam_subscriptions_lock_); local_slam_subscriptions_ GUARDED_BY(local_slam_subscriptions_lock_);
std::unique_ptr<DataUploader> data_uploader_; std::unique_ptr<LocalTrajectoryUploader> local_trajectory_uploader_;
}; };
} // namespace cartographer_grpc } // namespace cartographer_grpc

View File

@ -18,6 +18,7 @@
#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/local_slam_result_data.h"
#include "cartographer_grpc/proto/map_builder_service.pb.h" #include "cartographer_grpc/proto/map_builder_service.pb.h"
#include "cartographer_grpc/sensor/serialization.h"
#include "glog/logging.h" #include "glog/logging.h"
namespace cartographer_grpc { namespace cartographer_grpc {
@ -75,9 +76,9 @@ void TrajectoryBuilderStub::AddSensorData(
CHECK(rangefinder_writer_.client_writer); CHECK(rangefinder_writer_.client_writer);
} }
proto::AddRangefinderDataRequest request; proto::AddRangefinderDataRequest request;
*request.mutable_sensor_metadata() = CreateSensorMetadata(sensor_id); CreateAddRangeFinderDataRequest(
*request.mutable_timed_point_cloud_data() = sensor_id, trajectory_id_,
cartographer::sensor::ToProto(timed_point_cloud_data); cartographer::sensor::ToProto(timed_point_cloud_data), &request);
rangefinder_writer_.client_writer->Write(request); rangefinder_writer_.client_writer->Write(request);
} }
@ -90,8 +91,8 @@ void TrajectoryBuilderStub::AddSensorData(
CHECK(imu_writer_.client_writer); CHECK(imu_writer_.client_writer);
} }
proto::AddImuDataRequest request; proto::AddImuDataRequest request;
*request.mutable_sensor_metadata() = CreateSensorMetadata(sensor_id); CreateAddImuDataRequest(sensor_id, trajectory_id_,
*request.mutable_imu_data() = cartographer::sensor::ToProto(imu_data); cartographer::sensor::ToProto(imu_data), &request);
imu_writer_.client_writer->Write(request); imu_writer_.client_writer->Write(request);
} }
@ -104,9 +105,9 @@ void TrajectoryBuilderStub::AddSensorData(
CHECK(odometry_writer_.client_writer); CHECK(odometry_writer_.client_writer);
} }
proto::AddOdometryDataRequest request; proto::AddOdometryDataRequest request;
*request.mutable_sensor_metadata() = CreateSensorMetadata(sensor_id); CreateAddOdometryDataRequest(sensor_id, trajectory_id_,
*request.mutable_odometry_data() = cartographer::sensor::ToProto(odometry_data),
cartographer::sensor::ToProto(odometry_data); &request);
odometry_writer_.client_writer->Write(request); odometry_writer_.client_writer->Write(request);
} }
@ -119,9 +120,9 @@ void TrajectoryBuilderStub::AddSensorData(
CHECK(fixed_frame_writer_.client_writer); CHECK(fixed_frame_writer_.client_writer);
} }
proto::AddFixedFramePoseDataRequest request; proto::AddFixedFramePoseDataRequest request;
*request.mutable_sensor_metadata() = CreateSensorMetadata(sensor_id); CreateAddFixedFramePoseDataRequest(
*request.mutable_fixed_frame_pose_data() = sensor_id, trajectory_id_,
cartographer::sensor::ToProto(fixed_frame_pose); cartographer::sensor::ToProto(fixed_frame_pose), &request);
fixed_frame_writer_.client_writer->Write(request); fixed_frame_writer_.client_writer->Write(request);
} }
@ -131,14 +132,6 @@ void TrajectoryBuilderStub::AddLocalSlamResultData(
LOG(FATAL) << "Not implemented"; LOG(FATAL) << "Not implemented";
} }
proto::SensorMetadata TrajectoryBuilderStub::CreateSensorMetadata(
const std::string& sensor_id) {
proto::SensorMetadata sensor_metadata;
sensor_metadata.set_sensor_id(sensor_id);
sensor_metadata.set_trajectory_id(trajectory_id_);
return sensor_metadata;
}
void TrajectoryBuilderStub::RunLocalSlamResultReader( void TrajectoryBuilderStub::RunLocalSlamResultReader(
grpc::ClientReader<proto::ReceiveLocalSlamResultsResponse>* client_reader, grpc::ClientReader<proto::ReceiveLocalSlamResultsResponse>* client_reader,
LocalSlamResultCallback local_slam_result_callback) { LocalSlamResultCallback local_slam_result_callback) {

View File

@ -66,7 +66,6 @@ class TrajectoryBuilderStub
std::unique_ptr<std::thread> thread; std::unique_ptr<std::thread> thread;
}; };
proto::SensorMetadata CreateSensorMetadata(const std::string& sensor_id);
static void RunLocalSlamResultReader( static void RunLocalSlamResultReader(
grpc::ClientReader<proto::ReceiveLocalSlamResultsResponse>* client_reader, grpc::ClientReader<proto::ReceiveLocalSlamResultsResponse>* client_reader,
LocalSlamResultCallback local_slam_result_callback); LocalSlamResultCallback local_slam_result_callback);

View File

@ -0,0 +1,67 @@
/*
* 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 "serialization.h"
namespace cartographer_grpc {
namespace mapping {
void CreateSensorMetadata(const std::string& sensor_id, const int trajectory_id,
proto::SensorMetadata* proto) {
proto->set_sensor_id(sensor_id);
proto->set_trajectory_id(trajectory_id);
}
void CreateAddFixedFramePoseDataRequest(
const std::string& sensor_id, int trajectory_id,
const cartographer::sensor::proto::FixedFramePoseData&
fixed_frame_pose_data,
proto::AddFixedFramePoseDataRequest* proto) {
CreateSensorMetadata(sensor_id, trajectory_id,
proto->mutable_sensor_metadata());
*proto->mutable_fixed_frame_pose_data() = fixed_frame_pose_data;
}
void CreateAddImuDataRequest(
const std::string& sensor_id, const int trajectory_id,
const cartographer::sensor::proto::ImuData& imu_data,
proto::AddImuDataRequest* proto) {
CreateSensorMetadata(sensor_id, trajectory_id,
proto->mutable_sensor_metadata());
*proto->mutable_imu_data() = imu_data;
}
void CreateAddOdometryDataRequest(
const std::string& sensor_id, int trajectory_id,
const cartographer::sensor::proto::OdometryData& odometry_data,
proto::AddOdometryDataRequest* proto) {
CreateSensorMetadata(sensor_id, trajectory_id,
proto->mutable_sensor_metadata());
*proto->mutable_odometry_data() = odometry_data;
}
void CreateAddRangeFinderDataRequest(
const std::string& sensor_id, int trajectory_id,
const cartographer::sensor::proto::TimedPointCloudData&
timed_point_cloud_data,
proto::AddRangefinderDataRequest* proto) {
CreateSensorMetadata(sensor_id, trajectory_id,
proto->mutable_sensor_metadata());
*proto->mutable_timed_point_cloud_data() = timed_point_cloud_data;
}
} // namespace mapping
} // namespace cartographer_grpc

View File

@ -0,0 +1,53 @@
/*
* 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.
*/
#ifndef CARTOGRAPHER_GRPC_SENSOR_SERIALIZATION_H
#define CARTOGRAPHER_GRPC_SENSOR_SERIALIZATION_H
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/timed_point_cloud_data.h"
#include "cartographer_grpc/proto/map_builder_service.pb.h"
namespace cartographer_grpc {
namespace mapping {
void CreateSensorMetadata(const std::string& sensor_id, int trajectory_id,
proto::SensorMetadata* proto);
void CreateAddFixedFramePoseDataRequest(
const std::string& sensor_id, int trajectory_id,
const cartographer::sensor::proto::FixedFramePoseData&
fixed_frame_pose_data,
proto::AddFixedFramePoseDataRequest* proto);
void CreateAddImuDataRequest(
const std::string& sensor_id, int trajectory_id,
const cartographer::sensor::proto::ImuData& imu_data,
proto::AddImuDataRequest* proto);
void CreateAddOdometryDataRequest(
const std::string& sensor_id, int trajectory_id,
const cartographer::sensor::proto::OdometryData& odometry_data,
proto::AddOdometryDataRequest* proto);
void CreateAddRangeFinderDataRequest(
const std::string& sensor_id, int trajectory_id,
const cartographer::sensor::proto::TimedPointCloudData&
timed_point_cloud_data,
proto::AddRangefinderDataRequest* proto);
} // namespace mapping
} // namespace cartographer_grpc
#endif // CARTOGRAPHER_GRPC_SENSOR_SERIALIZATION_H