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

Implement IsTrajectory{Finished,Frozen} in cloud mapping (#1057)

master
Christoph Schütte 2018-04-12 12:37:04 +02:00 committed by Wally B. Feed
parent 771336b3c9
commit f83d0a6ed6
8 changed files with 210 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
cartographer/cloud/internal/client/pose_graph_stub.cc
View File

@ -21,6 +21,8 @@
#include "cartographer/cloud/internal/handlers/get_landmark_poses_handler.h"
#include "cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.h"
#include "cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.h"
#include "cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h"
#include "cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h"
#include "cartographer/cloud/internal/handlers/run_final_optimization_handler.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/transform/transform.h"
@ -108,11 +110,21 @@ std::map<std::string, transform::Rigid3d> PoseGraphStub::GetLandmarkPoses() {
}
bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) {
LOG(FATAL) << "Not implemented";
proto::IsTrajectoryFinishedRequest request;
request.set_trajectory_id(trajectory_id);
async_grpc::Client<handlers::IsTrajectoryFinishedSignature> client(
client_channel_);
CHECK(client.Write(request));
return client.response().is_finished();
}
bool PoseGraphStub::IsTrajectoryFrozen(int trajectory_id) {
LOG(FATAL) << "Not implemented";
proto::IsTrajectoryFrozenRequest request;
request.set_trajectory_id(trajectory_id);
async_grpc::Client<handlers::IsTrajectoryFrozenSignature> client(
client_channel_);
CHECK(client.Write(request));
return client.response().is_frozen();
}
std::map<int, mapping::PoseGraphInterface::TrajectoryData>

3
cartographer/cloud/internal/client_server_test.cc
View File

@ -140,7 +140,10 @@ TEST_F(ClientServerTest, AddTrajectoryBuilder) {
InitializeStub();
int trajectory_id = stub_->AddTrajectoryBuilder(
{kImuSensorId}, trajectory_builder_options_, nullptr);
EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFinished(trajectory_id));
stub_->FinishTrajectory(trajectory_id);
EXPECT_TRUE(stub_->pose_graph()->IsTrajectoryFinished(trajectory_id));
EXPECT_FALSE(stub_->pose_graph()->IsTrajectoryFrozen(trajectory_id));
server_->Shutdown();
}

41
cartographer/cloud/internal/handlers/is_trajectory_finished_handler.cc Normal file
View File

