22 #ifndef _PLUGINS_PERCEPTION_PCL_DB_PCL_DB_PIPELINE_H_ 23 #define _PLUGINS_PERCEPTION_PCL_DB_PCL_DB_PIPELINE_H_ 25 #include "mongodb_tf_transformer.h" 27 #include <config/config.h> 28 #include <logging/logger.h> 29 #include <pcl/point_cloud.h> 30 #include <pcl/point_types.h> 31 #include <pcl_utils/comparisons.h> 32 #include <pcl_utils/storage_adapter.h> 33 #include <pcl_utils/transforms.h> 34 #include <pcl_utils/utils.h> 37 #ifdef USE_TIMETRACKER 38 # include <utils/time/tracker.h> 40 #include <utils/time/tracker_macros.h> 43 #define USE_ICP_ALIGNMENT 46 #define CFG_PREFIX "/perception/pcl-db/" 48 #include <mongo/client/dbclient.h> 49 #include <mongo/client/gridfs.h> 50 #include <pcl/filters/approximate_voxel_grid.h> 51 #include <pcl/filters/conditional_removal.h> 52 #include <pcl/filters/extract_indices.h> 53 #include <pcl/filters/passthrough.h> 54 #include <pcl/filters/voxel_grid.h> 55 #include <pcl/point_cloud.h> 56 #include <pcl/point_types.h> 57 #include <pcl/segmentation/sac_segmentation.h> 58 #include <pcl/surface/convex_hull.h> 60 #ifdef HAVE_MONGODB_VERSION_H 62 # define QUERY MONGO_QUERY 65 static const uint8_t cluster_colors[12][3] = {{176, 0, 30},
78 typedef enum { APPLICABLE = 0, TYPE_MISMATCH, NO_POINTCLOUD, QUERY_FAILED } ApplicabilityStatus;
85 template <
typename Po
intType>
119 name_ =
"PCL_DB_Pipeline";
122 std::vector<float> transform_range = config->
get_floats(CFG_PREFIX
"transform-range");
123 if (transform_range.size() != 2) {
124 throw fawkes::Exception(
"Transform range must be a list with exactly two elements");
126 if (transform_range[1] < transform_range[0]) {
148 applicable(std::vector<long long> ×, std::string &database, std::string &collection)
150 const unsigned int num_clouds = times.size();
152 #if PCL_VERSION_COMPARE(>=, 1, 7, 0) 153 std::vector<pcl::PCLPointField> pfields;
155 std::vector<sensor_msgs::PointField> pfields;
157 pcl::for_each_type<typename pcl::traits::fieldList<PointType>::type>(
158 pcl::detail::FieldAdder<PointType>(pfields));
160 std::string fq_collection = database +
"." + collection;
162 for (
unsigned int i = 0; i < num_clouds; ++i) {
163 #if __cplusplus >= 201103L 164 std::unique_ptr<mongo::DBClientCursor> cursor =
166 std::auto_ptr<mongo::DBClientCursor> cursor =
169 QUERY(
"timestamp" << mongo::LTE << times[i] << mongo::GTE
171 .sort(
"timestamp", -1),
174 if (cursor->more()) {
175 mongo::BSONObj p = cursor->next();
176 mongo::BSONObj pcldoc = p.getObjectField(
"pointcloud");
177 std::vector<mongo::BSONElement> fields = pcldoc[
"field_info"].Array();
179 if (fields.size() == pfields.size()) {
180 for (
unsigned int i = 0; i < pfields.size(); ++i) {
181 #if PCL_VERSION_COMPARE(>=, 1, 7, 0) 182 pcl::PCLPointField &pf = pfields[i];
184 sensor_msgs::PointField &pf = pfields[i];
188 for (
unsigned int j = 0; j < fields.size(); ++j) {
189 if ((fields[j][
"name"].String() == pf.name)
190 && (fields[j][
"offset"].Int() == (int)pf.offset)
191 && (fields[j][
"datatype"].Int() == pf.datatype)
192 && (fields[j][
"count"].Int() == (int)pf.count)) {
199 "Type mismatch (fields) for pointcloud " 202 return TYPE_MISMATCH;
207 "Type mismatch (num fields) for pointcloud " 210 return TYPE_MISMATCH;
214 "No pointclouds for timestamp %lli in %s",
216 fq_collection.c_str());
217 return NO_POINTCLOUD;
220 }
catch (mongo::DBException &e) {
238 char * tmp = (
char *)dataptr;
241 mongo::GridFile file = gridfs.findFile(filename);
242 if (!file.exists()) {
248 for (
int c = 0; c < file.getNumChunks(); ++c) {
249 mongo::GridFSChunk chunk = file.getChunk(c);
251 const char * chunk_data = chunk.data(len);
252 memcpy(tmp, chunk_data, len);
270 std::vector<CloudPtr>
272 std::vector<long long> &actual_times,
273 std::string & database,
274 std::string & collection)
276 #ifdef HAVE_MONGODB_VERSION_H 278 mongodb_client_->createIndex(database +
"." + collection, mongo::fromjson(
"{timestamp:1}"));
280 mongodb_client_->ensureIndex(database +
"." + collection, mongo::fromjson(
"{timestamp:1}"));
283 const unsigned int num_clouds = times.size();
284 std::vector<CloudPtr> pcls(num_clouds);
287 for (
unsigned int i = 0; i < num_clouds; ++i) {
288 #if __cplusplus >= 201103L 289 std::unique_ptr<mongo::DBClientCursor> cursor =
291 std::auto_ptr<mongo::DBClientCursor> cursor =
294 QUERY(
"timestamp" << mongo::LTE << times[i] << mongo::GTE
296 .sort(
"timestamp", -1),
299 if (cursor->more()) {
300 mongo::BSONObj p = cursor->next();
301 mongo::BSONObj pcldoc = p.getObjectField(
"pointcloud");
302 std::vector<mongo::BSONElement> fields = pcldoc[
"field_info"].Array();
304 long long timestamp = p[
"timestamp"].Long();
305 double age = (double)(times[i] - timestamp) / 1000.;
312 actual_times[i] = p[
"timestamp"].Number();
315 lpcl->header.frame_id = pcldoc[
"frame_id"].String();
316 lpcl->is_dense = pcldoc[
"is_dense"].Bool();
317 lpcl->width = pcldoc[
"width"].Int();
318 lpcl->height = pcldoc[
"height"].Int();
319 fawkes::pcl_utils::set_time(lpcl, actual_time);
320 lpcl->points.resize(pcldoc[
"num_points"].Int());
324 pcldoc.getFieldDotted(
"data.filename").String());
327 return std::vector<CloudPtr>();
352 to_string(ApplicabilityStatus status)
355 case APPLICABLE:
return "Applicable";
356 case TYPE_MISMATCH:
return "PointCloud in database does not match type";
357 case NO_POINTCLOUD:
return "For at least one time no pointcloud found";
358 case QUERY_FAILED:
return "MongoDB query failed";
359 default:
return "Unknown error";
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Cloud::Ptr CloudPtr
Shared pointer to cloud.
pcl::PointCloud< PointType > Cloud
Basic point cloud type.
mongo::DBClientBase * mongodb_client_
MongoDB client to retrieve data.
A class for handling time.
virtual ~PointCloudDBPipeline()
Destructor.
long cfg_pcl_age_tolerance_
Age tolerance for retrieved point clouds.
pcl::PointCloud< ColorPointType > ColorCloud
Type for colored point clouds based on ColorPointType.
void read_gridfs_file(void *dataptr, std::string &database, std::string filename)
Read a file from MongoDB GridFS.
pcl::PointXYZRGB ColorPointType
Colored point type.
Cloud::ConstPtr CloudConstPtr
Shared pointer to constant cloud.
Database point cloud pipeline base class.
fawkes::Logger * logger_
Logger for informative messages.
ColorCloud::Ptr ColorCloudPtr
Shared pointer to colored cloud.
Base class for exceptions in Fawkes.
PointCloudDBPipeline(mongo::DBClientBase *mongodb_client, fawkes::Configuration *config, fawkes::Logger *logger, ColorCloudPtr output)
Constructor.
const char * name_
Name of the pipeline.
virtual std::vector< float > get_floats(const char *path)=0
Get list of values from configuration which is of type float.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
ColorCloudPtr output_
The final (colored) output of the pipeline.
ColorCloud::ConstPtr ColorCloudConstPtr
Shared pointer to constant colored cloud.
long cfg_transform_range_[2]
Transform range start and end times.
Interface for configuration handling.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
ApplicabilityStatus applicable(std::vector< long long > ×, std::string &database, std::string &collection)
Check if this pipeline instance is suitable for the given times.
std::vector< CloudPtr > retrieve_clouds(std::vector< long long > ×, std::vector< long long > &actual_times, std::string &database, std::string &collection)
Retrieve point clouds from database.