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( void AddLocalSlamResultDataHandler::OnRequest(
const proto::AddLocalSlamResultDataRequest& request) { 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( GetContext<MapBuilderContextInterface>()->EnqueueLocalSlamResultData(
request.sensor_metadata().trajectory_id(), 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() { void AddLocalSlamResultDataHandler::OnReadsDone() {

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

@ -24,39 +24,23 @@ namespace cartographer {
namespace cloud { namespace cloud {
template <> template <>
std::unique_ptr<mapping::LocalSlamResultData> void MapBuilderContext<mapping::Submap2D>::EnqueueLocalSlamResultData(
MapBuilderContext<mapping::Submap2D>::ProcessLocalSlamResultData( int trajectory_id, const std::string& sensor_id,
const std::string& sensor_id, common::Time time, const mapping::proto::LocalSlamResultData& local_slam_result_data) {
const mapping::proto::LocalSlamResultData& proto) { map_builder_server_->incoming_data_queue_.Push(common::make_unique<Data>(
CHECK_GE(proto.submaps().size(), 1); Data{trajectory_id,
CHECK(proto.submaps(0).has_submap_2d()); common::make_unique<mapping::LocalSlamResult2D>(
std::vector<std::shared_ptr<const mapping::Submap2D>> submaps; sensor_id, local_slam_result_data, &submap_controller_)}));
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);
} }
template <> template <>
std::unique_ptr<mapping::LocalSlamResultData> void MapBuilderContext<mapping::Submap3D>::EnqueueLocalSlamResultData(
MapBuilderContext<mapping::Submap3D>::ProcessLocalSlamResultData( int trajectory_id, const std::string& sensor_id,
const std::string& sensor_id, common::Time time, const mapping::proto::LocalSlamResultData& local_slam_result_data) {
const mapping::proto::LocalSlamResultData& proto) { map_builder_server_->incoming_data_queue_.Push(common::make_unique<Data>(
CHECK_GE(proto.submaps().size(), 1); Data{trajectory_id,
CHECK(proto.submaps(0).has_submap_3d()); common::make_unique<mapping::LocalSlamResult3D>(
std::vector<std::shared_ptr<const mapping::Submap3D>> submaps; sensor_id, local_slam_result_data, &submap_controller_)}));
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);
} }
} // namespace cloud } // 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)})); common::make_unique<Data>(Data{trajectory_id, std::move(data)}));
} }
template <class SubmapType> template <>
void MapBuilderContext<SubmapType>::EnqueueLocalSlamResultData( void MapBuilderContext<mapping::Submap2D>::EnqueueLocalSlamResultData(
int trajectory_id, const std::string& sensor_id, int trajectory_id, const std::string& sensor_id,
std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) { const mapping::proto::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)}));
}
template <> template <>
std::unique_ptr<mapping::LocalSlamResultData> void MapBuilderContext<mapping::Submap3D>::EnqueueLocalSlamResultData(
MapBuilderContext<mapping::Submap2D>::ProcessLocalSlamResultData( int trajectory_id, const std::string& sensor_id,
const std::string& sensor_id, common::Time time, const mapping::proto::LocalSlamResultData& local_slam_result_data);
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);
} // namespace cloud } // namespace cloud
} // namespace cartographer } // 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/cloud/internal/local_trajectory_uploader.h"
#include "cartographer/common/blocking_queue.h" #include "cartographer/common/blocking_queue.h"
#include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/proto/serialization.pb.h"
#include "cartographer/sensor/data.h" #include "cartographer/sensor/data.h"
#include "cartographer/sensor/range_data.h" #include "cartographer/sensor/range_data.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
@ -68,16 +69,12 @@ class MapBuilderContextInterface : public async_grpc::ExecutionContext {
virtual void UnsubscribeLocalSlamResults( virtual void UnsubscribeLocalSlamResults(
const SubscriptionId& subscription_id) = 0; const SubscriptionId& subscription_id) = 0;
virtual void NotifyFinishTrajectory(int trajectory_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 LocalTrajectoryUploaderInterface* local_trajectory_uploader() = 0;
virtual void EnqueueSensorData(int trajectory_id, virtual void EnqueueSensorData(int trajectory_id,
std::unique_ptr<sensor::Data> data) = 0; std::unique_ptr<sensor::Data> data) = 0;
virtual void EnqueueLocalSlamResultData( virtual void EnqueueLocalSlamResultData(
int trajectory_id, const std::string& sensor_id, 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 } // namespace cloud

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

@ -21,13 +21,13 @@
#include "async_grpc/server.h" #include "async_grpc/server.h"
#include "cartographer/cloud/internal/local_trajectory_uploader.h" #include "cartographer/cloud/internal/local_trajectory_uploader.h"
#include "cartographer/cloud/internal/map_builder_context_interface.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/map_builder_server_interface.h"
#include "cartographer/cloud/proto/map_builder_server_options.pb.h" #include "cartographer/cloud/proto/map_builder_server_options.pb.h"
#include "cartographer/common/blocking_queue.h" #include "cartographer/common/blocking_queue.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/3d/submap_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/local_slam_result_data.h"
#include "cartographer/mapping/map_builder.h" #include "cartographer/mapping/map_builder.h"
#include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/mapping/trajectory_builder_interface.h"
@ -53,21 +53,17 @@ class MapBuilderContext : public MapBuilderContextInterface {
void UnsubscribeLocalSlamResults( void UnsubscribeLocalSlamResults(
const SubscriptionId& subscription_id) override; const SubscriptionId& subscription_id) override;
void NotifyFinishTrajectory(int trajectory_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; LocalTrajectoryUploaderInterface* local_trajectory_uploader() override;
void EnqueueSensorData(int trajectory_id, void EnqueueSensorData(int trajectory_id,
std::unique_ptr<sensor::Data> data) override; std::unique_ptr<sensor::Data> data) override;
void EnqueueLocalSlamResultData(int trajectory_id, void EnqueueLocalSlamResultData(int trajectory_id,
const std::string& sensor_id, const std::string& sensor_id,
std::unique_ptr<mapping::LocalSlamResultData> const mapping::proto::LocalSlamResultData&
local_slam_result_data) override; local_slam_result_data) override;
private: private:
MapBuilderServer* map_builder_server_; MapBuilderServer* map_builder_server_;
SubmapController<SubmapType> submap_controller_; mapping::SubmapController<SubmapType> submap_controller_;
}; };
class MapBuilderServer : public MapBuilderServerInterface { 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, MOCK_METHOD1(UnsubscribeLocalSlamResults,
void(const MapBuilderContextInterface::SubscriptionId &)); void(const MapBuilderContextInterface::SubscriptionId &));
MOCK_METHOD1(NotifyFinishTrajectory, void(int)); 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_METHOD0(local_trajectory_uploader, LocalTrajectoryUploaderInterface *());
MOCK_METHOD2(DoEnqueueSensorData, void(int, sensor::Data *)); MOCK_METHOD2(DoEnqueueSensorData, void(int, sensor::Data *));
@ -62,15 +52,9 @@ class MockMapBuilderContext : public MapBuilderContextInterface {
std::unique_ptr<sensor::Data> data) override { std::unique_ptr<sensor::Data> data) override {
DoEnqueueSensorData(trajectory_id, data.get()); DoEnqueueSensorData(trajectory_id, data.get());
} }
MOCK_METHOD3(DoEnqueueLocalSlamResultData, MOCK_METHOD3(EnqueueLocalSlamResultData,
void(int, const std::string &, mapping::LocalSlamResultData *)); void(int, const std::string &,
void EnqueueLocalSlamResultData(int trajectory_id, const mapping::proto::LocalSlamResultData &));
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());
}
}; };
} // namespace testing } // 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, void LocalSlamResult2D::AddToPoseGraph(int trajectory_id,
PoseGraph* pose_graph) const { PoseGraph* pose_graph) const {
DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph)); 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) 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 } // 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_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_SLAM_RESULT_2D_H_
#define 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/local_slam_result_data.h"
#include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/mapping/trajectory_builder_interface.h"
@ -27,20 +27,23 @@ namespace mapping {
class LocalSlamResult2D : public LocalSlamResultData { class LocalSlamResult2D : public LocalSlamResultData {
public: public:
LocalSlamResult2D( LocalSlamResult2D(
const std::string& sensor_id, common::Time time, const std::string& sensor_id,
std::shared_ptr<const TrajectoryNode::Data> node_data, const mapping::proto::LocalSlamResultData local_slam_result_data,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) SubmapController<mapping::Submap2D>* submap_controller)
: LocalSlamResultData(sensor_id, time), : LocalSlamResultData(sensor_id, common::FromUniversal(
node_data_(node_data), local_slam_result_data.timestamp())),
insertion_submaps_(insertion_submaps) {} sensor_id_(sensor_id),
local_slam_result_data_(local_slam_result_data),
submap_controller_(submap_controller) {}
void AddToTrajectoryBuilder( void AddToTrajectoryBuilder(
TrajectoryBuilderInterface* const trajectory_builder) override; TrajectoryBuilderInterface* const trajectory_builder) override;
void AddToPoseGraph(int trajectory_id, PoseGraph* pose_graph) const override; void AddToPoseGraph(int trajectory_id, PoseGraph* pose_graph) const override;
private: private:
std::shared_ptr<const TrajectoryNode::Data> node_data_; const std::string sensor_id_;
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps_; const mapping::proto::LocalSlamResultData local_slam_result_data_;
SubmapController<mapping::Submap2D>* submap_controller_;
}; };
} // namespace mapping } // 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, void LocalSlamResult3D::AddToPoseGraph(int trajectory_id,
PoseGraph* pose_graph) const { PoseGraph* pose_graph) const {
DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph)); 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) 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 } // 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_ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_SLAM_RESULT_3D_H_
#define 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/local_slam_result_data.h"
#include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/mapping/trajectory_builder_interface.h"
@ -27,20 +27,23 @@ namespace mapping {
class LocalSlamResult3D : public LocalSlamResultData { class LocalSlamResult3D : public LocalSlamResultData {
public: public:
LocalSlamResult3D( LocalSlamResult3D(
const std::string& sensor_id, common::Time time, const std::string& sensor_id,
std::shared_ptr<const TrajectoryNode::Data> node_data, const mapping::proto::LocalSlamResultData local_slam_result_data,
const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps) SubmapController<mapping::Submap3D>* submap_controller)
: LocalSlamResultData(sensor_id, time), : LocalSlamResultData(sensor_id, common::FromUniversal(
node_data_(node_data), local_slam_result_data.timestamp())),
insertion_submaps_(insertion_submaps) {} sensor_id_(sensor_id),
local_slam_result_data_(local_slam_result_data),
submap_controller_(submap_controller) {}
void AddToTrajectoryBuilder( void AddToTrajectoryBuilder(
TrajectoryBuilderInterface* const trajectory_builder) override; TrajectoryBuilderInterface* const trajectory_builder) override;
void AddToPoseGraph(int trajectory_id, PoseGraph* pose_graph) const override; void AddToPoseGraph(int trajectory_id, PoseGraph* pose_graph) const override;
private: private:
std::shared_ptr<const TrajectoryNode::Data> node_data_; const std::string sensor_id_;
std::vector<std::shared_ptr<const Submap3D>> insertion_submaps_; const mapping::proto::LocalSlamResultData local_slam_result_data_;
SubmapController<mapping::Submap3D>* submap_controller_;
}; };
} // namespace mapping } // 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. * limitations under the License.
*/ */
#include "cartographer/cloud/internal/submap_controller.h" #include "cartographer/mapping/internal/submap_controller.h"
namespace cartographer { namespace cartographer {
namespace cloud { namespace mapping {
template <> template <>
std::shared_ptr<mapping::Submap2D> std::shared_ptr<mapping::Submap2D>
@ -33,5 +33,5 @@ SubmapController<mapping::Submap3D>::CreateSubmap(
return std::make_shared<mapping::Submap3D>(proto.submap_3d()); return std::make_shared<mapping::Submap3D>(proto.submap_3d());
} }
} // namespace cloud } // namespace mapping
} // namespace cartographer } // 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. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_CLOUD_INTERNAL_SUBMAP_CONTROLLER_H #ifndef CARTOGRAPHER_MAPPING_INTERNAL_SUBMAP_CONTROLLER_H
#define CARTOGRAPHER_CLOUD_INTERNAL_SUBMAP_CONTROLLER_H #define CARTOGRAPHER_MAPPING_INTERNAL_SUBMAP_CONTROLLER_H
#include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/3d/submap_3d.h" #include "cartographer/mapping/3d/submap_3d.h"
@ -23,7 +23,7 @@
#include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/serialization.pb.h"
namespace cartographer { namespace cartographer {
namespace cloud { namespace mapping {
template <class SubmapType> template <class SubmapType>
class SubmapController { class SubmapController {
@ -71,7 +71,7 @@ std::shared_ptr<mapping::Submap3D>
SubmapController<mapping::Submap3D>::CreateSubmap( SubmapController<mapping::Submap3D>::CreateSubmap(
const mapping::proto::Submap& proto); const mapping::proto::Submap& proto);
} // namespace cloud } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_CLOUD_INTERNAL_SUBMAP_CONTROLLER_H #endif // CARTOGRAPHER_MAPPING_INTERNAL_SUBMAP_CONTROLLER_H