@ -0,0 +1,41 @@
/*
* 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 "cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h"
#include "async_grpc/rpc_handler.h"
#include "cartographer/cloud/internal/map_builder_context_interface.h"
#include "cartographer/cloud/proto/map_builder_service.pb.h"
#include "cartographer/common/make_unique.h"
namespace cartographer {
namespace cloud {
namespace handlers {
void IsTrajectoryFinishedHandler::OnRequest(
const proto::IsTrajectoryFinishedRequest& request) {
auto response = common::make_unique<proto::IsTrajectoryFinishedResponse>();
response->set_is_finished(
GetContext<MapBuilderContextInterface>()
->map_builder()
.pose_graph()
->IsTrajectoryFinished(request.trajectory_id()));
Send(std::move(response));
}
} // namespace handlers
} // namespace cloud
} // namespace cartographer

42
cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h Normal file
View File

@ -0,0 +1,42 @@
/*
* 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_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FINISHED_HANDLER_H
#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FINISHED_HANDLER_H
#include "async_grpc/rpc_handler.h"
#include "cartographer/cloud/proto/map_builder_service.pb.h"
namespace cartographer {
namespace cloud {
namespace handlers {
DEFINE_HANDLER_SIGNATURE(
IsTrajectoryFinishedSignature, proto::IsTrajectoryFinishedRequest,
proto::IsTrajectoryFinishedResponse,
"/cartographer.cloud.proto.MapBuilderService/IsTrajectoryFinished")
class IsTrajectoryFinishedHandler
: public async_grpc::RpcHandler<IsTrajectoryFinishedSignature> {
public:
void OnRequest(const proto::IsTrajectoryFinishedRequest& request) override;
};
} // namespace handlers
} // namespace cloud
} // namespace cartographer
#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FINISHED_HANDLER_H

40
cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.cc Normal file
View File

@ -0,0 +1,40 @@
/*
* 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 "cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h"
#include "async_grpc/rpc_handler.h"
#include "cartographer/cloud/internal/map_builder_context_interface.h"
#include "cartographer/cloud/proto/map_builder_service.pb.h"
#include "cartographer/common/make_unique.h"
namespace cartographer {
namespace cloud {
namespace handlers {
void IsTrajectoryFrozenHandler::OnRequest(
const proto::IsTrajectoryFrozenRequest& request) {
auto response = common::make_unique<proto::IsTrajectoryFrozenResponse>();
response->set_is_frozen(GetContext<MapBuilderContextInterface>()
->map_builder()
.pose_graph()
->IsTrajectoryFrozen(request.trajectory_id()));
Send(std::move(response));
}
} // namespace handlers
} // namespace cloud
} // namespace cartographer

42
cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h Normal file
View File

@ -0,0 +1,42 @@
/*
* 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_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FROZEN_HANDLER_H
#define CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FROZEN_HANDLER_H
#include "async_grpc/rpc_handler.h"
#include "cartographer/cloud/proto/map_builder_service.pb.h"
namespace cartographer {
namespace cloud {
namespace handlers {
DEFINE_HANDLER_SIGNATURE(
IsTrajectoryFrozenSignature, proto::IsTrajectoryFrozenRequest,
proto::IsTrajectoryFrozenResponse,
"/cartographer.cloud.proto.MapBuilderService/IsTrajectoryFrozen")
class IsTrajectoryFrozenHandler
: public async_grpc::RpcHandler<IsTrajectoryFrozenSignature> {
public:
void OnRequest(const proto::IsTrajectoryFrozenRequest& request) override;
};
} // namespace handlers
} // namespace cloud
} // namespace cartographer
#endif // CARTOGRAPHER_CLOUD_INTERNAL_HANDLERS_IS_TRAJECTORY_FROZEN_HANDLER_H

4
cartographer/cloud/internal/map_builder_server.cc
View File

@ -30,6 +30,8 @@
#include "cartographer/cloud/internal/handlers/get_local_to_global_transform_handler.h"
#include "cartographer/cloud/internal/handlers/get_submap_handler.h"
#include "cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.h"
#include "cartographer/cloud/internal/handlers/is_trajectory_finished_handler.h"
#include "cartographer/cloud/internal/handlers/is_trajectory_frozen_handler.h"
#include "cartographer/cloud/internal/handlers/load_state_handler.h"
#include "cartographer/cloud/internal/handlers/receive_local_slam_results_handler.h"
#include "cartographer/cloud/internal/handlers/run_final_optimization_handler.h"
@ -74,6 +76,8 @@ MapBuilderServer::MapBuilderServer(
server_builder.RegisterHandler<handlers::GetAllSubmapPosesHandler>();
server_builder.RegisterHandler<handlers::GetLocalToGlobalTransformHandler>();
server_builder.RegisterHandler<handlers::GetConstraintsHandler>();
server_builder.RegisterHandler<handlers::IsTrajectoryFinishedHandler>();
server_builder.RegisterHandler<handlers::IsTrajectoryFrozenHandler>();
server_builder.RegisterHandler<handlers::LoadStateHandler>();
server_builder.RegisterHandler<handlers::RunFinalOptimizationHandler>();
server_builder.RegisterHandler<handlers::WriteStateHandler>();

24
cartographer/cloud/proto/map_builder_service.proto
View File

@ -171,6 +171,22 @@ message WriteStateResponse {
}
}
message IsTrajectoryFinishedRequest {
int32 trajectory_id = 1;
}
message IsTrajectoryFinishedResponse {
bool is_finished = 1;
}
message IsTrajectoryFrozenRequest {
int32 trajectory_id = 1;
}
message IsTrajectoryFrozenResponse {
bool is_frozen = 1;
}
service MapBuilderService {
// Starts a new trajectory and returns its index.
rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse);
@ -230,6 +246,14 @@ service MapBuilderService {
// Returns the list of constraints in the current optimization problem.
rpc GetConstraints(google.protobuf.Empty) returns (GetConstraintsResponse);
// Checks whether the trajectory is finished.
rpc IsTrajectoryFinished(IsTrajectoryFinishedRequest)
returns (IsTrajectoryFinishedResponse);
// Checks whether the trajectory is frozen.
rpc IsTrajectoryFrozen(IsTrajectoryFrozenRequest)
returns (IsTrajectoryFrozenResponse);
// Requests a PoseGraph to call RunFinalOptimization.
rpc RunFinalOptimization(google.protobuf.Empty)
returns (google.protobuf.Empty);