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

PopWithTimeout for sensor data queue (#763) 

PopWithTimeout is necessary to ensure the SLAM thread
in the MapBuilderServer finishes when the server is
shut down.
master
gaschler 2017-12-18 10:02:57 +01:00 committed by Wally B. Feed
parent d49706944f
commit 3fbc642a89
2 changed files with 36 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cartographer_grpc/map_builder_server.cc
View File

@ -26,10 +26,17 @@
#include "glog/logging.h"
namespace cartographer_grpc {
namespace {
const cartographer::common::Duration kPopTimeout =
cartographer::common::FromMilliseconds(100);
} // namespace
MapBuilderServer::MapBuilderContext::MapBuilderContext(
cartographer::mapping::MapBuilder* map_builder,
cartographer::common::BlockingQueue<SensorData>* sensor_data_queue)
cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>*
sensor_data_queue)
: map_builder_(map_builder), sensor_data_queue_(sensor_data_queue) {}
cartographer::mapping::MapBuilder&
@ -37,7 +44,8 @@ MapBuilderServer::MapBuilderContext::map_builder() {
return *map_builder_;
}
cartographer::common::BlockingQueue<MapBuilderServer::SensorData>&
cartographer::common::BlockingQueue<
std::unique_ptr<MapBuilderServer::SensorData>>&
MapBuilderServer::MapBuilderContext::sensor_data_queue() {
return *sensor_data_queue_;
}
@ -102,9 +110,12 @@ void MapBuilderServer::Shutdown() {
void MapBuilderServer::ProcessSensorDataQueue() {
LOG(INFO) << "Starting SLAM thread.";
while (!shutting_down_) {
SensorData sensor_data = sensor_data_queue_.Pop();
std::unique_ptr<SensorData> sensor_data =
sensor_data_queue_.PopWithTimeout(kPopTimeout);
if (sensor_data) {
grpc_server_->GetContext<MapBuilderContext>()->AddSensorDataToTrajectory(
sensor_data);
*sensor_data);
}
}
}

20
cartographer_grpc/map_builder_server.h
View File

@ -37,22 +37,27 @@ class MapBuilderServer {
public:
MapBuilderContext(
cartographer::mapping::MapBuilder* map_builder,
cartographer::common::BlockingQueue<SensorData> *sensor_data_queue);
cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>*
sensor_data_queue);
cartographer::mapping::MapBuilder& map_builder();
cartographer::common::BlockingQueue<SensorData> &sensor_data_queue();
cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>&
sensor_data_queue();
void AddSensorDataToTrajectory(const SensorData& sensor_data);
template <typename SensorDataType>
void EnqueueSensorData(int trajectory_id, const std::string& sensor_id,
const SensorDataType& sensor_data) {
sensor_data_queue_->Push(MapBuilderServer::SensorData{
trajectory_id,
cartographer::sensor::MakeDispatchable(sensor_id, sensor_data)});
sensor_data_queue_->Push(
cartographer::common::make_unique<MapBuilderServer::SensorData>(
MapBuilderServer::SensorData{
trajectory_id, cartographer::sensor::MakeDispatchable(
sensor_id, sensor_data)}));
}
private:
cartographer::mapping::MapBuilder* map_builder_;
cartographer::common::BlockingQueue<SensorData> *sensor_data_queue_;
cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>*
sensor_data_queue_;
};
MapBuilderServer(
@ -77,7 +82,8 @@ class MapBuilderServer {
std::unique_ptr<std::thread> slam_thread_;
std::unique_ptr<framework::Server> grpc_server_;
cartographer::mapping::MapBuilder map_builder_;
cartographer::common::BlockingQueue<SensorData> sensor_data_queue_;
cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>
sensor_data_queue_;
};
} // namespace cartographer_grpc