Implement ReceiveLocalSlamResultsHandler. (#772)
parent
5fbc4ca568
commit
02734c296d
|
@ -33,6 +33,13 @@ message TimedPointCloudData {
|
||||||
repeated transform.proto.Vector4f point_data = 3;
|
repeated transform.proto.Vector4f point_data = 3;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Proto representation of ::cartographer::sensor::RangeData.
|
||||||
|
message RangeData {
|
||||||
|
transform.proto.Vector3f origin = 1;
|
||||||
|
repeated transform.proto.Vector3f returns = 2;
|
||||||
|
repeated transform.proto.Vector3f misses = 3;
|
||||||
|
}
|
||||||
|
|
||||||
// Proto representation of ::cartographer::sensor::ImuData.
|
// Proto representation of ::cartographer::sensor::ImuData.
|
||||||
message ImuData {
|
message ImuData {
|
||||||
int64 timestamp = 1;
|
int64 timestamp = 1;
|
||||||
|
|
|
@ -54,5 +54,36 @@ TimedRangeData CropTimedRangeData(const TimedRangeData& range_data,
|
||||||
CropTimedPointCloud(range_data.misses, min_z, max_z)};
|
CropTimedPointCloud(range_data.misses, min_z, max_z)};
|
||||||
}
|
}
|
||||||
|
|
||||||
|
proto::RangeData ToProto(const RangeData& range_data) {
|
||||||
|
proto::RangeData proto;
|
||||||
|
*proto.mutable_origin() = transform::ToProto(range_data.origin);
|
||||||
|
proto.mutable_returns()->Reserve(range_data.returns.size());
|
||||||
|
for (const auto& point : range_data.returns) {
|
||||||
|
*proto.add_returns() = transform::ToProto(point);
|
||||||
|
}
|
||||||
|
proto.mutable_misses()->Reserve(range_data.misses.size());
|
||||||
|
for (const auto& point : range_data.misses) {
|
||||||
|
*proto.add_misses() = transform::ToProto(point);
|
||||||
|
}
|
||||||
|
return proto;
|
||||||
|
}
|
||||||
|
|
||||||
|
RangeData FromProto(const proto::RangeData& proto) {
|
||||||
|
PointCloud returns;
|
||||||
|
returns.reserve(proto.returns().size());
|
||||||
|
std::transform(
|
||||||
|
proto.returns().begin(), proto.returns().end(),
|
||||||
|
std::back_inserter(returns),
|
||||||
|
static_cast<Eigen::Vector3f (*)(const transform::proto::Vector3f&)>(
|
||||||
|
transform::ToEigen));
|
||||||
|
PointCloud misses;
|
||||||
|
misses.reserve(proto.misses().size());
|
||||||
|
std::transform(
|
||||||
|
proto.misses().begin(), proto.misses().end(), std::back_inserter(misses),
|
||||||
|
static_cast<Eigen::Vector3f (*)(const transform::proto::Vector3f&)>(
|
||||||
|
transform::ToEigen));
|
||||||
|
return RangeData{transform::ToEigen(proto.origin()), returns, misses};
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace sensor
|
} // namespace sensor
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
|
@ -55,6 +55,12 @@ RangeData CropRangeData(const RangeData& range_data, float min_z, float max_z);
|
||||||
TimedRangeData CropTimedRangeData(const TimedRangeData& range_data, float min_z,
|
TimedRangeData CropTimedRangeData(const TimedRangeData& range_data, float min_z,
|
||||||
float max_z);
|
float max_z);
|
||||||
|
|
||||||
|
// Converts 'range_data' to a proto::RangeData.
|
||||||
|
proto::RangeData ToProto(const RangeData& range_data);
|
||||||
|
|
||||||
|
// Converts 'proto' to RangeData.
|
||||||
|
RangeData FromProto(const proto::RangeData& proto);
|
||||||
|
|
||||||
} // namespace sensor
|
} // namespace sensor
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
||||||
|
|
|
@ -74,6 +74,8 @@ void Rpc::OnRequest() { handler_->OnRequestInternal(request_.get()); }
|
||||||
|
|
||||||
void Rpc::OnReadsDone() { handler_->OnReadsDone(); }
|
void Rpc::OnReadsDone() { handler_->OnReadsDone(); }
|
||||||
|
|
||||||
|
void Rpc::OnFinish() { handler_->OnFinish(); }
|
||||||
|
|
||||||
void Rpc::RequestNextMethodInvocation() {
|
void Rpc::RequestNextMethodInvocation() {
|
||||||
// Ask gRPC to notify us when the connection terminates.
|
// Ask gRPC to notify us when the connection terminates.
|
||||||
SetRpcEventState(Event::DONE, true);
|
SetRpcEventState(Event::DONE, true);
|
||||||
|
|
|
@ -62,6 +62,7 @@ class Rpc {
|
||||||
std::unique_ptr<Rpc> Clone();
|
std::unique_ptr<Rpc> Clone();
|
||||||
void OnRequest();
|
void OnRequest();
|
||||||
void OnReadsDone();
|
void OnReadsDone();
|
||||||
|
void OnFinish();
|
||||||
void RequestNextMethodInvocation();
|
void RequestNextMethodInvocation();
|
||||||
void RequestStreamingReadIfNeeded();
|
void RequestStreamingReadIfNeeded();
|
||||||
void HandleSendQueue();
|
void HandleSendQueue();
|
||||||
|
|
|
@ -33,6 +33,7 @@ class RpcHandlerInterface {
|
||||||
virtual void OnRequestInternal(
|
virtual void OnRequestInternal(
|
||||||
const ::google::protobuf::Message* request) = 0;
|
const ::google::protobuf::Message* request) = 0;
|
||||||
virtual void OnReadsDone(){};
|
virtual void OnReadsDone(){};
|
||||||
|
virtual void OnFinish(){};
|
||||||
};
|
};
|
||||||
|
|
||||||
using RpcHandlerFactory = std::function<std::unique_ptr<RpcHandlerInterface>(
|
using RpcHandlerFactory = std::function<std::unique_ptr<RpcHandlerInterface>(
|
||||||
|
|
|
@ -134,6 +134,8 @@ void Service::HandleFinish(Rpc* rpc, bool ok) {
|
||||||
LOG(ERROR) << "Finish failed";
|
LOG(ERROR) << "Finish failed";
|
||||||
}
|
}
|
||||||
|
|
||||||
|
rpc->OnFinish();
|
||||||
|
|
||||||
RemoveIfNotPending(rpc);
|
RemoveIfNotPending(rpc);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1,92 @@
|
||||||
|
/*
|
||||||
|
* 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_RECEIVE_LOCAL_SLAM_RESULTS_HANDLER_H
|
||||||
|
#define CARTOGRAPHER_GRPC_HANDLERS_RECEIVE_LOCAL_SLAM_RESULTS_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"
|
||||||
|
|
||||||
|
namespace cartographer_grpc {
|
||||||
|
namespace handlers {
|
||||||
|
|
||||||
|
class ReceiveLocalSlamResultsHandler
|
||||||
|
: public framework::RpcHandler<
|
||||||
|
proto::ReceiveLocalSlamResultsRequest,
|
||||||
|
framework::Stream<proto::ReceiveLocalSlamResultsResponse>> {
|
||||||
|
public:
|
||||||
|
void OnRequest(
|
||||||
|
const proto::ReceiveLocalSlamResultsRequest& request) override {
|
||||||
|
auto writer = GetWriter();
|
||||||
|
MapBuilderServer::SubscriptionId subscription_id =
|
||||||
|
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
|
||||||
|
->SubscribeLocalSlamResults(
|
||||||
|
request.trajectory_id(),
|
||||||
|
[writer](int trajectory_id, cartographer::common::Time time,
|
||||||
|
cartographer::transform::Rigid3d local_pose,
|
||||||
|
std::shared_ptr<const cartographer::sensor::RangeData>
|
||||||
|
range_data,
|
||||||
|
std::unique_ptr<const cartographer::mapping::NodeId>
|
||||||
|
node_id) {
|
||||||
|
writer(GenerateResponse(trajectory_id, time, local_pose,
|
||||||
|
range_data, std::move(node_id)));
|
||||||
|
});
|
||||||
|
|
||||||
|
subscription_id_ =
|
||||||
|
cartographer::common::make_unique<MapBuilderServer::SubscriptionId>(
|
||||||
|
subscription_id);
|
||||||
|
}
|
||||||
|
|
||||||
|
static std::unique_ptr<proto::ReceiveLocalSlamResultsResponse>
|
||||||
|
GenerateResponse(
|
||||||
|
int trajectory_id, cartographer::common::Time time,
|
||||||
|
const cartographer::transform::Rigid3d& local_pose,
|
||||||
|
std::shared_ptr<const cartographer::sensor::RangeData> range_data,
|
||||||
|
std::unique_ptr<const cartographer::mapping::NodeId> node_id) {
|
||||||
|
auto response = cartographer::common::make_unique<
|
||||||
|
proto::ReceiveLocalSlamResultsResponse>();
|
||||||
|
response->set_trajectory_id(trajectory_id);
|
||||||
|
response->set_timestamp(cartographer::common::ToUniversal(time));
|
||||||
|
*response->mutable_local_pose() =
|
||||||
|
cartographer::transform::ToProto(local_pose);
|
||||||
|
if (range_data) {
|
||||||
|
*response->mutable_range_data() =
|
||||||
|
cartographer::sensor::ToProto(*range_data);
|
||||||
|
}
|
||||||
|
if (node_id) {
|
||||||
|
response->mutable_node_id()->set_trajectory_id(node_id->trajectory_id);
|
||||||
|
response->mutable_node_id()->set_node_index(node_id->node_index);
|
||||||
|
}
|
||||||
|
return response;
|
||||||
|
}
|
||||||
|
|
||||||
|
void OnFinish() override {
|
||||||
|
if (subscription_id_) {
|
||||||
|
GetUnsynchronizedContext<MapBuilderServer::MapBuilderContext>()
|
||||||
|
->UnsubscribeLocalSlamResults(*subscription_id_);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::unique_ptr<MapBuilderServer::SubscriptionId> subscription_id_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace handlers
|
||||||
|
} // namespace cartographer_grpc
|
||||||
|
|
||||||
|
#endif // CARTOGRAPHER_GRPC_HANDLERS_RECEIVE_LOCAL_SLAM_RESULTS_HANDLER_H
|
|
@ -22,6 +22,7 @@
|
||||||
#include "cartographer_grpc/handlers/add_rangefinder_data_handler.h"
|
#include "cartographer_grpc/handlers/add_rangefinder_data_handler.h"
|
||||||
#include "cartographer_grpc/handlers/add_trajectory_handler.h"
|
#include "cartographer_grpc/handlers/add_trajectory_handler.h"
|
||||||
#include "cartographer_grpc/handlers/finish_trajectory_handler.h"
|
#include "cartographer_grpc/handlers/finish_trajectory_handler.h"
|
||||||
|
#include "cartographer_grpc/handlers/receive_local_slam_results_handler.h"
|
||||||
#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
|
#include "cartographer_grpc/proto/map_builder_service.grpc.pb.h"
|
||||||
#include "glog/logging.h"
|
#include "glog/logging.h"
|
||||||
|
|
||||||
|
@ -70,6 +71,18 @@ void MapBuilderServer::MapBuilderContext::AddSensorDataToTrajectory(
|
||||||
sensor_data.trajectory_id));
|
sensor_data.trajectory_id));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
MapBuilderServer::SubscriptionId
|
||||||
|
MapBuilderServer::MapBuilderContext::SubscribeLocalSlamResults(
|
||||||
|
int trajectory_id, LocalSlamSubscriptionCallback callback) {
|
||||||
|
return map_builder_server_->SubscribeLocalSlamResults(trajectory_id,
|
||||||
|
callback);
|
||||||
|
}
|
||||||
|
|
||||||
|
void MapBuilderServer::MapBuilderContext::UnsubscribeLocalSlamResults(
|
||||||
|
const SubscriptionId& subscription_id) {
|
||||||
|
map_builder_server_->UnsubscribeLocalSlamResults(subscription_id);
|
||||||
|
}
|
||||||
|
|
||||||
MapBuilderServer::MapBuilderServer(
|
MapBuilderServer::MapBuilderServer(
|
||||||
const proto::MapBuilderServerOptions& map_builder_server_options,
|
const proto::MapBuilderServerOptions& map_builder_server_options,
|
||||||
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder)
|
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder)
|
||||||
|
@ -95,6 +108,9 @@ MapBuilderServer::MapBuilderServer(
|
||||||
"AddFixedFramePoseData");
|
"AddFixedFramePoseData");
|
||||||
server_builder.RegisterHandler<handlers::FinishTrajectoryHandler,
|
server_builder.RegisterHandler<handlers::FinishTrajectoryHandler,
|
||||||
proto::MapBuilderService>("FinishTrajectory");
|
proto::MapBuilderService>("FinishTrajectory");
|
||||||
|
server_builder.RegisterHandler<handlers::ReceiveLocalSlamResultsHandler,
|
||||||
|
proto::MapBuilderService>(
|
||||||
|
"ReceiveLocalSlamResults");
|
||||||
grpc_server_ = server_builder.Build();
|
grpc_server_ = server_builder.Build();
|
||||||
grpc_server_->SetExecutionContext(
|
grpc_server_->SetExecutionContext(
|
||||||
cartographer::common::make_unique<MapBuilderContext>(this));
|
cartographer::common::make_unique<MapBuilderContext>(this));
|
||||||
|
|
|
@ -14,8 +14,10 @@
|
||||||
|
|
||||||
syntax = "proto3";
|
syntax = "proto3";
|
||||||
|
|
||||||
|
import "cartographer/mapping/proto/pose_graph.proto";
|
||||||
import "cartographer/mapping/proto/trajectory_builder_options.proto";
|
import "cartographer/mapping/proto/trajectory_builder_options.proto";
|
||||||
import "cartographer/sensor/proto/sensor.proto";
|
import "cartographer/sensor/proto/sensor.proto";
|
||||||
|
import "cartographer/transform/proto/transform.proto";
|
||||||
import "google/protobuf/empty.proto";
|
import "google/protobuf/empty.proto";
|
||||||
|
|
||||||
package cartographer_grpc.proto;
|
package cartographer_grpc.proto;
|
||||||
|
@ -59,6 +61,18 @@ message FinishTrajectoryRequest {
|
||||||
int32 trajectory_id = 1;
|
int32 trajectory_id = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
message ReceiveLocalSlamResultsRequest {
|
||||||
|
int32 trajectory_id = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
message ReceiveLocalSlamResultsResponse {
|
||||||
|
int32 trajectory_id = 1;
|
||||||
|
int64 timestamp = 2;
|
||||||
|
cartographer.transform.proto.Rigid3d local_pose = 3;
|
||||||
|
cartographer.sensor.proto.RangeData range_data = 4;
|
||||||
|
cartographer.mapping.proto.NodeId node_id = 5;
|
||||||
|
}
|
||||||
|
|
||||||
service MapBuilderService {
|
service MapBuilderService {
|
||||||
// Starts a new trajectory and returns its index.
|
// Starts a new trajectory and returns its index.
|
||||||
rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse);
|
rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse);
|
||||||
|
@ -79,6 +93,11 @@ service MapBuilderService {
|
||||||
rpc AddFixedFramePoseData(stream AddFixedFramePoseDataRequest)
|
rpc AddFixedFramePoseData(stream AddFixedFramePoseDataRequest)
|
||||||
returns (google.protobuf.Empty);
|
returns (google.protobuf.Empty);
|
||||||
|
|
||||||
|
// Requests the server to send a stream of local SLAM results for the given
|
||||||
|
// 'trajectory_id'.
|
||||||
|
rpc ReceiveLocalSlamResults(ReceiveLocalSlamResultsRequest)
|
||||||
|
returns (stream ReceiveLocalSlamResultsResponse);
|
||||||
|
|
||||||
// Marks a trajectory corresponding to 'trajectory_id' as finished,
|
// Marks a trajectory corresponding to 'trajectory_id' as finished,
|
||||||
// i.e. no further sensor data is expected.
|
// i.e. no further sensor data is expected.
|
||||||
rpc FinishTrajectory(FinishTrajectoryRequest) returns (google.protobuf.Empty);
|
rpc FinishTrajectory(FinishTrajectoryRequest) returns (google.protobuf.Empty);
|
||||||
|
|
Loading…
Reference in New Issue