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

Update unfinished submap list on SLAM thread (#1061) 

Fixes #1052
master
Christoph Schütte 2018-04-13 12:15:26 +02:00 committed by Wally B. Feed
parent b5e5477f75
commit b4c4ae6ea9
12 changed files with 79 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cartographer/cloud/internal/handlers/add_local_slam_result_data_handler.cc
View File

@ -31,14 +31,9 @@ namespace handlers {
void AddLocalSlamResultDataHandler::OnRequest(
const proto::AddLocalSlamResultDataRequest& request) {
auto local_slam_result_data =
GetContext<MapBuilderContextInterface>()->ProcessLocalSlamResultData(
request.sensor_metadata().sensor_id(),
common::FromUniversal(request.local_slam_result_data().timestamp()),
request.local_slam_result_data());
GetContext<MapBuilderContextInterface>()->EnqueueLocalSlamResultData(
request.sensor_metadata().trajectory_id(),
request.sensor_metadata().sensor_id(), std::move(local_slam_result_data));
request.sensor_metadata().sensor_id(), request.local_slam_result_data());
}
void AddLocalSlamResultDataHandler::OnReadsDone() {

44
cartographer/cloud/internal/map_builder_context_impl.cc
View File

@ -24,39 +24,23 @@ namespace cartographer {
namespace cloud {
template <>
std::unique_ptr<mapping::LocalSlamResultData>
MapBuilderContext<mapping::Submap2D>::ProcessLocalSlamResultData(
const std::string& sensor_id, common::Time time,
const mapping::proto::LocalSlamResultData& proto) {
CHECK_GE(proto.submaps().size(), 1);
CHECK(proto.submaps(0).has_submap_2d());
std::vector<std::shared_ptr<const mapping::Submap2D>> submaps;
for (const auto& submap_proto : proto.submaps()) {
submaps.push_back(submap_controller_.UpdateSubmap(submap_proto));
}
return common::make_unique<mapping::LocalSlamResult2D>(
sensor_id, time,
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::FromProto(proto.node_data())),
submaps);
void MapBuilderContext<mapping::Submap2D>::EnqueueLocalSlamResultData(
int trajectory_id, const std::string& sensor_id,
const mapping::proto::LocalSlamResultData& local_slam_result_data) {
map_builder_server_->incoming_data_queue_.Push(common::make_unique<Data>(
Data{trajectory_id,
common::make_unique<mapping::LocalSlamResult2D>(
sensor_id, local_slam_result_data, &submap_controller_)}));
}
template <>
std::unique_ptr<mapping::LocalSlamResultData>
MapBuilderContext<mapping::Submap3D>::ProcessLocalSlamResultData(
const std::string& sensor_id, common::Time time,
const mapping::proto::LocalSlamResultData& proto) {
CHECK_GE(proto.submaps().size(), 1);
CHECK(proto.submaps(0).has_submap_3d());
std::vector<std::shared_ptr<const mapping::Submap3D>> submaps;
for (const auto& submap_proto : proto.submaps()) {
submaps.push_back(submap_controller_.UpdateSubmap(submap_proto));
}
return common::make_unique<mapping::LocalSlamResult3D>(
sensor_id, time,
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::FromProto(proto.node_data())),
submaps);
void MapBuilderContext<mapping::Submap3D>::EnqueueLocalSlamResultData(
int trajectory_id, const std::string& sensor_id,
const mapping::proto::LocalSlamResultData& local_slam_result_data) {
map_builder_server_->incoming_data_queue_.Push(common::make_unique<Data>(
Data{trajectory_id,
common::make_unique<mapping::LocalSlamResult3D>(
sensor_id, local_slam_result_data, &submap_controller_)}));
}
} // namespace cloud

22
cartographer/cloud/internal/map_builder_context_impl.h
View File

@ -92,24 +92,14 @@ void MapBuilderContext<SubmapType>::EnqueueSensorData(
common::make_unique<Data>(Data{trajectory_id, std::move(data)}));
}
template <class SubmapType>
void MapBuilderContext<SubmapType>::EnqueueLocalSlamResultData(
template <>
void MapBuilderContext<mapping::Submap2D>::EnqueueLocalSlamResultData(
int trajectory_id, const std::string& sensor_id,
std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) {
map_builder_server_->incoming_data_queue_.Push(common::make_unique<Data>(
Data{trajectory_id, std::move(local_slam_result_data)}));
}
const mapping::proto::LocalSlamResultData& local_slam_result_data);
template <>
std::unique_ptr<mapping::LocalSlamResultData>
MapBuilderContext<mapping::Submap2D>::ProcessLocalSlamResultData(
const std::string& sensor_id, common::Time time,
const mapping::proto::LocalSlamResultData& proto);
template <>
std::unique_ptr<mapping::LocalSlamResultData>
MapBuilderContext<mapping::Submap3D>::ProcessLocalSlamResultData(
const std::string& sensor_id, common::Time time,
const mapping::proto::LocalSlamResultData& proto);
void MapBuilderContext<mapping::Submap3D>::EnqueueLocalSlamResultData(
int trajectory_id, const std::string& sensor_id,
const mapping::proto::LocalSlamResultData& local_slam_result_data);
} // namespace cloud
} // namespace cartographer

