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 <pcl_utils/pcl_adapter.h>
27 
28 #include <core/threading/thread.h>
29 #include <aspect/clock.h>
30 #include <aspect/configurable.h>
31 #include <aspect/logging.h>
32 #include <aspect/pointcloud.h>
33 #include <plugins/mongodb/aspect/mongodb.h>
34 #include <blackboard/interface_listener.h>
35 #include <blackboard/interface_observer.h>
36 #include <interfaces/TransformInterface.h>
37 #include <core/threading/mutex.h>
38 
39 #include <list>
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 
48 namespace fawkes {
49  class Mutex;
50  class TimeWait;
51 }
52 
53 namespace mongo {
54  class GridFS;
55 }
56 
58 : public fawkes::Thread,
59  public fawkes::ClockAspect,
60  public fawkes::LoggingAspect,
64 {
65  public:
67  virtual ~MongoLogPointCloudThread();
68 
69  virtual void init();
70  virtual void loop();
71  virtual bool prepare_finalize_user();
72  virtual void finalize();
73 
74  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
75  protected: virtual void run() { Thread::run(); }
76 
77  private:
78  PointCloudAdapter *adapter_;
79 
80  /// @cond INTERNALS
81  typedef struct {
82  std::string topic_name;
83 #if PCL_VERSION_COMPARE(>=,1,7,0)
84  pcl::PCLPointCloud2 msg;
85 #else
86  sensor_msgs::PointCloud2 msg;
87 #endif
88  fawkes::Time last_sent;
89  } PointCloudInfo;
90  /// @endcond
91  std::map<std::string, PointCloudInfo> pcls_;
92 
93  mongo::DBClientBase *mongodb_;
94  mongo::GridFS *gridfs_;
95  std::string collection_;
96  std::string database_;
97 
98  fawkes::Mutex *mutex_;
99  fawkes::TimeWait *wait_;
100 
101  bool cfg_flush_after_write_;
102  unsigned int cfg_chunk_size_;
103  float cfg_storage_interval_;
104 };
105 
106 #endif
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:36
Fawkes library namespace.
Definition: mongodb.h:29
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:40
virtual void run()
Code to execute in the thread.
Definition: thread.cpp:939
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void run()
Stub to see name in backtrace for easier debugging.
Thread aspect to access MongoDB.
Definition: mongodb.h:40
Thread aspect to log output.
Definition: logging.h:35
Thread to store point clouds to MongoDB.
Thread aspect to access configuration data.
Definition: configurable.h:35
Point cloud adapter class.
Definition: pcl_adapter.h:38
Mutex mutual exclusion lock.
Definition: mutex.h:32
Time wait utility.
Definition: wait.h:32