Add LocalSlamResultData retrieval and submap management (#810)
[RFC=0002](https://github.com/googlecartographer/rfcs/blob/master/text/0002-cloud-based-mapping-1.md)master
Christoph Schütte
Wally B. Feed
parent
51ccee3e74
commit
fee77c8a99
|
|
@ -24,9 +24,14 @@ LocalSlamResultData::LocalSlamResultData(const std::string &sensor_id,
|
|||
common::Time time)
|
||||
: Data(sensor_id), time_(time) {}
|
||||
|
||||
LocalSlamResult2D::LocalSlamResult2D(const std::string &sensor_id,
|
||||
common::Time time)
|
||||
: LocalSlamResultData(sensor_id, time) {}
|
||||
LocalSlamResult2D::LocalSlamResult2D(
|
||||
const std::string &sensor_id, common::Time time,
|
||||
std::shared_ptr<const mapping::TrajectoryNode::Data> node_data,
|
||||
const std::vector<std::shared_ptr<const mapping_2d::Submap>>
|
||||
&insertion_submaps)
|
||||
: LocalSlamResultData(sensor_id, time),
|
||||
node_data_(node_data),
|
||||
insertion_submaps_(insertion_submaps) {}
|
||||
|
||||
void LocalSlamResult2D::AddToTrajectoryBuilder(
|
||||
mapping::TrajectoryBuilderInterface *const trajectory_builder) {
|
||||
|
|
@ -39,12 +44,17 @@ void LocalSlamResult2D::AddToPoseGraph(int trajectory_id,
|
|||
DCHECK(dynamic_cast<mapping_2d::PoseGraph *>(pose_graph));
|
||||
mapping_2d::PoseGraph *pose_graph_2d =
|
||||
static_cast<mapping_2d::PoseGraph *>(pose_graph);
|
||||
pose_graph_2d->AddNode(constant_data, trajectory_id, insertion_submaps);
|
||||
pose_graph_2d->AddNode(node_data_, trajectory_id, insertion_submaps_);
|
||||
}
|
||||
|
||||
LocalSlamResult3D::LocalSlamResult3D(const std::string &sensor_id,
|
||||
common::Time time)
|
||||
: LocalSlamResultData(sensor_id, time) {}
|
||||
LocalSlamResult3D::LocalSlamResult3D(
|
||||
const std::string &sensor_id, common::Time time,
|
||||
std::shared_ptr<const mapping::TrajectoryNode::Data> node_data,
|
||||
const std::vector<std::shared_ptr<const mapping_3d::Submap>>
|
||||
&insertion_submaps)
|
||||
: LocalSlamResultData(sensor_id, time),
|
||||
node_data_(node_data),
|
||||
insertion_submaps_(insertion_submaps) {}
|
||||
|
||||
void LocalSlamResult3D::AddToTrajectoryBuilder(
|
||||
mapping::TrajectoryBuilderInterface *const trajectory_builder) {
|
||||
|
|
@ -57,7 +67,7 @@ void LocalSlamResult3D::AddToPoseGraph(int trajectory_id,
|
|||
DCHECK(dynamic_cast<mapping_3d::PoseGraph *>(pose_graph));
|
||||
mapping_3d::PoseGraph *pose_graph_3d =
|
||||
static_cast<mapping_3d::PoseGraph *>(pose_graph);
|
||||
pose_graph_3d->AddNode(constant_data, trajectory_id, insertion_submaps);
|
||||
pose_graph_3d->AddNode(node_data_, trajectory_id, insertion_submaps_);
|
||||
}
|
||||
|
||||
} // namespace mapping
|
||||
|
|
|
|||
|
|
@ -39,7 +39,11 @@ class LocalSlamResultData : public cartographer::sensor::Data {
|
|||
|
||||
class LocalSlamResult2D : public LocalSlamResultData {
|
||||
public:
|
||||
LocalSlamResult2D(const std::string &sensor_id, common::Time time);
|
||||
LocalSlamResult2D(
|
||||
const std::string &sensor_id, common::Time time,
|
||||
std::shared_ptr<const mapping::TrajectoryNode::Data> node_data,
|
||||
const std::vector<std::shared_ptr<const mapping_2d::Submap>>
|
||||
&insertion_submaps);
|
||||
|
||||
void AddToTrajectoryBuilder(
|
||||
mapping::TrajectoryBuilderInterface *const trajectory_builder) override;
|
||||
|
|
@ -47,13 +51,17 @@ class LocalSlamResult2D : public LocalSlamResultData {
|
|||
mapping::PoseGraph *pose_graph) const override;
|
||||
|
||||
private:
|
||||
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
|
||||
std::vector<std::shared_ptr<const mapping_2d::Submap>> insertion_submaps;
|
||||
std::shared_ptr<const mapping::TrajectoryNode::Data> node_data_;
|
||||
std::vector<std::shared_ptr<const mapping_2d::Submap>> insertion_submaps_;
|
||||
};
|
||||
|
||||
class LocalSlamResult3D : public LocalSlamResultData {
|
||||
public:
|
||||
LocalSlamResult3D(const std::string &sensor_id, common::Time time);
|
||||
LocalSlamResult3D(
|
||||
const std::string &sensor_id, common::Time time,
|
||||
std::shared_ptr<const mapping::TrajectoryNode::Data> node_data,
|
||||
const std::vector<std::shared_ptr<const mapping_3d::Submap>>
|
||||
&insertion_submaps);
|
||||
|
||||
void AddToTrajectoryBuilder(
|
||||
mapping::TrajectoryBuilderInterface *const trajectory_builder) override;
|
||||
|
|
@ -61,8 +69,8 @@ class LocalSlamResult3D : public LocalSlamResultData {
|
|||
mapping::PoseGraph *pose_graph) const override;
|
||||
|
||||
private:
|
||||
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data;
|
||||
std::vector<std::shared_ptr<const mapping_3d::Submap>> insertion_submaps;
|
||||
std::shared_ptr<const mapping::TrajectoryNode::Data> node_data_;
|
||||
std::vector<std::shared_ptr<const mapping_3d::Submap>> insertion_submaps_;
|
||||
};
|
||||
|
||||
} // namespace mapping
|
||||
|
|
|
|||
|
|
@ -54,3 +54,9 @@ message SerializedData {
|
|||
OdometryData odometry_data = 4;
|
||||
FixedFramePoseData fixed_frame_pose_data = 5;
|
||||
}
|
||||
|
||||
message LocalSlamResultData {
|
||||
int64 timestamp = 1;
|
||||
TrajectoryNodeData node_data = 2;
|
||||
repeated Submap submaps = 3;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -64,6 +64,7 @@ class Submap {
|
|||
virtual ~Submap() {}
|
||||
|
||||
virtual void ToProto(proto::Submap* proto) const = 0;
|
||||
virtual void UpdateFromProto(const proto::Submap& proto) = 0;
|
||||
|
||||
// Pose of this submap in the local map frame.
|
||||
transform::Rigid3d local_pose() const { return local_pose_; }
|
||||
|
|
@ -76,7 +77,6 @@ class Submap {
|
|||
const transform::Rigid3d& global_submap_pose,
|
||||
proto::SubmapQuery::Response* response) const = 0;
|
||||
|
||||
protected:
|
||||
void SetNumRangeData(const int num_range_data) {
|
||||
num_range_data_ = num_range_data;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -82,6 +82,16 @@ void Submap::ToProto(mapping::proto::Submap* const proto) const {
|
|||
*submap_2d->mutable_probability_grid() = probability_grid_.ToProto();
|
||||
}
|
||||
|
||||
void Submap::UpdateFromProto(const mapping::proto::Submap& proto) {
|
||||
CHECK(proto.has_submap_2d());
|
||||
const auto& submap_2d = proto.submap_2d();
|
||||
SetNumRangeData(submap_2d.num_range_data());
|
||||
finished_ = submap_2d.finished();
|
||||
if (submap_2d.has_probability_grid()) {
|
||||
probability_grid_ = ProbabilityGrid(submap_2d.probability_grid());
|
||||
}
|
||||
}
|
||||
|
||||
void Submap::ToResponseProto(
|
||||
const transform::Rigid3d&,
|
||||
mapping::proto::SubmapQuery::Response* const response) const {
|
||||
|
|
|
|||
|
|
@ -48,6 +48,7 @@ class Submap : public mapping::Submap {
|
|||
explicit Submap(const mapping::proto::Submap2D& proto);
|
||||
|
||||
void ToProto(mapping::proto::Submap* proto) const override;
|
||||
void UpdateFromProto(const mapping::proto::Submap& proto) override;
|
||||
|
||||
const ProbabilityGrid& probability_grid() const { return probability_grid_; }
|
||||
bool finished() const { return finished_; }
|
||||
|
|
|
|||
|
|
@ -226,6 +226,27 @@ void Submap::ToProto(mapping::proto::Submap* const proto) const {
|
|||
low_resolution_hybrid_grid().ToProto();
|
||||
}
|
||||
|
||||
void Submap::UpdateFromProto(const mapping::proto::Submap& proto) {
|
||||
CHECK(proto.has_submap_3d());
|
||||
const auto& submap_3d = proto.submap_3d();
|
||||
SetNumRangeData(submap_3d.num_range_data());
|
||||
finished_ = submap_3d.finished();
|
||||
if (submap_3d.has_high_resolution_hybrid_grid()) {
|
||||
high_resolution_hybrid_grid_ =
|
||||
submap_3d.has_high_resolution_hybrid_grid()
|
||||
? common::make_unique<HybridGrid>(
|
||||
submap_3d.high_resolution_hybrid_grid())
|
||||
: nullptr;
|
||||
}
|
||||
if (submap_3d.has_low_resolution_hybrid_grid()) {
|
||||
low_resolution_hybrid_grid_ =
|
||||
submap_3d.has_low_resolution_hybrid_grid()
|
||||
? common::make_unique<HybridGrid>(
|
||||
submap_3d.low_resolution_hybrid_grid())
|
||||
: nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
void Submap::ToResponseProto(
|
||||
const transform::Rigid3d& global_submap_pose,
|
||||
mapping::proto::SubmapQuery::Response* const response) const {
|
||||
|
|
|
|||
|
|
@ -48,6 +48,7 @@ class Submap : public mapping::Submap {
|
|||
explicit Submap(const mapping::proto::Submap3D& proto);
|
||||
|
||||
void ToProto(mapping::proto::Submap* proto) const override;
|
||||
void UpdateFromProto(const mapping::proto::Submap& proto) override;
|
||||
|
||||
const HybridGrid& high_resolution_hybrid_grid() const {
|
||||
return *high_resolution_hybrid_grid_;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,57 @@
|
|||
/*
|
||||
* 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_LOCAL_SLAM_RESULT_DATA_HANDLER_H
|
||||
#define CARTOGRAPHER_GRPC_HANDLERS_ADD_LOCAL_SLAM_RESULT_DATA_HANDLER_H
|
||||
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/mapping/trajectory_node.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 AddLocalSlamResultDataHandler
|
||||
: public framework::RpcHandler<
|
||||
framework::Stream<proto::AddLocalSlamResultDataRequest>,
|
||||
google::protobuf::Empty> {
|
||||
public:
|
||||
void OnRequest(const proto::AddLocalSlamResultDataRequest& request) override {
|
||||
auto local_slam_result_data =
|
||||
GetContext<MapBuilderServer::MapBuilderContext>()
|
||||
->ProcessLocalSlamResultData(
|
||||
request.sensor_metadata().sensor_id(),
|
||||
cartographer::common::FromUniversal(
|
||||
request.local_slam_result_data().timestamp()),
|
||||
request.local_slam_result_data());
|
||||
GetContext<MapBuilderServer::MapBuilderContext>()
|
||||
->EnqueueLocalSlamResultData(request.sensor_metadata().trajectory_id(),
|
||||
request.sensor_metadata().sensor_id(),
|
||||
std::move(local_slam_result_data));
|
||||
}
|
||||
|
||||
void OnReadsDone() override {
|
||||
Send(cartographer::common::make_unique<google::protobuf::Empty>());
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace handlers
|
||||
} // namespace cartographer_grpc
|
||||
|
||||
#endif // CARTOGRAPHER_GRPC_HANDLERS_ADD_LOCAL_SLAM_RESULT_DATA_HANDLER_H
|
||||
|
|
@ -18,6 +18,7 @@
|
|||
|
||||
#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_local_slam_result_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_trajectory_handler.h"
|
||||
|
|
@ -48,10 +49,9 @@ MapBuilderServer::MapBuilderContext::map_builder() {
|
|||
return *map_builder_server_->map_builder_;
|
||||
}
|
||||
|
||||
cartographer::common::BlockingQueue<
|
||||
std::unique_ptr<MapBuilderServer::SensorData>>&
|
||||
cartographer::common::BlockingQueue<std::unique_ptr<MapBuilderServer::Data>>&
|
||||
MapBuilderServer::MapBuilderContext::sensor_data_queue() {
|
||||
return map_builder_server_->sensor_data_queue_;
|
||||
return map_builder_server_->incoming_data_queue_;
|
||||
}
|
||||
|
||||
cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
|
||||
|
|
@ -72,8 +72,8 @@ MapBuilderServer::MapBuilderContext::
|
|||
}
|
||||
|
||||
void MapBuilderServer::MapBuilderContext::AddSensorDataToTrajectory(
|
||||
const SensorData& sensor_data) {
|
||||
sensor_data.sensor_data->AddToTrajectoryBuilder(
|
||||
const Data& sensor_data) {
|
||||
sensor_data.data->AddToTrajectoryBuilder(
|
||||
map_builder_server_->map_builder_->GetTrajectoryBuilder(
|
||||
sensor_data.trajectory_id));
|
||||
}
|
||||
|
|
@ -95,6 +95,108 @@ void MapBuilderServer::MapBuilderContext::NotifyFinishTrajectory(
|
|||
map_builder_server_->NotifyFinishTrajectory(trajectory_id);
|
||||
}
|
||||
|
||||
std::shared_ptr<cartographer::mapping_2d::Submap>
|
||||
MapBuilderServer::MapBuilderContext::UpdateSubmap2D(
|
||||
const cartographer::mapping::proto::Submap& proto) {
|
||||
CHECK(proto.has_submap_2d());
|
||||
cartographer::mapping::SubmapId submap_id{proto.submap_id().trajectory_id(),
|
||||
proto.submap_id().submap_index()};
|
||||
std::shared_ptr<cartographer::mapping_2d::Submap> submap_2d_ptr;
|
||||
auto submap_it = unfinished_submaps_.find(submap_id);
|
||||
if (submap_it == unfinished_submaps_.end()) {
|
||||
// Seeing a submap for the first time it should never be finished.
|
||||
CHECK(!proto.submap_2d().finished());
|
||||
submap_2d_ptr =
|
||||
std::make_shared<cartographer::mapping_2d::Submap>(proto.submap_2d());
|
||||
unfinished_submaps_.Insert(submap_id, submap_2d_ptr);
|
||||
} else {
|
||||
submap_2d_ptr = std::dynamic_pointer_cast<cartographer::mapping_2d::Submap>(
|
||||
submap_it->data);
|
||||
CHECK(submap_2d_ptr);
|
||||
submap_2d_ptr->UpdateFromProto(proto);
|
||||
|
||||
// If the submap was just finished by the recent update, remove it from the
|
||||
// list of unfinished submaps.
|
||||
if (submap_2d_ptr->finished()) {
|
||||
unfinished_submaps_.Trim(submap_id);
|
||||
} else {
|
||||
// If the submap is unfinished set the 'num_range_data' to 0 since we
|
||||
// haven't changed the HybridGrid.
|
||||
submap_2d_ptr->SetNumRangeData(0);
|
||||
}
|
||||
}
|
||||
return submap_2d_ptr;
|
||||
}
|
||||
|
||||
std::shared_ptr<cartographer::mapping_3d::Submap>
|
||||
MapBuilderServer::MapBuilderContext::UpdateSubmap3D(
|
||||
const cartographer::mapping::proto::Submap& proto) {
|
||||
CHECK(proto.has_submap_3d());
|
||||
cartographer::mapping::SubmapId submap_id{proto.submap_id().trajectory_id(),
|
||||
proto.submap_id().submap_index()};
|
||||
std::shared_ptr<cartographer::mapping_3d::Submap> submap_3d_ptr;
|
||||
auto submap_it = unfinished_submaps_.find(submap_id);
|
||||
if (submap_it == unfinished_submaps_.end()) {
|
||||
// Seeing a submap for the first time it should never be finished.
|
||||
CHECK(!proto.submap_3d().finished());
|
||||
submap_3d_ptr =
|
||||
std::make_shared<cartographer::mapping_3d::Submap>(proto.submap_3d());
|
||||
unfinished_submaps_.Insert(submap_id, submap_3d_ptr);
|
||||
submap_it = unfinished_submaps_.find(submap_id);
|
||||
} else {
|
||||
submap_3d_ptr = std::dynamic_pointer_cast<cartographer::mapping_3d::Submap>(
|
||||
submap_it->data);
|
||||
CHECK(submap_3d_ptr);
|
||||
|
||||
// Update submap with information in incoming request.
|
||||
submap_3d_ptr->UpdateFromProto(proto);
|
||||
|
||||
// If the submap was just finished by the recent update, remove it from the
|
||||
// list of unfinished submaps.
|
||||
if (submap_3d_ptr->finished()) {
|
||||
unfinished_submaps_.Trim(submap_id);
|
||||
} else {
|
||||
// If the submap is unfinished set the 'num_range_data' to 0 since we
|
||||
// haven't changed the HybridGrid.
|
||||
submap_3d_ptr->SetNumRangeData(0);
|
||||
}
|
||||
}
|
||||
return submap_3d_ptr;
|
||||
}
|
||||
|
||||
std::unique_ptr<cartographer::mapping::LocalSlamResultData>
|
||||
MapBuilderServer::MapBuilderContext::ProcessLocalSlamResultData(
|
||||
const std::string& sensor_id, cartographer::common::Time time,
|
||||
const cartographer::mapping::proto::LocalSlamResultData& proto) {
|
||||
CHECK_GE(proto.submaps().size(), 0);
|
||||
CHECK(proto.submaps(0).has_submap_2d() || proto.submaps(0).has_submap_3d());
|
||||
if (proto.submaps(0).has_submap_2d()) {
|
||||
std::vector<std::shared_ptr<const cartographer::mapping_2d::Submap>>
|
||||
submaps;
|
||||
for (const auto& submap_proto : proto.submaps()) {
|
||||
submaps.push_back(UpdateSubmap2D(submap_proto));
|
||||
}
|
||||
return cartographer::common::make_unique<
|
||||
cartographer::mapping::LocalSlamResult2D>(
|
||||
sensor_id, time,
|
||||
std::make_shared<const cartographer::mapping::TrajectoryNode::Data>(
|
||||
cartographer::mapping::FromProto(proto.node_data())),
|
||||
submaps);
|
||||
} else {
|
||||
std::vector<std::shared_ptr<const cartographer::mapping_3d::Submap>>
|
||||
submaps;
|
||||
for (const auto& submap_proto : proto.submaps()) {
|
||||
submaps.push_back(UpdateSubmap3D(submap_proto));
|
||||
}
|
||||
return cartographer::common::make_unique<
|
||||
cartographer::mapping::LocalSlamResult3D>(
|
||||
sensor_id, time,
|
||||
std::make_shared<const cartographer::mapping::TrajectoryNode::Data>(
|
||||
cartographer::mapping::FromProto(proto.node_data())),
|
||||
std::move(submaps));
|
||||
}
|
||||
}
|
||||
|
||||
MapBuilderServer::MapBuilderServer(
|
||||
const proto::MapBuilderServerOptions& map_builder_server_options,
|
||||
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder)
|
||||
|
|
@ -118,6 +220,9 @@ MapBuilderServer::MapBuilderServer(
|
|||
server_builder.RegisterHandler<handlers::AddFixedFramePoseDataHandler,
|
||||
proto::MapBuilderService>(
|
||||
"AddFixedFramePoseData");
|
||||
server_builder.RegisterHandler<handlers::AddLocalSlamResultDataHandler,
|
||||
proto::MapBuilderService>(
|
||||
"AddLocalSlamResultData");
|
||||
server_builder.RegisterHandler<handlers::FinishTrajectoryHandler,
|
||||
proto::MapBuilderService>("FinishTrajectory");
|
||||
server_builder.RegisterHandler<handlers::ReceiveLocalSlamResultsHandler,
|
||||
|
|
@ -165,8 +270,8 @@ void MapBuilderServer::Shutdown() {
|
|||
void MapBuilderServer::ProcessSensorDataQueue() {
|
||||
LOG(INFO) << "Starting SLAM thread.";
|
||||
while (!shutting_down_) {
|
||||
std::unique_ptr<SensorData> sensor_data =
|
||||
sensor_data_queue_.PopWithTimeout(kPopTimeout);
|
||||
std::unique_ptr<Data> sensor_data =
|
||||
incoming_data_queue_.PopWithTimeout(kPopTimeout);
|
||||
if (sensor_data) {
|
||||
grpc_server_->GetContext<MapBuilderContext>()->AddSensorDataToTrajectory(
|
||||
*sensor_data);
|
||||
|
|
@ -232,7 +337,7 @@ void MapBuilderServer::NotifyFinishTrajectory(int trajectory_id) {
|
|||
}
|
||||
|
||||
void MapBuilderServer::WaitUntilIdle() {
|
||||
sensor_data_queue_.WaitUntilEmpty();
|
||||
incoming_data_queue_.WaitUntilEmpty();
|
||||
map_builder_->pose_graph()->RunFinalOptimization();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
|
||||
#include "cartographer/common/blocking_queue.h"
|
||||
#include "cartographer/common/time.h"
|
||||
#include "cartographer/mapping/local_slam_result_data.h"
|
||||
#include "cartographer/mapping/map_builder.h"
|
||||
#include "cartographer/mapping/trajectory_builder_interface.h"
|
||||
#include "cartographer/sensor/dispatchable.h"
|
||||
|
|
@ -42,9 +43,9 @@ class MapBuilderServer {
|
|||
// Calling with 'nullptr' signals subscribers that the subscription has ended.
|
||||
using LocalSlamSubscriptionCallback =
|
||||
std::function<void(std::unique_ptr<LocalSlamResult>)>;
|
||||
struct SensorData {
|
||||
struct Data {
|
||||
int trajectory_id;
|
||||
std::unique_ptr<cartographer::sensor::Data> sensor_data;
|
||||
std::unique_ptr<cartographer::sensor::Data> data;
|
||||
};
|
||||
struct SubscriptionId {
|
||||
const int trajectory_id;
|
||||
|
|
@ -55,28 +56,51 @@ class MapBuilderServer {
|
|||
public:
|
||||
MapBuilderContext(MapBuilderServer* map_builder_server);
|
||||
cartographer::mapping::MapBuilderInterface& map_builder();
|
||||
cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>&
|
||||
cartographer::common::BlockingQueue<std::unique_ptr<Data>>&
|
||||
sensor_data_queue();
|
||||
cartographer::mapping::TrajectoryBuilderInterface::LocalSlamResultCallback
|
||||
GetLocalSlamResultCallbackForSubscriptions();
|
||||
void AddSensorDataToTrajectory(const SensorData& sensor_data);
|
||||
void AddSensorDataToTrajectory(const Data& sensor_data);
|
||||
SubscriptionId SubscribeLocalSlamResults(
|
||||
int trajectory_id, LocalSlamSubscriptionCallback callback);
|
||||
void UnsubscribeLocalSlamResults(const SubscriptionId& subscription_id);
|
||||
void NotifyFinishTrajectory(int trajectory_id);
|
||||
std::unique_ptr<cartographer::mapping::LocalSlamResultData>
|
||||
ProcessLocalSlamResultData(
|
||||
const std::string& sensor_id, cartographer::common::Time time,
|
||||
const cartographer::mapping::proto::LocalSlamResultData& proto);
|
||||
|
||||
template <typename SensorDataType>
|
||||
template <typename DataType>
|
||||
void EnqueueSensorData(int trajectory_id, const std::string& sensor_id,
|
||||
const SensorDataType& sensor_data) {
|
||||
map_builder_server_->sensor_data_queue_.Push(
|
||||
cartographer::common::make_unique<MapBuilderServer::SensorData>(
|
||||
MapBuilderServer::SensorData{
|
||||
trajectory_id, cartographer::sensor::MakeDispatchable(
|
||||
sensor_id, sensor_data)}));
|
||||
const DataType& data) {
|
||||
map_builder_server_->incoming_data_queue_.Push(
|
||||
cartographer::common::make_unique<MapBuilderServer::Data>(
|
||||
MapBuilderServer::Data{
|
||||
trajectory_id,
|
||||
cartographer::sensor::MakeDispatchable(sensor_id, data)}));
|
||||
}
|
||||
|
||||
void EnqueueLocalSlamResultData(
|
||||
int trajectory_id, const std::string& sensor_id,
|
||||
std::unique_ptr<cartographer::mapping::LocalSlamResultData>
|
||||
local_slam_result_data) {
|
||||
map_builder_server_->incoming_data_queue_.Push(
|
||||
cartographer::common::make_unique<MapBuilderServer::Data>(
|
||||
MapBuilderServer::Data{trajectory_id,
|
||||
std::move(local_slam_result_data)}));
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<cartographer::mapping_2d::Submap> UpdateSubmap2D(
|
||||
const cartographer::mapping::proto::Submap& proto);
|
||||
std::shared_ptr<cartographer::mapping_3d::Submap> UpdateSubmap3D(
|
||||
const cartographer::mapping::proto::Submap& proto);
|
||||
|
||||
MapBuilderServer* map_builder_server_;
|
||||
cartographer::mapping::MapById<
|
||||
cartographer::mapping::SubmapId,
|
||||
std::shared_ptr<cartographer::mapping::Submap>>
|
||||
unfinished_submaps_;
|
||||
};
|
||||
friend MapBuilderContext;
|
||||
|
||||
|
|
@ -120,8 +144,8 @@ class MapBuilderServer {
|
|||
std::unique_ptr<std::thread> slam_thread_;
|
||||
std::unique_ptr<framework::Server> grpc_server_;
|
||||
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
|
||||
cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>
|
||||
sensor_data_queue_;
|
||||
cartographer::common::BlockingQueue<std::unique_ptr<Data>>
|
||||
incoming_data_queue_;
|
||||
cartographer::common::Mutex local_slam_subscriptions_lock_;
|
||||
int current_subscription_index_ = 0;
|
||||
std::map<int /* trajectory ID */, LocalSlamResultHandlerSubscriptions>
|
||||
|
|
|
|||
|
|
@ -15,6 +15,7 @@
|
|||
syntax = "proto3";
|
||||
|
||||
import "cartographer/mapping/proto/pose_graph.proto";
|
||||
import "cartographer/mapping/proto/serialization.proto";
|
||||
import "cartographer/mapping/proto/submap_visualization.proto";
|
||||
import "cartographer/mapping/proto/trajectory_builder_options.proto";
|
||||
import "cartographer/sensor/proto/sensor.proto";
|
||||
|
|
@ -119,6 +120,11 @@ message GetConstraintsResponse {
|
|||
repeated cartographer.mapping.proto.PoseGraph.Constraint constraints = 1;
|
||||
}
|
||||
|
||||
message AddLocalSlamResultDataRequest {
|
||||
SensorMetadata sensor_metadata = 1;
|
||||
cartographer.mapping.proto.LocalSlamResultData local_slam_result_data = 2;
|
||||
}
|
||||
|
||||
service MapBuilderService {
|
||||
// Starts a new trajectory and returns its index.
|
||||
rpc AddTrajectory(AddTrajectoryRequest) returns (AddTrajectoryResponse);
|
||||
|
|
@ -139,6 +145,10 @@ service MapBuilderService {
|
|||
rpc AddFixedFramePoseData(stream AddFixedFramePoseDataRequest)
|
||||
returns (google.protobuf.Empty);
|
||||
|
||||
// Adds a local SLAM result.
|
||||
rpc AddLocalSlamResultData(stream AddLocalSlamResultDataRequest)
|
||||
returns (google.protobuf.Empty);
|
||||
|
||||
// Requests the server to send a stream of local SLAM results for the given
|
||||
// 'trajectory_id'.
|
||||
rpc ReceiveLocalSlamResults(ReceiveLocalSlamResultsRequest)
|
||||
|
|
|
|||
Loading…
Reference in New Issue