7
cartographer/cloud/internal/map_builder_context_interface.h
View File

@ -21,6 +21,7 @@
#include "cartographer/cloud/internal/local_trajectory_uploader.h"
#include "cartographer/common/blocking_queue.h"
#include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/proto/serialization.pb.h"
#include "cartographer/sensor/data.h"
#include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h"
@ -68,16 +69,12 @@ class MapBuilderContextInterface : public async_grpc::ExecutionContext {
virtual void UnsubscribeLocalSlamResults(
const SubscriptionId& subscription_id) = 0;
virtual void NotifyFinishTrajectory(int trajectory_id) = 0;
virtual std::unique_ptr<mapping::LocalSlamResultData>
ProcessLocalSlamResultData(
const std::string& sensor_id, common::Time time,
const mapping::proto::LocalSlamResultData& proto) = 0;
virtual LocalTrajectoryUploaderInterface* local_trajectory_uploader() = 0;
virtual void EnqueueSensorData(int trajectory_id,
std::unique_ptr<sensor::Data> data) = 0;
virtual void EnqueueLocalSlamResultData(
int trajectory_id, const std::string& sensor_id,
std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) = 0;
const mapping::proto::LocalSlamResultData& local_slam_result_data) = 0;
};
} // namespace cloud

10
cartographer/cloud/internal/map_builder_server.h
View File

@ -21,13 +21,13 @@
#include "async_grpc/server.h"
#include "cartographer/cloud/internal/local_trajectory_uploader.h"
#include "cartographer/cloud/internal/map_builder_context_interface.h"
#include "cartographer/cloud/internal/submap_controller.h"
#include "cartographer/cloud/map_builder_server_interface.h"
#include "cartographer/cloud/proto/map_builder_server_options.pb.h"
#include "cartographer/common/blocking_queue.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/3d/submap_3d.h"
#include "cartographer/mapping/internal/submap_controller.h"
#include "cartographer/mapping/local_slam_result_data.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
@ -53,21 +53,17 @@ class MapBuilderContext : public MapBuilderContextInterface {
void UnsubscribeLocalSlamResults(
const SubscriptionId& subscription_id) override;
void NotifyFinishTrajectory(int trajectory_id) override;
std::unique_ptr<mapping::LocalSlamResultData> ProcessLocalSlamResultData(
const std::string& sensor_id, common::Time time,
const mapping::proto::LocalSlamResultData& proto) override;
LocalTrajectoryUploaderInterface* local_trajectory_uploader() override;
void EnqueueSensorData(int trajectory_id,
std::unique_ptr<sensor::Data> data) override;
void EnqueueLocalSlamResultData(int trajectory_id,
const std::string& sensor_id,
std::unique_ptr<mapping::LocalSlamResultData>
const mapping::proto::LocalSlamResultData&
local_slam_result_data) override;
private:
MapBuilderServer* map_builder_server_;
SubmapController<SubmapType> submap_controller_;
mapping::SubmapController<SubmapType> submap_controller_;
};
class MapBuilderServer : public MapBuilderServerInterface {

22
cartographer/cloud/internal/testing/mock_map_builder_context.h
View File

@ -45,16 +45,6 @@ class MockMapBuilderContext : public MapBuilderContextInterface {
MOCK_METHOD1(UnsubscribeLocalSlamResults,
void(const MapBuilderContextInterface::SubscriptionId &));
MOCK_METHOD1(NotifyFinishTrajectory, void(int));
MOCK_METHOD3(DoProcessLocalSlamResultData,
mapping::LocalSlamResultData *(
const std::string &, common::Time,
const mapping::proto::LocalSlamResultData &));
std::unique_ptr<mapping::LocalSlamResultData> ProcessLocalSlamResultData(
const std::string &sensor_id, common::Time time,
const mapping::proto::LocalSlamResultData &proto) override {
return std::unique_ptr<mapping::LocalSlamResultData>(
DoProcessLocalSlamResultData(sensor_id, time, proto));
}
MOCK_METHOD0(local_trajectory_uploader, LocalTrajectoryUploaderInterface *());
MOCK_METHOD2(DoEnqueueSensorData, void(int, sensor::Data *));
@ -62,15 +52,9 @@ class MockMapBuilderContext : public MapBuilderContextInterface {
std::unique_ptr<sensor::Data> data) override {
DoEnqueueSensorData(trajectory_id, data.get());
}
MOCK_METHOD3(DoEnqueueLocalSlamResultData,
void(int, const std::string &, mapping::LocalSlamResultData *));
void EnqueueLocalSlamResultData(int trajectory_id,
const std::string &sensor_id,
std::unique_ptr<mapping::LocalSlamResultData>
local_slam_result_data) override {
DoEnqueueLocalSlamResultData(trajectory_id, sensor_id,
local_slam_result_data.get());
}
MOCK_METHOD3(EnqueueLocalSlamResultData,
void(int, const std::string &,
const mapping::proto::LocalSlamResultData &));
};
} // namespace testing

