2017-12-13 22:53:47 +08:00
|
|
|
/*
|
|
|
|
* 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.
|
|
|
|
*/
|
|
|
|
|
2018-03-02 01:30:30 +08:00
|
|
|
#include "cartographer_grpc/internal/client/trajectory_builder_stub.h"
|
2017-12-13 22:53:47 +08:00
|
|
|
|
2018-01-11 17:19:37 +08:00
|
|
|
#include "cartographer/mapping/local_slam_result_data.h"
|
2018-03-02 21:16:49 +08:00
|
|
|
#include "cartographer_grpc/internal/sensor/serialization.h"
|
2017-12-18 18:24:16 +08:00
|
|
|
#include "cartographer_grpc/proto/map_builder_service.pb.h"
|
2017-12-13 22:53:47 +08:00
|
|
|
#include "glog/logging.h"
|
|
|
|
|
|
|
|
namespace cartographer_grpc {
|
|
|
|
namespace mapping {
|
|
|
|
|
|
|
|
TrajectoryBuilderStub::TrajectoryBuilderStub(
|
2017-12-20 19:42:27 +08:00
|
|
|
std::shared_ptr<grpc::Channel> client_channel, const int trajectory_id,
|
|
|
|
LocalSlamResultCallback local_slam_result_callback)
|
2018-02-02 00:19:15 +08:00
|
|
|
: client_channel_(client_channel),
|
|
|
|
trajectory_id_(trajectory_id),
|
|
|
|
receive_local_slam_results_client_(client_channel_) {
|
2017-12-20 19:42:27 +08:00
|
|
|
if (local_slam_result_callback) {
|
|
|
|
proto::ReceiveLocalSlamResultsRequest request;
|
|
|
|
request.set_trajectory_id(trajectory_id);
|
2018-02-02 00:19:15 +08:00
|
|
|
receive_local_slam_results_client_.Write(request);
|
|
|
|
auto* receive_local_slam_results_client_ptr =
|
|
|
|
&receive_local_slam_results_client_;
|
|
|
|
receive_local_slam_results_thread_ =
|
2017-12-20 19:42:27 +08:00
|
|
|
cartographer::common::make_unique<std::thread>(
|
2018-02-02 00:19:15 +08:00
|
|
|
[receive_local_slam_results_client_ptr,
|
|
|
|
local_slam_result_callback]() {
|
|
|
|
RunLocalSlamResultsReader(receive_local_slam_results_client_ptr,
|
|
|
|
local_slam_result_callback);
|
2017-12-20 19:42:27 +08:00
|
|
|
});
|
|
|
|
}
|
2017-12-18 18:24:16 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
TrajectoryBuilderStub::~TrajectoryBuilderStub() {
|
2018-02-02 00:19:15 +08:00
|
|
|
if (receive_local_slam_results_thread_) {
|
|
|
|
receive_local_slam_results_thread_->join();
|
2017-12-20 19:42:27 +08:00
|
|
|
}
|
2018-02-02 00:19:15 +08:00
|
|
|
if (add_rangefinder_client_) {
|
|
|
|
CHECK(add_rangefinder_client_->WritesDone());
|
|
|
|
CHECK(add_rangefinder_client_->Finish().ok());
|
2017-12-18 18:24:16 +08:00
|
|
|
}
|
2018-02-02 00:19:15 +08:00
|
|
|
if (add_imu_client_) {
|
|
|
|
CHECK(add_imu_client_->WritesDone());
|
|
|
|
CHECK(add_imu_client_->Finish().ok());
|
2017-12-18 18:24:16 +08:00
|
|
|
}
|
2018-02-02 00:19:15 +08:00
|
|
|
if (add_odometry_client_) {
|
|
|
|
CHECK(add_odometry_client_->WritesDone());
|
|
|
|
CHECK(add_odometry_client_->Finish().ok());
|
2017-12-18 18:24:16 +08:00
|
|
|
}
|
2018-02-02 00:19:15 +08:00
|
|
|
if (add_landmark_client_) {
|
|
|
|
CHECK(add_landmark_client_->WritesDone());
|
|
|
|
CHECK(add_landmark_client_->Finish().ok());
|
|
|
|
}
|
|
|
|
if (add_fixed_frame_pose_client_) {
|
|
|
|
CHECK(add_fixed_frame_pose_client_->WritesDone());
|
|
|
|
CHECK(add_fixed_frame_pose_client_->Finish().ok());
|
2017-12-18 18:24:16 +08:00
|
|
|
}
|
|
|
|
}
|
2017-12-13 22:53:47 +08:00
|
|
|
|
|
|
|
void TrajectoryBuilderStub::AddSensorData(
|
|
|
|
const std::string& sensor_id,
|
|
|
|
const cartographer::sensor::TimedPointCloudData& timed_point_cloud_data) {
|
2018-02-02 00:19:15 +08:00
|
|
|
if (!add_rangefinder_client_) {
|
|
|
|
add_rangefinder_client_ = cartographer::common::make_unique<
|
|
|
|
framework::Client<handlers::AddRangefinderDataHandler>>(
|
|
|
|
client_channel_);
|
2017-12-18 18:24:16 +08:00
|
|
|
}
|
|
|
|
proto::AddRangefinderDataRequest request;
|
2018-01-16 19:20:15 +08:00
|
|
|
sensor::CreateAddRangeFinderDataRequest(
|
2018-01-15 22:31:33 +08:00
|
|
|
sensor_id, trajectory_id_,
|
|
|
|
cartographer::sensor::ToProto(timed_point_cloud_data), &request);
|
2018-02-02 00:19:15 +08:00
|
|
|
add_rangefinder_client_->Write(request);
|
2017-12-13 22:53:47 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
void TrajectoryBuilderStub::AddSensorData(
|
|
|
|
const std::string& sensor_id,
|
|
|
|
const cartographer::sensor::ImuData& imu_data) {
|
2018-02-02 00:19:15 +08:00
|
|
|
if (!add_imu_client_) {
|
|
|
|
add_imu_client_ = cartographer::common::make_unique<
|
|
|
|
framework::Client<handlers::AddImuDataHandler>>(client_channel_);
|
2017-12-18 18:24:16 +08:00
|
|
|
}
|
|
|
|
proto::AddImuDataRequest request;
|
2018-01-16 19:20:15 +08:00
|
|
|
sensor::CreateAddImuDataRequest(sensor_id, trajectory_id_,
|
|
|
|
cartographer::sensor::ToProto(imu_data),
|
|
|
|
&request);
|
2018-02-02 00:19:15 +08:00
|
|
|
add_imu_client_->Write(request);
|
2017-12-13 22:53:47 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
void TrajectoryBuilderStub::AddSensorData(
|
|
|
|
const std::string& sensor_id,
|
|
|
|
const cartographer::sensor::OdometryData& odometry_data) {
|
2018-02-02 00:19:15 +08:00
|
|
|
if (!add_odometry_client_) {
|
|
|
|
add_odometry_client_ = cartographer::common::make_unique<
|
|
|
|
framework::Client<handlers::AddOdometryDataHandler>>(client_channel_);
|
2017-12-18 18:24:16 +08:00
|
|
|
}
|
|
|
|
proto::AddOdometryDataRequest request;
|
2018-01-16 19:20:15 +08:00
|
|
|
sensor::CreateAddOdometryDataRequest(
|
|
|
|
sensor_id, trajectory_id_, cartographer::sensor::ToProto(odometry_data),
|
|
|
|
&request);
|
2018-02-02 00:19:15 +08:00
|
|
|
add_odometry_client_->Write(request);
|
2017-12-13 22:53:47 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
void TrajectoryBuilderStub::AddSensorData(
|
|
|
|
const std::string& sensor_id,
|
|
|
|
const cartographer::sensor::FixedFramePoseData& fixed_frame_pose) {
|
2018-02-02 00:19:15 +08:00
|
|
|
if (!add_fixed_frame_pose_client_) {
|
|
|
|
add_fixed_frame_pose_client_ = cartographer::common::make_unique<
|
|
|
|
framework::Client<handlers::AddFixedFramePoseDataHandler>>(
|
|
|
|
client_channel_);
|
2017-12-18 18:24:16 +08:00
|
|
|
}
|
|
|
|
proto::AddFixedFramePoseDataRequest request;
|
2018-01-16 19:20:15 +08:00
|
|
|
sensor::CreateAddFixedFramePoseDataRequest(
|
2018-01-15 22:31:33 +08:00
|
|
|
sensor_id, trajectory_id_,
|
|
|
|
cartographer::sensor::ToProto(fixed_frame_pose), &request);
|
2018-02-02 00:19:15 +08:00
|
|
|
add_fixed_frame_pose_client_->Write(request);
|
2017-12-18 18:24:16 +08:00
|
|
|
}
|
|
|
|
|
2018-01-19 16:41:24 +08:00
|
|
|
void TrajectoryBuilderStub::AddSensorData(
|
|
|
|
const std::string& sensor_id,
|
|
|
|
const cartographer::sensor::LandmarkData& landmark_data) {
|
2018-02-02 00:19:15 +08:00
|
|
|
if (!add_landmark_client_) {
|
|
|
|
add_landmark_client_ = cartographer::common::make_unique<
|
|
|
|
framework::Client<handlers::AddLandmarkDataHandler>>(client_channel_);
|
2018-01-20 00:24:30 +08:00
|
|
|
}
|
|
|
|
proto::AddLandmarkDataRequest request;
|
|
|
|
sensor::CreateAddLandmarkDataRequest(
|
|
|
|
sensor_id, trajectory_id_, cartographer::sensor::ToProto(landmark_data),
|
|
|
|
&request);
|
2018-02-02 00:19:15 +08:00
|
|
|
add_landmark_client_->Write(request);
|
2018-01-19 16:41:24 +08:00
|
|
|
}
|
|
|
|
|
2018-01-11 17:19:37 +08:00
|
|
|
void TrajectoryBuilderStub::AddLocalSlamResultData(
|
|
|
|
std::unique_ptr<cartographer::mapping::LocalSlamResultData>
|
|
|
|
local_slam_result_data) {
|
|
|
|
LOG(FATAL) << "Not implemented";
|
|
|
|
}
|
|
|
|
|
2018-02-02 00:19:15 +08:00
|
|
|
void TrajectoryBuilderStub::RunLocalSlamResultsReader(
|
|
|
|
framework::Client<handlers::ReceiveLocalSlamResultsHandler>* client,
|
2017-12-20 19:42:27 +08:00
|
|
|
LocalSlamResultCallback local_slam_result_callback) {
|
|
|
|
proto::ReceiveLocalSlamResultsResponse response;
|
2018-02-02 00:19:15 +08:00
|
|
|
while (client->Read(&response)) {
|
2017-12-20 19:42:27 +08:00
|
|
|
int trajectory_id = response.trajectory_id();
|
|
|
|
cartographer::common::Time time =
|
|
|
|
cartographer::common::FromUniversal(response.timestamp());
|
|
|
|
cartographer::transform::Rigid3d local_pose =
|
|
|
|
cartographer::transform::ToRigid3(response.local_pose());
|
|
|
|
cartographer::sensor::RangeData range_data =
|
|
|
|
cartographer::sensor::FromProto(response.range_data());
|
2018-01-11 00:26:04 +08:00
|
|
|
auto insertion_result =
|
|
|
|
response.has_insertion_result()
|
|
|
|
? cartographer::common::make_unique<InsertionResult>(
|
|
|
|
InsertionResult{cartographer::mapping::NodeId{
|
|
|
|
response.insertion_result().node_id().trajectory_id(),
|
|
|
|
response.insertion_result().node_id().node_index()}})
|
2017-12-20 19:42:27 +08:00
|
|
|
: nullptr;
|
|
|
|
local_slam_result_callback(trajectory_id, time, local_pose, range_data,
|
2018-01-11 00:26:04 +08:00
|
|
|
std::move(insertion_result));
|
2017-12-20 19:42:27 +08:00
|
|
|
}
|
2018-02-02 00:19:15 +08:00
|
|
|
client->Finish();
|
2017-12-20 19:42:27 +08:00
|
|
|
}
|
|
|
|
|
2017-12-13 22:53:47 +08:00
|
|
|
} // namespace mapping
|
|
|
|
} // namespace cartographer_grpc
|