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

34
cartographer_grpc/map_builder_server.h
View File

@ -36,27 +36,32 @@ class MapBuilderServer {
class MapBuilderContext : public framework::ExecutionContext { class MapBuilderContext : public framework::ExecutionContext {
public: public:
MapBuilderContext( MapBuilderContext(
cartographer::mapping::MapBuilder *map_builder, cartographer::mapping::MapBuilder* map_builder,
cartographer::common::BlockingQueue<SensorData> *sensor_data_queue); cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>*
cartographer::mapping::MapBuilder &map_builder(); sensor_data_queue);
cartographer::common::BlockingQueue<SensorData> &sensor_data_queue(); cartographer::mapping::MapBuilder& map_builder();
void AddSensorDataToTrajectory(const SensorData &sensor_data); cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>&
sensor_data_queue();
void AddSensorDataToTrajectory(const SensorData& sensor_data);
template <typename SensorDataType> template <typename SensorDataType>
void EnqueueSensorData(int trajectory_id, const std::string &sensor_id, void EnqueueSensorData(int trajectory_id, const std::string& sensor_id,
const SensorDataType &sensor_data) { const SensorDataType& sensor_data) {
sensor_data_queue_->Push(MapBuilderServer::SensorData{ sensor_data_queue_->Push(
trajectory_id, cartographer::common::make_unique<MapBuilderServer::SensorData>(
cartographer::sensor::MakeDispatchable(sensor_id, sensor_data)}); MapBuilderServer::SensorData{
trajectory_id, cartographer::sensor::MakeDispatchable(
sensor_id, sensor_data)}));
} }
private: private:
cartographer::mapping::MapBuilder *map_builder_; cartographer::mapping::MapBuilder* map_builder_;
cartographer::common::BlockingQueue<SensorData> *sensor_data_queue_; cartographer::common::BlockingQueue<std::unique_ptr<SensorData>>*
sensor_data_queue_;
}; };
MapBuilderServer( MapBuilderServer(
const proto::MapBuilderServerOptions &map_builder_server_options); const proto::MapBuilderServerOptions& map_builder_server_options);
// Starts the gRPC server and the SLAM thread. // Starts the gRPC server and the SLAM thread.
void Start(); void Start();
@ -77,7 +82,8 @@ class MapBuilderServer {
std::unique_ptr<std::thread> slam_thread_; std::unique_ptr<std::thread> slam_thread_;
std::unique_ptr<framework::Server> grpc_server_; std::unique_ptr<framework::Server> grpc_server_;
cartographer::mapping::MapBuilder map_builder_; 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 } // namespace cartographer_grpc