Send LandmarkData via gRPC. (#831)
* Send LandmarkData via gRPC. [RFC PR](https://github.com/googlecartographer/rfcs/pull/18) * Fix the nits.master
parent
746c9c83c8
commit
9e30c1e0cd
|
@ -0,0 +1,69 @@
|
||||||
|
/*
|
||||||
|
* 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_HANDLERS_ADD_LANDMARK_DATA_HANDLER_H
|
||||||
|
#define CARTOGRAPHER_GRPC_HANDLERS_ADD_LANDMARK_DATA_HANDLER_H
|
||||||
|
|
||||||
|
#include "cartographer/common/make_unique.h"
|
||||||
|
#include "cartographer_grpc/framework/rpc_handler.h"
|
||||||
|
#include "cartographer_grpc/map_builder_server.h"
|
||||||
|
#include "cartographer_grpc/proto/map_builder_service.pb.h"
|
||||||
|
#include "google/protobuf/empty.pb.h"
|
||||||
|
|
||||||
|
namespace cartographer_grpc {
|
||||||
|
namespace handlers {
|
||||||
|
|
||||||
|
class AddLandmarkDataHandler
|
||||||
|
: public framework::RpcHandler<
|
||||||
|
framework::Stream<proto::AddLandmarkDataRequest>,
|
||||||
|
google::protobuf::Empty> {
|
||||||
|
public:
|
||||||
|
void OnRequest(const proto::AddLandmarkDataRequest &request) override {
|
||||||
|
// The 'BlockingQueue' returned by 'sensor_data_queue()' is already
|
||||||
|
// thread-safe. Therefore it suffices to get an unsynchronized reference to
|
||||||
|
// the 'MapBuilderContext'.
|
||||||
|
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
|
||||||
|
->EnqueueSensorData(
|
||||||
|
request.sensor_metadata().trajectory_id(),
|
||||||
|
request.sensor_metadata().sensor_id(),
|
||||||
|
cartographer::sensor::FromProto(request.landmark_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::AddLandmarkDataRequest>();
|
||||||
|
sensor::CreateAddLandmarkDataRequest(
|
||||||
|
request.sensor_metadata().sensor_id(),
|
||||||
|
request.sensor_metadata().trajectory_id(), request.landmark_data(),
|
||||||
|
data_request.get());
|
||||||
|
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
|
||||||
|
->local_trajectory_uploader()
|
||||||
|
->EnqueueDataRequest(std::move(data_request));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void OnReadsDone() override {
|
||||||
|
Send(cartographer::common::make_unique<google::protobuf::Empty>());
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace handlers
|
||||||
|
} // namespace cartographer_grpc
|
||||||
|
|
||||||
|
#endif // CARTOGRAPHER_GRPC_HANDLERS_ADD_LANDMARK_DATA_HANDLER_H
|
|
@ -18,6 +18,7 @@
|
||||||
|
|
||||||
#include "cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h"
|
#include "cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h"
|
||||||
#include "cartographer_grpc/handlers/add_imu_data_handler.h"
|
#include "cartographer_grpc/handlers/add_imu_data_handler.h"
|
||||||
|
#include "cartographer_grpc/handlers/add_landmark_data_handler.h"
|
||||||
#include "cartographer_grpc/handlers/add_local_slam_result_data_handler.h"
|
#include "cartographer_grpc/handlers/add_local_slam_result_data_handler.h"
|
||||||
#include "cartographer_grpc/handlers/add_odometry_data_handler.h"
|
#include "cartographer_grpc/handlers/add_odometry_data_handler.h"
|
||||||
#include "cartographer_grpc/handlers/add_rangefinder_data_handler.h"
|
#include "cartographer_grpc/handlers/add_rangefinder_data_handler.h"
|
||||||
|
@ -225,6 +226,8 @@ MapBuilderServer::MapBuilderServer(
|
||||||
server_builder.RegisterHandler<handlers::AddFixedFramePoseDataHandler,
|
server_builder.RegisterHandler<handlers::AddFixedFramePoseDataHandler,
|
||||||
proto::MapBuilderService>(
|
proto::MapBuilderService>(
|
||||||
"AddFixedFramePoseData");
|
"AddFixedFramePoseData");
|
||||||
|
server_builder.RegisterHandler<handlers::AddLandmarkDataHandler,
|
||||||
|
proto::MapBuilderService>("AddLandmarkData");
|
||||||
server_builder.RegisterHandler<handlers::AddLocalSlamResultDataHandler,
|
server_builder.RegisterHandler<handlers::AddLocalSlamResultDataHandler,
|
||||||
proto::MapBuilderService>(
|
proto::MapBuilderService>(
|
||||||
"AddLocalSlamResultData");
|
"AddLocalSlamResultData");
|
||||||
|
|
|
@ -130,7 +130,16 @@ void TrajectoryBuilderStub::AddSensorData(
|
||||||
void TrajectoryBuilderStub::AddSensorData(
|
void TrajectoryBuilderStub::AddSensorData(
|
||||||
const std::string& sensor_id,
|
const std::string& sensor_id,
|
||||||
const cartographer::sensor::LandmarkData& landmark_data) {
|
const cartographer::sensor::LandmarkData& landmark_data) {
|
||||||
LOG(FATAL) << "Not implemented";
|
if (!landmark_writer_.client_writer) {
|
||||||
|
landmark_writer_.client_writer = stub_->AddLandmarkData(
|
||||||
|
&landmark_writer_.client_context, &landmark_writer_.response);
|
||||||
|
CHECK(landmark_writer_.client_writer);
|
||||||
|
}
|
||||||
|
proto::AddLandmarkDataRequest request;
|
||||||
|
sensor::CreateAddLandmarkDataRequest(
|
||||||
|
sensor_id, trajectory_id_, cartographer::sensor::ToProto(landmark_data),
|
||||||
|
&request);
|
||||||
|
landmark_writer_.client_writer->Write(request);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TrajectoryBuilderStub::AddLocalSlamResultData(
|
void TrajectoryBuilderStub::AddLocalSlamResultData(
|
||||||
|
|
|
@ -76,6 +76,7 @@ class TrajectoryBuilderStub
|
||||||
framework::ClientWriter<proto::AddOdometryDataRequest> odometry_writer_;
|
framework::ClientWriter<proto::AddOdometryDataRequest> odometry_writer_;
|
||||||
framework::ClientWriter<proto::AddFixedFramePoseDataRequest>
|
framework::ClientWriter<proto::AddFixedFramePoseDataRequest>
|
||||||
fixed_frame_writer_;
|
fixed_frame_writer_;
|
||||||
|
framework::ClientWriter<proto::AddLandmarkDataRequest> landmark_writer_;
|
||||||
LocalSlamResultReader local_slam_result_reader_;
|
LocalSlamResultReader local_slam_result_reader_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -59,6 +59,11 @@ message AddFixedFramePoseDataRequest {
|
||||||
cartographer.sensor.proto.FixedFramePoseData fixed_frame_pose_data = 2;
|
cartographer.sensor.proto.FixedFramePoseData fixed_frame_pose_data = 2;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
message AddLandmarkDataRequest {
|
||||||
|
SensorMetadata sensor_metadata = 1;
|
||||||
|
cartographer.sensor.proto.LandmarkData landmark_data = 2;
|
||||||
|
}
|
||||||
|
|
||||||
message FinishTrajectoryRequest {
|
message FinishTrajectoryRequest {
|
||||||
int32 trajectory_id = 1;
|
int32 trajectory_id = 1;
|
||||||
}
|
}
|
||||||
|
@ -145,6 +150,10 @@ service MapBuilderService {
|
||||||
rpc AddFixedFramePoseData(stream AddFixedFramePoseDataRequest)
|
rpc AddFixedFramePoseData(stream AddFixedFramePoseDataRequest)
|
||||||
returns (google.protobuf.Empty);
|
returns (google.protobuf.Empty);
|
||||||
|
|
||||||
|
// Same for landmark data.
|
||||||
|
rpc AddLandmarkData(stream AddLandmarkDataRequest)
|
||||||
|
returns (google.protobuf.Empty);
|
||||||
|
|
||||||
// Adds a local SLAM result.
|
// Adds a local SLAM result.
|
||||||
rpc AddLocalSlamResultData(stream AddLocalSlamResultDataRequest)
|
rpc AddLocalSlamResultData(stream AddLocalSlamResultDataRequest)
|
||||||
returns (google.protobuf.Empty);
|
returns (google.protobuf.Empty);
|
||||||
|
|
|
@ -63,5 +63,14 @@ void CreateAddRangeFinderDataRequest(
|
||||||
*proto->mutable_timed_point_cloud_data() = timed_point_cloud_data;
|
*proto->mutable_timed_point_cloud_data() = timed_point_cloud_data;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void CreateAddLandmarkDataRequest(
|
||||||
|
const std::string& sensor_id, int trajectory_id,
|
||||||
|
const cartographer::sensor::proto::LandmarkData& landmark_data,
|
||||||
|
proto::AddLandmarkDataRequest* proto) {
|
||||||
|
CreateSensorMetadata(sensor_id, trajectory_id,
|
||||||
|
proto->mutable_sensor_metadata());
|
||||||
|
*proto->mutable_landmark_data() = landmark_data;
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace sensor
|
} // namespace sensor
|
||||||
} // namespace cartographer_grpc
|
} // namespace cartographer_grpc
|
||||||
|
|
|
@ -19,6 +19,7 @@
|
||||||
|
|
||||||
#include "cartographer/sensor/fixed_frame_pose_data.h"
|
#include "cartographer/sensor/fixed_frame_pose_data.h"
|
||||||
#include "cartographer/sensor/imu_data.h"
|
#include "cartographer/sensor/imu_data.h"
|
||||||
|
#include "cartographer/sensor/landmark_data.h"
|
||||||
#include "cartographer/sensor/odometry_data.h"
|
#include "cartographer/sensor/odometry_data.h"
|
||||||
#include "cartographer/sensor/timed_point_cloud_data.h"
|
#include "cartographer/sensor/timed_point_cloud_data.h"
|
||||||
#include "cartographer_grpc/proto/map_builder_service.pb.h"
|
#include "cartographer_grpc/proto/map_builder_service.pb.h"
|
||||||
|
@ -28,6 +29,7 @@ namespace sensor {
|
||||||
|
|
||||||
void CreateSensorMetadata(const std::string& sensor_id, int trajectory_id,
|
void CreateSensorMetadata(const std::string& sensor_id, int trajectory_id,
|
||||||
proto::SensorMetadata* proto);
|
proto::SensorMetadata* proto);
|
||||||
|
|
||||||
void CreateAddFixedFramePoseDataRequest(
|
void CreateAddFixedFramePoseDataRequest(
|
||||||
const std::string& sensor_id, int trajectory_id,
|
const std::string& sensor_id, int trajectory_id,
|
||||||
const cartographer::sensor::proto::FixedFramePoseData&
|
const cartographer::sensor::proto::FixedFramePoseData&
|
||||||
|
@ -46,6 +48,10 @@ void CreateAddRangeFinderDataRequest(
|
||||||
const cartographer::sensor::proto::TimedPointCloudData&
|
const cartographer::sensor::proto::TimedPointCloudData&
|
||||||
timed_point_cloud_data,
|
timed_point_cloud_data,
|
||||||
proto::AddRangefinderDataRequest* proto);
|
proto::AddRangefinderDataRequest* proto);
|
||||||
|
void CreateAddLandmarkDataRequest(
|
||||||
|
const std::string& sensor_id, int trajectory_id,
|
||||||
|
const cartographer::sensor::proto::LandmarkData& landmark_data,
|
||||||
|
proto::AddLandmarkDataRequest* proto);
|
||||||
|
|
||||||
} // namespace sensor
|
} // namespace sensor
|
||||||
} // namespace cartographer_grpc
|
} // namespace cartographer_grpc
|
||||||
|
|
Loading…
Reference in New Issue