Fawkes API Fawkes Development Version
pcl_db_retrieve_pipeline.h
1
2/***************************************************************************
3 * pcl_db_retrieve_pipeline.h - Retrieve point clouds from MongoDB
4 * Template class for variying point types
5 *
6 * Created: Thu Aug 22 11:02:59 2013
7 * Copyright 2012-2013 Tim Niemueller [www.niemueller.de]
8 ****************************************************************************/
9
10/* This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * This program is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU Library General Public License for more details.
19 *
20 * Read the full text in the LICENSE.GPL file in the doc directory.
21 */
22
23#ifndef _PLUGINS_PERCEPTION_PCL_DB_PCL_DB_RETRIEVE_PIPELINE_H_
24#define _PLUGINS_PERCEPTION_PCL_DB_PCL_DB_RETRIEVE_PIPELINE_H_
25
26#include "mongodb_tf_transformer.h"
27#include "pcl_db_pipeline.h"
28
29#include <pcl_utils/transforms.h>
30#include <pcl_utils/utils.h>
31#include <tf/transformer.h>
32#ifdef USE_TIMETRACKER
33# include <utils/time/tracker.h>
34#endif
35#include <utils/time/tracker_macros.h>
36
37#define CFG_PREFIX_RETRV "/perception/pcl-db-retrieve/"
38
39#include <pcl/point_cloud.h>
40#include <pcl/point_types.h>
41
42#include <mongocxx/client.hpp>
43
44/** Point cloud retrieve pipeline.
45 * This pipeline retrieves a point cloud from the database at a given point
46 * in time. The process assums a coordinate frame which is fixed in both,
47 * the recorded transforms and the current transform tree. Using this fixed
48 * frame it first transforms the point cloud to the fixed frame using the
49 * recorded transforms in the database, and then to transform it to the
50 * desired sensor frame using the current data.
51 * @author Tim Niemueller
52 */
53template <typename PointType>
55{
56public:
57 /** Constructor.
58 * @param mongodb_client MongoDB client
59 * @param config configuration
60 * @param logger Logger
61 * @param transformer TF transformer for point cloud transformations between
62 * coordinate reference frames
63 * @param original input point cloud
64 * @param output output point cloud
65 */
66 PointCloudDBRetrievePipeline(mongocxx::client * mongodb_client,
67 fawkes::Configuration * config,
68 fawkes::Logger * logger,
69 fawkes::tf::Transformer *transformer,
72 : PointCloudDBPipeline<PointType>(mongodb_client, config, logger, output),
73 tf_(transformer),
74 original_(original)
75 {
76 this->name_ = "PCL_DB_RetrievePL";
77
78 cfg_fixed_frame_ = config->get_string(CFG_PREFIX_RETRV "fixed-frame");
79 cfg_sensor_frame_ = config->get_string(CFG_PREFIX_RETRV "sensor-frame");
80
81#ifdef USE_TIMETRACKER
82 tt_ = new fawkes::TimeTracker();
83 tt_loopcount_ = 0;
84 ttc_retrieve_ = tt_->add_class("Full Retrieve");
85 ttc_retrieval_ = tt_->add_class("Retrieval");
86 ttc_transforms_ = tt_->add_class("Transforms");
87#endif
88 }
89
90 /** Destructor. */
92 {
93#ifdef USE_TIMETRACKER
94 delete tt_;
95#endif
96 }
97
98 /** Retrieve point clouds.
99 * @param timestamp time for which to retrieve the point cloud
100 * @param database database to retrieve from
101 * @param collection collection from which to retrieve the data
102 * @param target_frame coordinate frame to transform to
103 * @param actual_time upon return contains the actual time for
104 * which a point cloud was retrieved
105 */
106 void
107 retrieve(long timestamp,
108 std::string &database,
109 std::string &collection,
110 std::string &target_frame,
111 long & actual_time)
112 {
113 TIMETRACK_START(ttc_retrieve_);
114
115 this->output_->points.clear();
116 this->output_->height = 1;
117 this->output_->width = 0;
118 this->output_->is_dense = false;
119
120 std::vector<long> times(1, timestamp);
121 std::vector<long> actual_times(1, 0);
122 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> pcls(1);
123
124 TIMETRACK_START(ttc_retrieval_);
125
126 pcls =
127 PointCloudDBPipeline<PointType>::retrieve_clouds(times, actual_times, database, collection);
128 if (pcls.empty()) {
129 this->logger_->log_warn(this->name_, "No point clouds found for desired timestamp");
130 TIMETRACK_ABORT(ttc_retrieval_);
131 TIMETRACK_ABORT(ttc_retrieve_);
132 return;
133 }
134
135 copy_output(pcls[0], original_, 128, 128, 128);
136 actual_time = actual_times[0];
137
138 if (target_frame == "SENSOR") {
139 target_frame = cfg_sensor_frame_;
140 }
141
142 if (target_frame != "") {
143 // perform transformation
144
145 TIMETRACK_INTER(ttc_retrieval_, ttc_transforms_);
146
147 // retrieve transforms
148 fawkes::tf::MongoDBTransformer transformer(this->mongodb_client_, database);
149
150 transformer.restore(/* start */ actual_times[0] + this->cfg_transform_range_[0],
151 /* end */ actual_times[0] + this->cfg_transform_range_[1]);
152 this->logger_->log_debug(this->name_,
153 "Restored transforms for %zu frames for range (%li..%li)",
154 transformer.get_frame_caches().size(),
155 /* start */ actual_times[0] + this->cfg_transform_range_[0],
156 /* end */ actual_times[0] + this->cfg_transform_range_[1]);
157
158 fawkes::Time source_time;
159 fawkes::pcl_utils::get_time(pcls[0], source_time);
160 fawkes::tf::StampedTransform transform_recorded;
161 transformer.lookup_transform(cfg_fixed_frame_,
162 pcls[0]->header.frame_id,
163 source_time,
164 transform_recorded);
165
166 fawkes::tf::StampedTransform transform_current;
167 tf_->lookup_transform(cfg_sensor_frame_, cfg_fixed_frame_, transform_current);
168
169 fawkes::tf::Transform transform = transform_current * transform_recorded;
170
171 try {
172 fawkes::pcl_utils::transform_pointcloud(*pcls[0], transform);
173 } catch (fawkes::Exception &e) {
174 this->logger_->log_warn(this->name_, "Failed to transform point cloud, exception follows");
175 this->logger_->log_warn(this->name_, e);
176 }
177
178 // retrieve point clouds
179 TIMETRACK_END(ttc_transforms_);
180 }
181
182 copy_output(pcls[0], this->output_);
183
184 TIMETRACK_END(ttc_retrieve_);
185
186#ifdef USE_TIMETRACKER
187 //if (++tt_loopcount_ >= 5) {
188 // tt_loopcount_ = 0;
189 tt_->print_to_stdout();
190 //}
191#endif
192 }
193
194private: //methods
195 void
198 {
199 size_t num_points = in->points.size();
200 out->header.frame_id = in->header.frame_id;
201 out->points.resize(num_points);
202 out->height = 1;
203 out->width = num_points;
204
205 for (size_t p = 0; p < num_points; ++p) {
206 const PointType & ip = in->points[p];
207 typename PointCloudDBPipeline<PointType>::ColorPointType &op = out->points[p];
208
209 op.x = ip.x;
210 op.y = ip.y;
211 op.z = ip.z;
212
213 op.r = ip.r;
214 op.g = ip.g;
215 op.b = ip.b;
216 }
217 }
218
219 void
222 const int r,
223 const int g,
224 const int b)
225 {
226 size_t num_points = in->points.size();
227 out->header.frame_id = in->header.frame_id;
228 out->points.resize(num_points);
229 out->height = 1;
230 out->width = num_points;
231
232 for (size_t p = 0; p < num_points; ++p) {
233 const PointType & ip = in->points[p];
234 typename PointCloudDBPipeline<PointType>::ColorPointType &op = out->points[p];
235
236 op.x = ip.x;
237 op.y = ip.y;
238 op.z = ip.z;
239
240 op.r = r;
241 op.g = g;
242 op.b = b;
243 }
244 }
245
246 void
249 const int r = 255,
250 const int g = 255,
251 const int b = 255)
252 {
253 size_t num_points = in->points.size();
254 out->header.frame_id = in->header.frame_id;
255 out->points.resize(num_points);
256 out->height = 1;
257 out->width = num_points;
258
259 for (size_t p = 0; p < num_points; ++p) {
260 const PointType & ip = in->points[p];
261 typename PointCloudDBPipeline<PointType>::ColorPointType &op = out->points[p];
262
263 op.x = ip.x;
264 op.y = ip.y;
265 op.z = ip.z;
266
267 op.r = r;
268 op.g = g;
269 op.b = b;
270 }
271 }
272
273private: // members
274 std::string cfg_fixed_frame_;
275 std::string cfg_sensor_frame_;
276
278
280
281#ifdef USE_TIMETRACKER
283 unsigned int tt_loopcount_;
284 unsigned int ttc_retrieve_;
285 unsigned int ttc_retrieval_;
286 unsigned int ttc_transforms_;
287#endif
288};
289
290#endif
Database point cloud pipeline base class.
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.
std::vector< CloudPtr > retrieve_clouds(std::vector< long > &times, std::vector< long > &actual_times, std::string &database, std::string &collection_name)
Retrieve point clouds from database.
const char * name_
Name of the pipeline.
Point cloud retrieve pipeline.
PointCloudDBRetrievePipeline(mongocxx::client *mongodb_client, fawkes::Configuration *config, fawkes::Logger *logger, fawkes::tf::Transformer *transformer, typename PointCloudDBPipeline< PointType >::ColorCloudPtr original, typename PointCloudDBPipeline< PointType >::ColorCloudPtr output)
Constructor.
virtual ~PointCloudDBRetrievePipeline()
Destructor.
void retrieve(long timestamp, std::string &database, std::string &collection, std::string &target_frame, long &actual_time)
Retrieve point clouds.
Interface for configuration handling.
Definition: config.h:68
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Time tracking utility.
Definition: tracker.h:37
A class for handling time.
Definition: time.h:93
Read transforms from MongoDB and answer queries.
void restore(fawkes::Time &start, fawkes::Time &end)
Restore transforms from database.
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
std::vector< TimeCacheInterfacePtr > get_frame_caches() const
Get all currently existing caches.