22 #include "pcl_db_retrieve_thread.h" 23 #include <interfaces/PclDatabaseRetrieveInterface.h> 25 #include <blackboard/utils/on_message_waker.h> 37 :
Thread(
"PointCloudDBRetrieveThread",
Thread::OPMODE_WAITFORWAKEUP)
61 foutput_->is_dense =
false;
63 output_ = pcl_utils::cloudptr_from_refptr(foutput_);
66 foriginal_->is_dense =
false;
68 original_ = pcl_utils::cloudptr_from_refptr(foriginal_);
114 long long timestamp = 0;
115 std::vector<long long> times(1);
116 std::string database;
117 std::string collection;
118 std::string target_frame;
119 bool original_timestamp;
129 retrieve_if_->
write();
131 timestamp = msg->timestamp();
132 times[0] = timestamp;
134 (strcmp(msg->database(),
"") != 0) ? msg->database() : cfg_database_;
135 collection = msg->collection();
136 target_frame = msg->target_frame();
137 original_timestamp = msg->is_original_timestamp();
146 collection.c_str(), timestamp);
148 ApplicabilityStatus st_xyz, st_xyzrgb;
149 long long actual_time = 0;
151 pl_xyz_->
applicable(times, database, collection);
152 if ((st_xyz = pl_xyz_->
applicable(times, database, collection)) == APPLICABLE) {
154 pl_xyz_->
retrieve(timestamp, database, collection, target_frame, actual_time);
155 }
else if ((st_xyzrgb = pl_xyzrgb_->
applicable(times, database, collection)) == APPLICABLE) {
157 pl_xyzrgb_->
retrieve(timestamp, database, collection, target_frame, actual_time);
158 if (! original_timestamp) {
160 pcl_utils::set_time(foutput_, now);
166 retrieve_if_->
set_error(
"No applicable merging pipeline known (for details see log)");
169 if (actual_time != 0) {
170 if (original_timestamp) {
171 Time now((
long)actual_time);
173 pcl_utils::get_time(foutput_, last);
175 if (last == now) now += 1L;
176 pcl_utils::set_time(foutput_, now);
179 pcl_utils::set_time(foutput_, now);
184 retrieve_if_->
write();
virtual void loop()
Code to execute in the thread.
RetrieveMessage Fawkes BlackBoard Interface Message.
bool msgq_empty()
Check if queue is empty.
void set_final(const bool new_final)
Set final value.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void set_error(const char *new_error)
Set error value.
void remove_pointcloud(const char *id)
Remove the point cloud.
PclDatabaseRetrieveInterface Fawkes BlackBoard Interface.
Fawkes library namespace.
mongo::DBClientBase * mongodb_client
MongoDB client to use to interact with the database.
Wake threads on receiving a blackboard message.
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.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
Logger * logger
This is the Logger member used to access the logger.
Clock * clock
By means of this member access to the clock is given.
void reset()
Reset pointer.
void msgq_pop()
Erase first message from queue.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Base class for exceptions in Fawkes.
virtual ~PointCloudDBRetrieveThread()
Destructor.
const char * name() const
Get name of thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void finalize()
Finalize the thread.
PointCloudDBRetrieveThread()
Constructor.
MessageType * msgq_first_safe(MessageType *&msg)
Get first message casted to the desired type without exceptions.
Configuration * config
This is the Configuration member used to access the configuration.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
ApplicabilityStatus applicable(std::vector< long long > ×, std::string &database, std::string &collection)
Check if this pipeline instance is suitable for the given times.
virtual void init()
Initialize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.