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)",
155 actual_times[0] + this->cfg_transform_range_[0],
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_;
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 > ×, 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.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
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.
A class for handling time.