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