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 <pcl/filters/approximate_voxel_grid.h>
49 #include <pcl/filters/conditional_removal.h>
50 #include <pcl/filters/extract_indices.h>
51 #include <pcl/filters/passthrough.h>
52 #include <pcl/filters/voxel_grid.h>
53 #include <pcl/point_cloud.h>
54 #include <pcl/point_types.h>
55 #include <pcl/segmentation/sac_segmentation.h>
56 #include <pcl/surface/convex_hull.h>
58 #include <bsoncxx/builder/basic/document.hpp>
59 #include <mongocxx/client.hpp>
60 #include <mongocxx/exception/gridfs_exception.hpp>
61 #include <mongocxx/exception/operation_exception.hpp>
62 #include <mongocxx/gridfs/bucket.hpp>
63 #include <mongocxx/gridfs/downloader.hpp>
65 #ifdef HAVE_MONGODB_VERSION_H
67 # define QUERY MONGO_QUERY
70 static const uint8_t cluster_colors[12][3] = {{176, 0, 30},
83 typedef enum { APPLICABLE = 0, TYPE_MISMATCH, NO_POINTCLOUD, QUERY_FAILED } ApplicabilityStatus;
90 template <
typename Po
intType>
124 name_ =
"PCL_DB_Pipeline";
127 std::vector<float> transform_range = config->
get_floats(CFG_PREFIX
"transform-range");
128 if (transform_range.size() != 2) {
129 throw fawkes::Exception(
"Transform range must be a list with exactly two elements");
131 if (transform_range[1] < transform_range[0]) {
153 applicable(std::vector<long long> ×, std::string &database, std::string &collection)
155 const unsigned int num_clouds = times.size();
157 #if PCL_VERSION_COMPARE(>=, 1, 7, 0)
158 std::vector<pcl::PCLPointField> pfields;
160 std::vector<sensor_msgs::PointField> pfields;
162 pcl::for_each_type<typename pcl::traits::fieldList<PointType>::type>(
163 pcl::detail::FieldAdder<PointType>(pfields));
166 for (
unsigned int i = 0; i < num_clouds; ++i) {
167 using namespace bsoncxx::builder;
168 auto result =
mongodb_client_->database(database)[collection].find_one(
169 basic::make_document(
170 basic::kvp(
"timestamp",
171 [&](basic::sub_document subdoc) {
172 subdoc.append(basic::kvp(
"$lt",
static_cast<int64_t
>(times[i])));
177 mongocxx::options::find().sort(basic::make_document(basic::kvp(
"timestamp", -1))));
179 bsoncxx::document::view pcldoc = result->view()[
"pointcloud"].get_document().view();
180 bsoncxx::array::view fields = pcldoc[
"field_info"].get_array();
182 if (fields.length() == pfields.size()) {
183 for (
unsigned int i = 0; i < pfields.size(); ++i) {
184 #if PCL_VERSION_COMPARE(>=, 1, 7, 0)
185 pcl::PCLPointField &pf = pfields[i];
187 sensor_msgs::PointField &pf = pfields[i];
191 for (
unsigned int j = 0; j < fields.length(); ++j) {
192 if ((fields[j][
"name"].get_utf8().value.to_string() == pf.name)
193 && (fields[j][
"offset"].get_int64() == (int64_t)pf.offset)
194 && (fields[j][
"datatype"].get_int64() == pf.datatype)
195 && (fields[j][
"count"].get_int64() == (int64_t)pf.count)) {
202 "Type mismatch (fields) for pointcloud "
205 return TYPE_MISMATCH;
210 "Type mismatch (num fields) for pointcloud "
213 return TYPE_MISMATCH;
217 "No pointclouds for timestamp %lli in %s.%s",
221 return NO_POINTCLOUD;
224 }
catch (mongocxx::operation_exception &e) {
244 auto downloader = gridfs.open_download_stream(file_id);
245 auto file_length = downloader.file_length();
246 auto buffer_size = std::min(file_length,
static_cast<int64_t
>(downloader.chunk_size()));
247 unsigned char *tmp = (
unsigned char *)dataptr;
248 while (
auto length_read = downloader.read(tmp,
static_cast<std::size_t
>(buffer_size))) {
251 }
catch (mongocxx::gridfs_exception &e) {
267 std::vector<CloudPtr>
269 std::vector<long> &actual_times,
270 std::string & database,
271 std::string & collection_name)
273 using namespace bsoncxx::builder;
274 auto collection =
mongodb_client_->database(database)[collection_name];
275 collection.create_index(basic::make_document(basic::kvp(
"timestamp", 1)));
277 const unsigned int num_clouds = times.size();
278 std::vector<CloudPtr> pcls(num_clouds);
281 for (
unsigned int i = 0; i < num_clouds; ++i) {
282 auto result = collection.find_one(
283 basic::make_document(basic::kvp(
"timestamp",
284 [&](basic::sub_document subdoc) {
285 subdoc.append(basic::kvp(
"$lt", times[i]));
289 mongocxx::options::find().sort(basic::make_document(basic::kvp(
"timestamp", -1))));
291 bsoncxx::document::view pcldoc = result->view()[
"pointcloud"].get_document().view();
293 int64_t timestamp = result->view()[
"timestamp"].get_int64();
294 double age = (double)(times[i] - timestamp) / 1000.;
302 actual_times[i] = timestamp;
305 lpcl->header.frame_id = pcldoc[
"frame_id"].get_utf8().value.to_string();
306 lpcl->is_dense = pcldoc[
"is_dense"].get_bool();
307 lpcl->width = pcldoc[
"width"].get_int64();
308 lpcl->height = pcldoc[
"height"].get_int64();
309 fawkes::pcl_utils::set_time(lpcl, actual_time);
310 lpcl->points.resize(pcldoc[
"num_points"].get_int64());
312 read_gridfs_file(&lpcl->points[0], database, pcldoc[
"data"][
"id"].get_value());
315 return std::vector<CloudPtr>();
340 to_string(ApplicabilityStatus status)
343 case APPLICABLE:
return "Applicable";
344 case TYPE_MISMATCH:
return "PointCloud in database does not match type";
345 case NO_POINTCLOUD:
return "For at least one time no pointcloud found";
346 case QUERY_FAILED:
return "MongoDB query failed";
347 default:
return "Unknown error";
Database point cloud pipeline base class.
pcl::PointCloud< ColorPointType > ColorCloud
Type for colored point clouds based on ColorPointType.
long cfg_transform_range_[2]
Transform range start and end times.
fawkes::Logger * logger_
Logger for informative messages.
ColorCloud::Ptr ColorCloudPtr
Shared pointer to colored cloud.
ColorCloudPtr output_
The final (colored) output of the pipeline.
pcl::PointXYZRGB ColorPointType
Colored point type.
mongocxx::client * mongodb_client_
MongoDB client to retrieve data.
long cfg_pcl_age_tolerance_
Age tolerance for retrieved point clouds.
Cloud::Ptr CloudPtr
Shared pointer to cloud.
PointCloudDBPipeline(mongocxx::client *mongodb_client, fawkes::Configuration *config, fawkes::Logger *logger, ColorCloudPtr output)
Constructor.
pcl::PointCloud< PointType > Cloud
Basic point cloud type.
ApplicabilityStatus applicable(std::vector< long long > ×, std::string &database, std::string &collection)
Check if this pipeline instance is suitable for the given times.
ColorCloud::ConstPtr ColorCloudConstPtr
Shared pointer to constant colored cloud.
Cloud::ConstPtr CloudConstPtr
Shared pointer to constant cloud.
std::vector< CloudPtr > retrieve_clouds(std::vector< long > ×, std::vector< long > &actual_times, std::string &database, std::string &collection_name)
Retrieve point clouds from database.
void read_gridfs_file(void *dataptr, std::string &database, bsoncxx::types::value file_id)
Read a file from MongoDB GridFS.
virtual ~PointCloudDBPipeline()
Destructor.
const char * name_
Name of the pipeline.
Interface for configuration handling.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::vector< float > get_floats(const char *path)=0
Get list of values from configuration which is of type float.
Base class for exceptions in Fawkes.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
A class for handling time.