22 #include "pcl_db_store_thread.h" 23 #include <interfaces/PclDatabaseStoreInterface.h> 25 #include <blackboard/utils/on_message_waker.h> 26 #include <pcl_utils/pcl_adapter.h> 29 #include <mongo/client/dbclient.h> 30 #include <mongo/client/gridfs.h> 32 #define CFG_PREFIX "/perception/pcl-db/" 35 using namespace mongo;
45 :
Thread(
"PointCloudDBStoreThread",
Thread::OPMODE_WAITFORWAKEUP)
100 std::string msg_database = msg->database();
101 std::string msg_collection = msg->collection();
105 std::string database = (msg_database !=
"") ? msg_database : cfg_database_;
106 std::string collection = database +
".";
107 if (msg_collection ==
"") {
108 collection +=
"pcls";
109 }
else if (msg_collection.find(
"fs.") == 0) {
110 errmsg =
"Passed in collection uses GridFS namespace";
113 collection += msg->collection();
116 if (store) store_pointcloud(msg->pcl_id(), database, collection, errmsg);
129 PointCloudDBStoreThread::store_pointcloud(std::string pcl_id, std::string database,
130 std::string collection, std::string &errmsg)
133 errmsg =
"PointCloud does not exist";
137 std::string frame_id;
138 unsigned int width, height;
141 size_t point_size, num_points;
146 fieldinfo, &point_data, point_size, num_points);
148 size_t data_size = point_size * num_points;
150 BSONObjBuilder document;
151 document.append(
"timestamp", (
long long) time.
in_msec());
152 BSONObjBuilder subb(document.subobjStart(
"pointcloud"));
153 subb.append(
"frame_id", frame_id);
154 subb.append(
"is_dense", is_dense);
155 subb.append(
"width", width);
156 subb.append(
"height", height);
157 subb.append(
"point_size", (
unsigned int)point_size);
158 subb.append(
"num_points", (
unsigned int)num_points);
162 std::stringstream
name;
163 name <<
"pcl_" << time.
in_msec();
164 subb.append(
"data", gridfs.storeFile((
char*) point_data, data_size, name.str()));
166 BSONArrayBuilder subb2(subb.subarrayStart(
"field_info"));
167 for (
unsigned int i = 0; i < fieldinfo.size(); i++) {
168 BSONObjBuilder fi(subb2.subobjStart());
169 fi.append(
"name", fieldinfo[i].name);
170 fi.append(
"offset", fieldinfo[i].offset);
171 fi.append(
"datatype", fieldinfo[i].datatype);
172 fi.append(
"count", fieldinfo[i].count);
179 }
catch (mongo::DBException &e) {
181 collection.c_str(), e.what());
std::vector< PointFieldInfo > V_PointFieldInfo
Vector of PointFieldInfo.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
virtual void loop()
Code to execute in the thread.
void set_final(const bool new_final)
Set final value.
bool msgq_empty()
Check if queue is empty.
void set_error(const char *new_error)
Set error value.
Fawkes library namespace.
mongo::DBClientBase * mongodb_client
MongoDB client to use to interact with the database.
Wake threads on receiving a blackboard message.
A class for handling time.
PointCloudDBStoreThread()
Constructor.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
virtual void init()
Initialize the thread.
Logger * logger
This is the Logger member used to access the logger.
void get_data_and_info(const std::string &id, std::string &frame_id, bool &is_dense, unsigned int &width, unsigned int &height, fawkes::Time &time, V_PointFieldInfo &pfi, void **data_ptr, size_t &point_size, size_t &num_points)
Get data and info of point cloud.
long in_msec() const
Convert the stored time into milli-seconds.
void msgq_pop()
Erase first message from queue.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Base class for exceptions in Fawkes.
const char * name() const
Get name of thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Point cloud adapter class.
StoreMessage Fawkes BlackBoard Interface Message.
PclDatabaseStoreInterface Fawkes BlackBoard Interface.
Configuration * config
This is the Configuration member used to access the configuration.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual ~PointCloudDBStoreThread()
Destructor.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void finalize()
Finalize the thread.
bool exists_pointcloud(const char *id)
Check if point cloud exists.
virtual void close(Interface *interface)=0
Close interface.