10
cartographer/mapping/internal/2d/local_slam_result_2d.cc
View File

@ -29,8 +29,16 @@ void LocalSlamResult2D::AddToTrajectoryBuilder(
void LocalSlamResult2D::AddToPoseGraph(int trajectory_id,
PoseGraph* pose_graph) const {
DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph));
CHECK_GE(local_slam_result_data_.submaps().size(), 1);
CHECK(local_slam_result_data_.submaps(0).has_submap_2d());
std::vector<std::shared_ptr<const mapping::Submap2D>> submaps;
for (const auto& submap_proto : local_slam_result_data_.submaps()) {
submaps.push_back(submap_controller_->UpdateSubmap(submap_proto));
}
static_cast<PoseGraph2D*>(pose_graph)
->AddNode(node_data_, trajectory_id, insertion_submaps_);
->AddNode(std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::FromProto(local_slam_result_data_.node_data())),
trajectory_id, submaps);
}
} // namespace mapping

21
cartographer/mapping/internal/2d/local_slam_result_2d.h
View File

@ -17,7 +17,7 @@
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_SLAM_RESULT_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_SLAM_RESULT_2D_H_
#include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/internal/submap_controller.h"
#include "cartographer/mapping/local_slam_result_data.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
@ -27,20 +27,23 @@ namespace mapping {
class LocalSlamResult2D : public LocalSlamResultData {
public:
LocalSlamResult2D(
const std::string& sensor_id, common::Time time,
std::shared_ptr<const TrajectoryNode::Data> node_data,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps)
: LocalSlamResultData(sensor_id, time),
node_data_(node_data),
insertion_submaps_(insertion_submaps) {}
const std::string& sensor_id,
const mapping::proto::LocalSlamResultData local_slam_result_data,
SubmapController<mapping::Submap2D>* submap_controller)
: LocalSlamResultData(sensor_id, common::FromUniversal(
local_slam_result_data.timestamp())),
sensor_id_(sensor_id),
local_slam_result_data_(local_slam_result_data),
submap_controller_(submap_controller) {}
void AddToTrajectoryBuilder(
TrajectoryBuilderInterface* const trajectory_builder) override;
void AddToPoseGraph(int trajectory_id, PoseGraph* pose_graph) const override;
private:
std::shared_ptr<const TrajectoryNode::Data> node_data_;
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps_;
const std::string sensor_id_;
const mapping::proto::LocalSlamResultData local_slam_result_data_;
SubmapController<mapping::Submap2D>* submap_controller_;
};
} // namespace mapping

10
cartographer/mapping/internal/3d/local_slam_result_3d.cc
View File

