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" 29 #include <pcl/point_types.h> 30 #include <pcl/point_cloud.h> 32 #include <logging/logger.h> 33 #include <config/config.h> 34 #include <pcl_utils/utils.h> 35 #include <pcl_utils/transforms.h> 36 #include <pcl_utils/storage_adapter.h> 37 #include <pcl_utils/comparisons.h> 38 #ifdef USE_TIMETRACKER 39 # include <utils/time/tracker.h> 41 #include <utils/time/tracker_macros.h> 44 #define USE_ICP_ALIGNMENT 47 #define CFG_PREFIX "/perception/pcl-db/" 49 #include <pcl/point_cloud.h> 50 #include <pcl/point_types.h> 51 #include <pcl/filters/approximate_voxel_grid.h> 52 #include <pcl/filters/voxel_grid.h> 53 #include <pcl/filters/passthrough.h> 54 #include <pcl/filters/extract_indices.h> 55 #include <pcl/filters/conditional_removal.h> 56 #include <pcl/segmentation/sac_segmentation.h> 57 #include <pcl/surface/convex_hull.h> 59 #include <mongo/client/dbclient.h> 60 #include <mongo/client/gridfs.h> 62 #ifdef HAVE_MONGODB_VERSION_H 64 # define QUERY MONGO_QUERY 67 static const uint8_t cluster_colors[12][3] =
68 { {176, 0, 30}, {0, 0, 255}, {255, 90, 0}, {137, 82, 39}, {56, 23, 90}, {99, 0, 30},
69 {255, 0, 0}, {0, 255, 0}, {255, 255, 0}, {255, 0, 255}, {0, 255, 255}, {27, 117, 196}};
76 } ApplicabilityStatus;
83 template <
typename Po
intType>
113 ColorCloudPtr output)
116 name_ =
"PCL_DB_Pipeline";
119 (long)round(config->
get_float(CFG_PREFIX
"pcl-age-tolerance") * 1000.);
120 std::vector<float> transform_range = config->
get_floats(CFG_PREFIX
"transform-range");
121 if (transform_range.size() != 2) {
122 throw fawkes::Exception(
"Transform range must be a list with exactly two elements");
124 if (transform_range[1] < transform_range[0]) {
146 applicable(std::vector<long long> ×, std::string &database,
147 std::string &collection)
149 const unsigned int num_clouds = times.size();
151 #if PCL_VERSION_COMPARE(>=,1,7,0) 152 std::vector<pcl::PCLPointField> pfields;
154 std::vector<sensor_msgs::PointField> pfields;
156 pcl::for_each_type<typename pcl::traits::fieldList<PointType>::type>
157 (pcl::detail::FieldAdder<PointType>(pfields));
159 std::string fq_collection = database +
"." + collection;
161 for (
unsigned int i = 0; i < num_clouds; ++i) {
162 #if __cplusplus >= 201103L 163 std::unique_ptr<mongo::DBClientCursor> cursor =
165 std::auto_ptr<mongo::DBClientCursor> cursor =
168 QUERY(
"timestamp" << mongo::LTE << times[i]
170 .sort(
"timestamp", -1),
173 if (cursor->more()) {
174 mongo::BSONObj p = cursor->next();
175 mongo::BSONObj pcldoc = p.getObjectField(
"pointcloud");
176 std::vector<mongo::BSONElement> fields = pcldoc[
"field_info"].Array();
178 if (fields.size() == pfields.size()) {
179 for (
unsigned int i = 0; i < pfields.size(); ++i) {
180 #if PCL_VERSION_COMPARE(>=,1,7,0) 181 pcl::PCLPointField &pf = pfields[i];
183 sensor_msgs::PointField &pf = pfields[i];
187 for (
unsigned int j = 0; j < fields.size(); ++j) {
188 if ((fields[j][
"name"].String() == pf.name) &&
189 (fields[j][
"offset"].Int() == (int)pf.offset) &&
190 (fields[j][
"datatype"].Int() == pf.datatype) &&
191 (fields[j][
"count"].Int() == (int)pf.count) )
199 "at timestamp %lli", times[i]);
200 return TYPE_MISMATCH;
205 "at timestamp %lli", times[i]);
206 return TYPE_MISMATCH;
210 times[i], fq_collection.c_str());
211 return NO_POINTCLOUD;
214 }
catch (mongo::DBException &e) {
232 char *tmp = (
char *)dataptr;
235 mongo::GridFile file =
236 gridfs.findFile(filename);
237 if (! file.exists()) {
243 for (
int c = 0; c < file.getNumChunks(); ++c) {
244 mongo::GridFSChunk chunk = file.getChunk(c);
246 const char *chunk_data = chunk.data(len);
247 memcpy(tmp, chunk_data, len);
265 std::vector<CloudPtr>
267 std::string &database, std::string &collection)
269 #ifdef HAVE_MONGODB_VERSION_H 272 mongo::fromjson(
"{timestamp:1}"));
275 mongo::fromjson(
"{timestamp:1}"));
278 const unsigned int num_clouds = times.size();
279 std::vector<CloudPtr> pcls(num_clouds);
282 for (
unsigned int i = 0; i < num_clouds; ++i) {
284 #if __cplusplus >= 201103L 285 std::unique_ptr<mongo::DBClientCursor> cursor =
287 std::auto_ptr<mongo::DBClientCursor> cursor =
290 QUERY(
"timestamp" << mongo::LTE << times[i]
292 .sort(
"timestamp", -1),
295 if (cursor->more()) {
296 mongo::BSONObj p = cursor->next();
297 mongo::BSONObj pcldoc = p.getObjectField(
"pointcloud");
298 std::vector<mongo::BSONElement> fields = pcldoc[
"field_info"].Array();
300 long long timestamp = p[
"timestamp"].Long();
301 double age = (double) (times[i] - timestamp) / 1000.;
306 CloudPtr lpcl(
new Cloud());
309 actual_times[i] = p[
"timestamp"].Number();
312 lpcl->header.frame_id = pcldoc[
"frame_id"].String();
313 lpcl->is_dense = pcldoc[
"is_dense"].Bool();
314 lpcl->width = pcldoc[
"width"].Int();
315 lpcl->height = pcldoc[
"height"].Int();
316 fawkes::pcl_utils::set_time(lpcl, actual_time);
317 lpcl->points.resize(pcldoc[
"num_points"].Int());
320 pcldoc.getFieldDotted(
"data.filename").String());
323 return std::vector<CloudPtr>();
350 to_string(ApplicabilityStatus status)
353 case APPLICABLE:
return "Applicable";
354 case TYPE_MISMATCH:
return "PointCloud in database does not match type";
355 case NO_POINTCLOUD:
return "For at least one time no pointcloud found";
356 case QUERY_FAILED:
return "MongoDB query failed";
357 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.