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 <pcl/point_cloud.h>
40 #include <pcl/point_types.h>
42 #include <mongocxx/client.hpp>
53 template <
typename Po
intType>
76 this->
name_ =
"PCL_DB_RetrievePL";
78 cfg_fixed_frame_ = config->
get_string(CFG_PREFIX_RETRV
"fixed-frame");
79 cfg_sensor_frame_ = config->
get_string(CFG_PREFIX_RETRV
"sensor-frame");
81 #ifdef USE_TIMETRACKER
84 ttc_retrieve_ = tt_->add_class(
"Full Retrieve");
85 ttc_retrieval_ = tt_->add_class(
"Retrieval");
86 ttc_transforms_ = tt_->add_class(
"Transforms");
93 #ifdef USE_TIMETRACKER
108 std::string &database,
109 std::string &collection,
110 std::string &target_frame,
113 TIMETRACK_START(ttc_retrieve_);
118 this->
output_->is_dense =
false;
120 std::vector<long> times(1, timestamp);
121 std::vector<long> actual_times(1, 0);
122 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> pcls(1);
124 TIMETRACK_START(ttc_retrieval_);
130 TIMETRACK_ABORT(ttc_retrieval_);
131 TIMETRACK_ABORT(ttc_retrieve_);
135 copy_output(pcls[0], original_, 128, 128, 128);
136 actual_time = actual_times[0];
138 if (target_frame ==
"SENSOR") {
139 target_frame = cfg_sensor_frame_;
142 if (target_frame !=
"") {
145 TIMETRACK_INTER(ttc_retrieval_, ttc_transforms_);
153 "Restored transforms for %zu frames for range (%li..%li)",
156 actual_times[0] + this->cfg_transform_range_[1]);
159 fawkes::pcl_utils::get_time(pcls[0], source_time);
162 pcls[0]->header.frame_id,
167 tf_->
lookup_transform(cfg_sensor_frame_, cfg_fixed_frame_, transform_current);
169 fawkes::tf::Transform transform = transform_current * transform_recorded;
172 fawkes::pcl_utils::transform_pointcloud(*pcls[0], transform);
179 TIMETRACK_END(ttc_transforms_);
182 copy_output(pcls[0], this->
output_);
184 TIMETRACK_END(ttc_retrieve_);
186 #ifdef USE_TIMETRACKER
189 tt_->print_to_stdout();
199 size_t num_points = in->points.size();
200 out->header.frame_id = in->header.frame_id;
201 out->points.resize(num_points);
203 out->width = num_points;
205 for (
size_t p = 0; p < num_points; ++p) {
206 const PointType & ip = in->points[p];
226 size_t num_points = in->points.size();
227 out->header.frame_id = in->header.frame_id;
228 out->points.resize(num_points);
230 out->width = num_points;
232 for (
size_t p = 0; p < num_points; ++p) {
233 const PointType & ip = in->points[p];
253 size_t num_points = in->points.size();
254 out->header.frame_id = in->header.frame_id;
255 out->points.resize(num_points);
257 out->width = num_points;
259 for (
size_t p = 0; p < num_points; ++p) {
260 const PointType & ip = in->points[p];
274 std::string cfg_fixed_frame_;
275 std::string cfg_sensor_frame_;
281 #ifdef USE_TIMETRACKER
283 unsigned int tt_loopcount_;
284 unsigned int ttc_retrieve_;
285 unsigned int ttc_retrieval_;
286 unsigned int ttc_transforms_;