@ -29,8 +29,16 @@ void LocalSlamResult3D::AddToTrajectoryBuilder(
void LocalSlamResult3D::AddToPoseGraph(int trajectory_id,
PoseGraph* pose_graph) const {
DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph));
CHECK_GE(local_slam_result_data_.submaps().size(), 1);
CHECK(local_slam_result_data_.submaps(0).has_submap_3d());
std::vector<std::shared_ptr<const mapping::Submap3D>> submaps;
for (const auto& submap_proto : local_slam_result_data_.submaps()) {
submaps.push_back(submap_controller_->UpdateSubmap(submap_proto));
}
static_cast<PoseGraph3D*>(pose_graph)
->AddNode(node_data_, trajectory_id, insertion_submaps_);
->AddNode(std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::FromProto(local_slam_result_data_.node_data())),
trajectory_id, submaps);
}
} // namespace mapping

21
cartographer/mapping/internal/3d/local_slam_result_3d.h
View File

@ -17,7 +17,7 @@
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_SLAM_RESULT_3D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_SLAM_RESULT_3D_H_
#include "cartographer/mapping/3d/submap_3d.h"
#include "cartographer/mapping/internal/submap_controller.h"
#include "cartographer/mapping/local_slam_result_data.h"
#include "cartographer/mapping/trajectory_builder_interface.h"
@ -27,20 +27,23 @@ namespace mapping {
class LocalSlamResult3D : public LocalSlamResultData {
public:
LocalSlamResult3D(
const std::string& sensor_id, common::Time time,
std::shared_ptr<const TrajectoryNode::Data> node_data,
const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps)
: LocalSlamResultData(sensor_id, time),
node_data_(node_data),
insertion_submaps_(insertion_submaps) {}
const std::string& sensor_id,
const mapping::proto::LocalSlamResultData local_slam_result_data,
SubmapController<mapping::Submap3D>* submap_controller)
: LocalSlamResultData(sensor_id, common::FromUniversal(
local_slam_result_data.timestamp())),
sensor_id_(sensor_id),
local_slam_result_data_(local_slam_result_data),
submap_controller_(submap_controller) {}
void AddToTrajectoryBuilder(
TrajectoryBuilderInterface* const trajectory_builder) override;
void AddToPoseGraph(int trajectory_id, PoseGraph* pose_graph) const override;
private:
std::shared_ptr<const TrajectoryNode::Data> node_data_;
std::vector<std::shared_ptr<const Submap3D>> insertion_submaps_;
const std::string sensor_id_;
const mapping::proto::LocalSlamResultData local_slam_result_data_;
SubmapController<mapping::Submap3D>* submap_controller_;
};
} // namespace mapping

6
cartographer/cloud/internal/submap_controller.cc → cartographer/mapping/internal/submap_controller.cc
View File

@ -14,10 +14,10 @@
* limitations under the License.
*/
#include "cartographer/cloud/internal/submap_controller.h"
#include "cartographer/mapping/internal/submap_controller.h"
namespace cartographer {
namespace cloud {
namespace mapping {
template <>
std::shared_ptr<mapping::Submap2D>
@ -33,5 +33,5 @@ SubmapController<mapping::Submap3D>::CreateSubmap(
return std::make_shared<mapping::Submap3D>(proto.submap_3d());
}
} // namespace cloud
} // namespace mapping
} // namespace cartographer

10
cartographer/cloud/internal/submap_controller.h → cartographer/mapping/internal/submap_controller.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_CLOUD_INTERNAL_SUBMAP_CONTROLLER_H
#define CARTOGRAPHER_CLOUD_INTERNAL_SUBMAP_CONTROLLER_H
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_SUBMAP_CONTROLLER_H
#define CARTOGRAPHER_MAPPING_INTERNAL_SUBMAP_CONTROLLER_H
#include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/3d/submap_3d.h"
@ -23,7 +23,7 @@
#include "cartographer/mapping/proto/serialization.pb.h"
namespace cartographer {
namespace cloud {
namespace mapping {
template <class SubmapType>
class SubmapController {
@ -71,7 +71,7 @@ std::shared_ptr<mapping::Submap3D>
SubmapController<mapping::Submap3D>::CreateSubmap(
const mapping::proto::Submap& proto);
} // namespace cloud
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_CLOUD_INTERNAL_SUBMAP_CONTROLLER_H
#endif // CARTOGRAPHER_MAPPING_INTERNAL_SUBMAP_CONTROLLER_H