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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h
View File

@ -21,6 +21,7 @@
#include "cartographer_grpc/framework/rpc_handler.h"
#include "cartographer_grpc/map_builder_server.h"
#include "cartographer_grpc/proto/map_builder_service.pb.h"
#include "cartographer_grpc/sensor/serialization.h"
#include "google/protobuf/empty.pb.h"
namespace cartographer_grpc {
@ -40,6 +41,22 @@ class AddFixedFramePoseDataHandler
request.sensor_metadata().trajectory_id(),
request.sensor_metadata().sensor_id(),
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 {

17
cartographer_grpc/handlers/add_imu_data_handler.h
View File

@ -21,6 +21,7 @@
#include "cartographer_grpc/framework/rpc_handler.h"
#include "cartographer_grpc/map_builder_server.h"
#include "cartographer_grpc/proto/map_builder_service.pb.h"
#include "cartographer_grpc/sensor/serialization.h"
#include "google/protobuf/empty.pb.h"
namespace cartographer_grpc {
@ -39,6 +40,22 @@ class AddImuDataHandler
request.sensor_metadata().trajectory_id(),
request.sensor_metadata().sensor_id(),
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 {

16
cartographer_grpc/handlers/add_odometry_data_handler.h
View File

@ -40,6 +40,22 @@ class AddOdometryDataHandler
request.sensor_metadata().trajectory_id(),
request.sensor_metadata().sensor_id(),
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 {

4
cartographer_grpc/handlers/add_trajectory_handler.h
View File

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

4
cartographer_grpc/handlers/finish_trajectory_handler.h
View File

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

14
cartographer_grpc/data_uploader.cc → cartographer_grpc/local_trajectory_uploader.cc
View File

@ -14,18 +14,19 @@
* limitations under the License.
*/
#include "cartographer_grpc/data_uploader.h"
#include "cartographer_grpc/local_trajectory_uploader.h"
#include "glog/logging.h"
namespace cartographer_grpc {
DataUploader::DataUploader(const std::string& server_address)
LocalTrajectoryUploader::LocalTrajectoryUploader(
const std::string& server_address)
: client_channel_(grpc::CreateChannel(server_address,
grpc::InsecureChannelCredentials())),
service_stub_(proto::MapBuilderService::NewStub(client_channel_)) {}
void DataUploader::AddTrajectory(
void LocalTrajectoryUploader::AddTrajectory(
int local_trajectory_id,
const std::unordered_set<std::string>& expected_sensor_ids,
const cartographer::mapping::proto::TrajectoryBuilderOptions&
@ -45,7 +46,7 @@ void DataUploader::AddTrajectory(
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);
int cloud_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());
}
void LocalTrajectoryUploader::EnqueueDataRequest(
std::unique_ptr<google::protobuf::Message> data_request) {
send_queue_.Push(std::move(data_request));
}
} // namespace cartographer_grpc

16
cartographer_grpc/data_uploader.h → cartographer_grpc/local_trajectory_uploader.h
View File

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

3
cartographer_grpc/map_builder_server.cc
View File

@ -208,7 +208,8 @@ MapBuilderServer::MapBuilderServer(
server_builder.SetNumEventThreads(
map_builder_server_options.num_event_threads());
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());
}
server_builder.RegisterHandler<handlers::AddTrajectoryHandler,

8
cartographer_grpc/map_builder_server.h
View File

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

31
cartographer_grpc/mapping/trajectory_builder_stub.cc
View File

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

1
cartographer_grpc/mapping/trajectory_builder_stub.h
View File

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

67
cartographer_grpc/sensor/serialization.cc Normal file
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

53
cartographer_grpc/sensor/serialization.h Normal file
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