From d49706944ffcfa2e7d91137475465f067006a4e4 Mon Sep 17 00:00:00 2001 From: gaschler Date: Fri, 15 Dec 2017 15:26:58 +0100 Subject: [PATCH] FixedFramePoseData in gRPC server (#761) Defines the FixedFramePoseData RPC and implements the handler for the gRPC server. --- cartographer/mapping/map_builder_test.cc | 4 +- .../add_fixed_frame_pose_data_handler.h | 53 +++++++++++++++++++ cartographer_grpc/map_builder_server.cc | 4 ++ .../proto/map_builder_service.proto | 15 ++++-- 4 files changed, 70 insertions(+), 6 deletions(-) create mode 100644 cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index 5525f2e..c8c5cf3 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/cartographer/mapping/map_builder_test.cc @@ -33,8 +33,8 @@ namespace { constexpr char kRangeSensorId[] = "range"; constexpr char kIMUSensorId[] = "imu"; -constexpr double kDuration = 4.; // Seconds. -constexpr double kTimeStep = 0.1; // Seconds. +constexpr double kDuration = 4.; // Seconds. +constexpr double kTimeStep = 0.1; // Seconds. constexpr double kTravelDistance = 1.2; // Meters. std::vector GenerateFakeRangeMeasurements() { diff --git a/cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h b/cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h new file mode 100644 index 0000000..696959b --- /dev/null +++ b/cartographer_grpc/handlers/add_fixed_frame_pose_data_handler.h @@ -0,0 +1,53 @@ +/* + * Copyright 2017 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_FIXED_FRAME_POSE_DATA_HANDLER_H +#define CARTOGRAPHER_GRPC_HANDLERS_ADD_FIXED_FRAME_POSE_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 AddFixedFramePoseDataHandler + : public framework::RpcHandler< + framework::Stream, + google::protobuf::Empty> { + public: + void OnRequest(const proto::AddFixedFramePoseDataRequest &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() + ->EnqueueSensorData( + request.sensor_metadata().trajectory_id(), + request.sensor_metadata().sensor_id(), + cartographer::sensor::FromProto(request.fixed_frame_pose_data())); + } + + void OnReadsDone() { + Send(cartographer::common::make_unique()); + } +}; + +} // namespace handlers +} // namespace cartographer_grpc + +#endif // CARTOGRAPHER_GRPC_HANDLERS_ADD_FIXED_FRAME_POSE_DATA_HANDLER_H diff --git a/cartographer_grpc/map_builder_server.cc b/cartographer_grpc/map_builder_server.cc index d4d1789..701b7e1 100644 --- a/cartographer_grpc/map_builder_server.cc +++ b/cartographer_grpc/map_builder_server.cc @@ -16,6 +16,7 @@ #include "cartographer_grpc/map_builder_server.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_odometry_data_handler.h" #include "cartographer_grpc/handlers/add_rangefinder_data_handler.h" @@ -66,6 +67,9 @@ MapBuilderServer::MapBuilderServer( server_builder.RegisterHandler( "AddRangefinderData"); + server_builder.RegisterHandler( + "AddFixedFramePoseData"); server_builder.RegisterHandler("FinishTrajectory"); grpc_server_ = server_builder.Build(); diff --git a/cartographer_grpc/proto/map_builder_service.proto b/cartographer_grpc/proto/map_builder_service.proto index b78ddd6..48a72b9 100644 --- a/cartographer_grpc/proto/map_builder_service.proto +++ b/cartographer_grpc/proto/map_builder_service.proto @@ -50,6 +50,11 @@ message AddRangefinderDataRequest { cartographer.sensor.proto.TimedPointCloudData timed_point_cloud_data = 2; } +message AddFixedFramePoseDataRequest { + SensorMetadata sensor_metadata = 1; + cartographer.sensor.proto.FixedFramePoseData fixed_frame_pose_data = 2; +} + message FinishTrajectoryRequest { int32 trajectory_id = 1; } @@ -63,15 +68,17 @@ service MapBuilderService { rpc AddOdometryData(stream AddOdometryDataRequest) returns (google.protobuf.Empty); - // Adds IMU data from the sensor with id 'sensor_metadata.sensor_id' to - // the trajectory corresponding to 'sensor_metadata.trajectory_id'. + // Same for IMU data. rpc AddImuData(stream AddImuDataRequest) returns (google.protobuf.Empty); - // Adds range-finder data from the sensor with id 'sensor_metadata.sensor_id' - // to the trajectory corresponding to 'sensor_metadata.trajectory_id'. + // Same for range-finder data. rpc AddRangefinderData(stream AddRangefinderDataRequest) returns (google.protobuf.Empty); + // Same for fixed-frame pose data. + rpc AddFixedFramePoseData(stream AddFixedFramePoseDataRequest) + returns (google.protobuf.Empty); + // Marks a trajectory corresponding to 'trajectory_id' as finished, // i.e. no further sensor data is expected. rpc FinishTrajectory(FinishTrajectoryRequest) returns (google.protobuf.Empty);