Fawkes API Fawkes Development Version
mongodb_log_pcl_thread.h
1
2/***************************************************************************
3 * mongodb_log_pcl_thread.h - Thread to log point clouds to MongoDB
4 *
5 * Created: Mon Nov 07 02:26:35 2011
6 * Copyright 2011-2012 Tim Niemueller [www.niemueller.de]
7 * 2012 Bastian Klingen
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#ifndef _PLUGINS_MONGODB_LOG_MONGODB_LOG_PCL_THREAD_H_
24#define _PLUGINS_MONGODB_LOG_MONGODB_LOG_PCL_THREAD_H_
25
26#include <aspect/clock.h>
27#include <aspect/configurable.h>
28#include <aspect/logging.h>
29#include <aspect/pointcloud.h>
30#include <blackboard/interface_listener.h>
31#include <blackboard/interface_observer.h>
32#include <core/threading/mutex.h>
33#include <core/threading/thread.h>
34#include <interfaces/TransformInterface.h>
35#include <pcl_utils/pcl_adapter.h>
36#include <plugins/mongodb/aspect/mongodb.h>
37
38#include <list>
39#include <mongocxx/gridfs/bucket.hpp>
40#include <queue>
41
42#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
43# include <pcl/PCLPointCloud2.h>
44#else
45# include <sensor_msgs/PointCloud2.h>
46#endif
47
48namespace fawkes {
49class Mutex;
50class TimeWait;
51} // namespace fawkes
52
59{
60public:
63
64 virtual void init();
65 virtual void loop();
66 virtual bool prepare_finalize_user();
67 virtual void finalize();
68
69 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
70protected:
71 virtual void
73 {
74 Thread::run();
75 }
76
77private:
78 PointCloudAdapter *adapter_;
79
80 /// @cond INTERNALS
81 typedef struct
82 {
83 std::string topic_name;
84#if PCL_VERSION_COMPARE(>=, 1, 7, 0)
85 pcl::PCLPointCloud2 msg;
86#else
87 sensor_msgs::PointCloud2 msg;
88#endif
89 fawkes::Time last_sent;
90 } PointCloudInfo;
91 /// @endcond
92 std::map<std::string, PointCloudInfo> pcls_;
93
94 mongocxx::client * mongodb_;
95 mongocxx::gridfs::bucket gridfs_;
96 std::string collection_;
97 std::string database_;
98
99 fawkes::Mutex * mutex_;
100 fawkes::TimeWait *wait_;
101
102 bool cfg_flush_after_write_;
103 unsigned int cfg_chunk_size_;
104 float cfg_storage_interval_;
105};
106
107#endif
Thread to store point clouds to MongoDB.
virtual void init()
Initialize the thread.
virtual ~MongoLogPointCloudThread()
Destructor.
virtual void run()
Stub to see name in backtrace for easier debugging.
virtual void loop()
Code to execute in the thread.
virtual bool prepare_finalize_user()
Prepare finalization user implementation.
virtual void finalize()
Finalize the thread.
Point cloud adapter class.
Definition: pcl_adapter.h:39
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
Thread aspect to access configuration data.
Definition: configurable.h:33
Thread aspect to log output.
Definition: logging.h:33
Thread aspect to access MongoDB.
Definition: mongodb.h:39
Mutex mutual exclusion lock.
Definition: mutex.h:33
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:38
Thread class encapsulation of pthreads.
Definition: thread.h:46
Time wait utility.
Definition: wait.h:33
A class for handling time.
Definition: time.h:93
Fawkes library namespace.