22 #include "pcl_db_store_thread.h" 24 #include <blackboard/utils/on_message_waker.h> 25 #include <interfaces/PclDatabaseStoreInterface.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;
85 if (store_if_->msgq_empty())
88 if (PclDatabaseStoreInterface::StoreMessage *msg = store_if_->msgq_first_safe(msg)) {
89 store_if_->set_final(
false);
90 store_if_->set_msgid(msg->id());
91 store_if_->set_error(
"");
94 std::string msg_database = msg->database();
95 std::string msg_collection = msg->collection();
99 std::string database = (msg_database !=
"") ? msg_database : cfg_database_;
100 std::string collection = database +
".";
101 if (msg_collection ==
"") {
102 collection +=
"pcls";
103 }
else if (msg_collection.compare(0, 3,
"fs.") == 0) {
104 errmsg =
"Passed in collection uses GridFS namespace";
107 collection += msg->collection();
111 store_pointcloud(msg->pcl_id(), database, collection, errmsg);
113 store_if_->set_error(errmsg.c_str());
114 store_if_->set_final(
true);
119 store_if_->msgq_pop();
123 PointCloudDBStoreThread::store_pointcloud(std::string pcl_id,
124 std::string database,
125 std::string collection,
129 errmsg =
"PointCloud does not exist";
133 std::string frame_id;
134 unsigned int width, height;
137 size_t point_size, num_points;
152 size_t data_size = point_size * num_points;
154 BSONObjBuilder document;
155 document.append(
"timestamp", (
long long)time.
in_msec());
156 BSONObjBuilder subb(document.subobjStart(
"pointcloud"));
157 subb.append(
"frame_id", frame_id);
158 subb.append(
"is_dense", is_dense);
159 subb.append(
"width", width);
160 subb.append(
"height", height);
161 subb.append(
"point_size", (
unsigned int)point_size);
162 subb.append(
"num_points", (
unsigned int)num_points);
166 std::stringstream
name;
168 subb.append(
"data", gridfs.storeFile((
char *)point_data, data_size,
name.str()));
170 BSONArrayBuilder subb2(subb.subarrayStart(
"field_info"));
171 for (
unsigned int i = 0; i < fieldinfo.size(); i++) {
172 BSONObjBuilder fi(subb2.subobjStart());
173 fi.append(
"name", fieldinfo[i].
name);
174 fi.append(
"offset", fieldinfo[i].offset);
175 fi.append(
"datatype", fieldinfo[i].datatype);
176 fi.append(
"count", fieldinfo[i].count);
183 }
catch (mongo::DBException &e) {
184 logger->
log_warn(this->
name(),
"Failed to insert into %s: %s", collection.c_str(), e.what());
std::vector< PointFieldInfo > V_PointFieldInfo
Vector of PointFieldInfo.
virtual void loop()
Code to execute in the thread.
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.
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.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Base class for exceptions in Fawkes.
Thread aspect to access MongoDB.
const char * name() const
Get name of thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Point cloud adapter class.
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.