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 <tf/transformer.h> 30 #include <pcl_utils/utils.h> 31 #include <pcl_utils/transforms.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 <mongo/client/dbclient.h> 43 #include <mongo/client/gridfs.h> 54 template <
typename Po
intType>
74 tf_(transformer), original_(original)
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 107 retrieve(
long long timestamp, std::string &database,
108 std::string &collection, std::string &target_frame,
109 long long &actual_time)
111 TIMETRACK_START(ttc_retrieve_);
116 this->
output_->is_dense =
false;
118 std::vector<long long> times(1, timestamp);
119 std::vector<long long> actual_times(1, 0);
120 std::vector<typename PointCloudDBPipeline<PointType>::CloudPtr> pcls(1);
122 TIMETRACK_START(ttc_retrieval_);
127 TIMETRACK_ABORT(ttc_retrieval_);
128 TIMETRACK_ABORT(ttc_retrieve_);
132 copy_output(pcls[0], original_, 128, 128, 128);
133 actual_time = actual_times[0];
135 if (target_frame ==
"SENSOR") {
136 target_frame == cfg_sensor_frame_;
139 if (target_frame !=
"") {
142 TIMETRACK_INTER(ttc_retrieval_, ttc_transforms_);
151 "Restored transforms for %zu frames for range (%li..%li)",
154 actual_times[0] + this->cfg_transform_range_[1]);
157 fawkes::pcl_utils::get_time(pcls[0], source_time);
160 source_time, transform_recorded);
163 tf_->
lookup_transform(cfg_sensor_frame_, cfg_fixed_frame_, transform_current);
165 fawkes::tf::Transform transform = transform_current * transform_recorded;
168 fawkes::pcl_utils::transform_pointcloud(*pcls[0], transform);
171 "Failed to transform point cloud, exception follows");
176 TIMETRACK_END(ttc_transforms_);
179 copy_output(pcls[0], this->
output_);
181 TIMETRACK_END(ttc_retrieve_);
183 #ifdef USE_TIMETRACKER 186 tt_->print_to_stdout();
195 size_t num_points = in->points.size();
196 out->header.frame_id = in->header.frame_id;
197 out->points.resize(num_points);
199 out->width = num_points;
201 for (
size_t p = 0; p < num_points; ++p) {
202 const PointType &ip = in->points[p];
217 const int r,
const int g,
const int b)
219 size_t num_points = in->points.size();
220 out->header.frame_id = in->header.frame_id;
221 out->points.resize(num_points);
223 out->width = num_points;
225 for (
size_t p = 0; p < num_points; ++p) {
226 const PointType &ip = in->points[p];
241 const int r = 255,
const int g = 255,
const int b = 255)
243 size_t num_points = in->points.size();
244 out->header.frame_id = in->header.frame_id;
245 out->points.resize(num_points);
247 out->width = num_points;
249 for (
size_t p = 0; p < num_points; ++p) {
250 const PointType &ip = in->points[p];
265 std::string cfg_fixed_frame_;
266 std::string cfg_sensor_frame_;
272 #ifdef USE_TIMETRACKER 274 unsigned int tt_loopcount_;
275 unsigned int ttc_retrieve_;
276 unsigned int ttc_retrieval_;
277 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.