23 #ifndef _PLUGINS_PERCEPTION_PCL_DB_PCL_DB_RETRIEVE_PIPELINE_H_ 24 #define _PLUGINS_PERCEPTION_PCL_DB_PCL_DB_RETRIEVE_PIPELINE_H_ 26 #include "mongodb_tf_transformer.h" 27 #include "pcl_db_pipeline.h" 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> 35 #include <utils/time/tracker_macros.h> 37 #define CFG_PREFIX_RETRV "/perception/pcl-db-retrieve/" 39 #include <mongo/client/dbclient.h> 40 #include <mongo/client/gridfs.h> 41 #include <pcl/point_cloud.h> 42 #include <pcl/point_types.h> 53 template <
typename Po
intType>
77 this->
name_ =
"PCL_DB_RetrievePL";
79 cfg_fixed_frame_ = config->
get_string(CFG_PREFIX_RETRV
"fixed-frame");
80 cfg_sensor_frame_ = config->
get_string(CFG_PREFIX_RETRV
"sensor-frame");
82 #ifdef USE_TIMETRACKER 85 ttc_retrieve_ = tt_->add_class(
"Full Retrieve");
86 ttc_retrieval_ = tt_->add_class(
"Retrieval");
87 ttc_transforms_ = tt_->add_class(
"Transforms");
94 #ifdef USE_TIMETRACKER 109 std::string &database,
110 std::string &collection,
111 std::string &target_frame,
112 long long & actual_time)
114 TIMETRACK_START(ttc_retrieve_);
119 this->
output_->is_dense =
false;
121 std::vector<long long> times(1, timestamp);
122 std::vector<long long> actual_times(1, 0);
123 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> pcls(1);
125 TIMETRACK_START(ttc_retrieval_);
131 TIMETRACK_ABORT(ttc_retrieval_);
132 TIMETRACK_ABORT(ttc_retrieve_);
136 copy_output(pcls[0], original_, 128, 128, 128);
137 actual_time = actual_times[0];
139 if (target_frame ==
"SENSOR") {
140 target_frame = cfg_sensor_frame_;
143 if (target_frame !=
"") {
146 TIMETRACK_INTER(ttc_retrieval_, ttc_transforms_);
154 "Restored transforms for %zu frames for range (%lli..%lli)",
157 actual_times[0] + this->cfg_transform_range_[1]);
160 fawkes::pcl_utils::get_time(pcls[0], source_time);
163 pcls[0]->header.frame_id,
168 tf_->
lookup_transform(cfg_sensor_frame_, cfg_fixed_frame_, transform_current);
170 fawkes::tf::Transform transform = transform_current * transform_recorded;
173 fawkes::pcl_utils::transform_pointcloud(*pcls[0], transform);
180 TIMETRACK_END(ttc_transforms_);
183 copy_output(pcls[0], this->
output_);
185 TIMETRACK_END(ttc_retrieve_);
187 #ifdef USE_TIMETRACKER 190 tt_->print_to_stdout();
200 size_t num_points = in->points.size();
201 out->header.frame_id = in->header.frame_id;
202 out->points.resize(num_points);
204 out->width = num_points;
206 for (
size_t p = 0; p < num_points; ++p) {
207 const PointType & ip = in->points[p];
227 size_t num_points = in->points.size();
228 out->header.frame_id = in->header.frame_id;
229 out->points.resize(num_points);
231 out->width = num_points;
233 for (
size_t p = 0; p < num_points; ++p) {
234 const PointType & ip = in->points[p];
254 size_t num_points = in->points.size();
255 out->header.frame_id = in->header.frame_id;
256 out->points.resize(num_points);
258 out->width = num_points;
260 for (
size_t p = 0; p < num_points; ++p) {
261 const PointType & ip = in->points[p];
275 std::string cfg_fixed_frame_;
276 std::string cfg_sensor_frame_;
282 #ifdef USE_TIMETRACKER 284 unsigned int tt_loopcount_;
285 unsigned int ttc_retrieve_;
286 unsigned int ttc_retrieval_;
287 unsigned int ttc_transforms_;
PointCloudDBRetrievePipeline(mongo::DBClientBase *mongodb_client, fawkes::Configuration *config, fawkes::Logger *logger, fawkes::tf::Transformer *transformer, typename PointCloudDBPipeline< PointType >::ColorCloudPtr original, typename PointCloudDBPipeline< PointType >::ColorCloudPtr output)
Constructor.
mongo::DBClientBase * mongodb_client_
MongoDB client to retrieve data.
A class for handling time.
void retrieve(long long timestamp, std::string &database, std::string &collection, std::string &target_frame, long long &actual_time)
Retrieve point clouds.
pcl::PointXYZRGB ColorPointType
Colored point type.
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.
const char * name_
Name of the pipeline.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
ColorCloudPtr output_
The final (colored) output of the pipeline.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Point cloud retrieve pipeline.
long cfg_transform_range_[2]
Transform range start and end times.
Interface for configuration handling.
virtual ~PointCloudDBRetrievePipeline()
Destructor.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
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.