22 #include "pcl_db_roscomm_thread.h" 23 #include <interfaces/PclDatabaseMergeInterface.h> 24 #include <interfaces/PclDatabaseRetrieveInterface.h> 25 #include <interfaces/PclDatabaseStoreInterface.h> 27 #include <core/threading/wait_condition.h> 28 #include <blackboard/utils/on_update_waker.h> 32 #include <pcl/pcl_config.h> 33 #if PCL_VERSION_COMPARE(>=,1,7,0) 34 # include <pcl/PCLPointCloud2.h> 35 # include <pcl/common/conversions.h> 36 # include <pcl_conversions/pcl_conversions.h> 41 #define CFG_PREFIX "/perception/pcl-db-merge/ros/" 51 :
Thread(
"PointCloudDBROSCommThread",
Thread::OPMODE_WAITFORWAKEUP),
66 cfg_store_pcl_id_ =
config->
get_string(
"/perception/pcl-db-roscomm/store-pcl-id");
86 srv_merge_ =
new ros::ServiceServer();
87 srv_retrieve_ =
new ros::ServiceServer();
88 srv_record_ =
new ros::ServiceServer();
89 srv_store_ =
new ros::ServiceServer();
91 *srv_merge_ =
rosnode->advertiseService(
"/pcl_db/merge",
92 &PointCloudDBROSCommThread::merge_cb,
this);
94 *srv_retrieve_ =
rosnode->advertiseService(
"/pcl_db/retrieve",
95 &PointCloudDBROSCommThread::retrieve_cb,
this);
97 *srv_store_ =
rosnode->advertiseService(
"/pcl_db/store",
98 &PointCloudDBROSCommThread::store_cb,
this);
104 srv_merge_->shutdown();
105 srv_retrieve_->shutdown();
106 srv_store_->shutdown();
107 srv_record_->shutdown();
109 delete srv_retrieve_;
113 delete merge_update_waker_;
114 delete retrieve_update_waker_;
115 delete store_update_waker_;
116 delete merge_waitcond_;
117 delete retrieve_waitcond_;
118 delete store_waitcond_;
134 merge_if_->
msgid(), merge_msg_id_, merge_if_->
is_final() ?
"yes" :
"no");
136 if ((merge_if_->
msgid() == merge_msg_id_) && merge_if_->
is_final()) {
142 retrieve_if_->
read();
147 retrieve_if_->
msgid(), retrieve_msg_id_,
148 retrieve_if_->
is_final() ?
"yes" :
"no");
150 if ((retrieve_if_->
msgid() == retrieve_msg_id_) && retrieve_if_->
is_final()) {
161 store_if_->
msgid(), store_msg_id_,
162 store_if_->
is_final() ?
"yes" :
"no");
164 if ((store_if_->
msgid() == store_msg_id_) && store_if_->
is_final()) {
172 PointCloudDBROSCommThread::merge_cb(hybris_c1_msgs::MergePointClouds::Request &req,
173 hybris_c1_msgs::MergePointClouds::Response &resp)
180 "exceeds maximum number allowed (%zu)",
183 resp.error =
"Number of requested timestamps exceeds maximum number allowed";
187 if (req.timestamps.empty()) {
190 resp.error =
"No times given in request";
196 std::vector<int64_t> timestamps(num_timestamps, 0);
197 for (
size_t i = 0; i < req.timestamps.size(); ++i) {
198 timestamps[i] = (int64_t)req.timestamps[i].sec * 1000L
199 + (int64_t)req.timestamps[i].nsec / 1000000L;
201 sort(timestamps.begin(), timestamps.begin() + req.timestamps.size());
208 merge_msg_id_ = mm->
id();
212 merge_waitcond_->
wait();
216 if (merge_if_->
is_final() && (std::string(
"") == merge_if_->
error())) {
220 resp.error = merge_if_->
error();
227 PointCloudDBROSCommThread::retrieve_cb(hybris_c1_msgs::RetrievePointCloud::Request &req,
228 hybris_c1_msgs::RetrievePointCloud::Response &resp)
233 int64_t timestamp = (int64_t)req.timestamp.sec * 1000L
234 + (int64_t)req.timestamp.nsec / 1000000L;
237 req.database.c_str(), req.collection.c_str());
247 retrieve_msg_id_ = mm->
id();
251 retrieve_waitcond_->
wait();
254 retrieve_if_->
read();
255 if (retrieve_if_->
is_final() && (std::string(
"") == retrieve_if_->
error())) {
259 resp.error = retrieve_if_->
error();
266 PointCloudDBROSCommThread::store_cb(hybris_c1_msgs::StorePointCloud::Request &req,
267 hybris_c1_msgs::StorePointCloud::Response &resp)
269 #if PCL_VERSION_COMPARE(>=,1,7,0) 274 (req.database ==
"") ?
"<default>" : req.database.c_str(),
275 (req.collection ==
"") ?
"<default>" : req.collection.c_str());
277 pcl::PCLPointCloud2 pcl_in;
278 pcl_conversions::moveToPCL(req.pointcloud, pcl_in);
285 std::string fields_in = pcl::getFieldsList(pcl_in);
286 std::string fields_xyz = pcl::getFieldsList(**pcl_xyz);
287 std::string fields_xyzrgb = pcl::getFieldsList(**pcl_xyzrgb);
289 if (fields_in == fields_xyz) {
290 pcl::fromPCLPointCloud2(pcl_in, **pcl_xyz);
293 }
else if (fields_in == fields_xyzrgb) {
294 pcl::fromPCLPointCloud2(pcl_in, **pcl_xyzrgb);
299 resp.error =
"Unsupported point cloud type";
308 store_msg_id_ = mm->
id();
312 store_waitcond_->
wait();
318 if (store_if_->
is_final() && (std::string(
"") == store_if_->
error())) {
322 resp.error = store_if_->
error();
328 resp.error =
"Storing only supported with PCL >= 1.7.0";
335 PointCloudDBROSCommThread::record_cb(hybris_c1_msgs::RecordData::Request &req,
336 hybris_c1_msgs::RecordData::Response &resp)
339 ros::Time begin = ros::Time::now();
340 ros::Time end = begin + req.range;
341 ros::Time::sleepUntil(end);
PclDatabaseMergeInterface Fawkes BlackBoard Interface.
virtual ~PointCloudDBROSCommThread()
Destructor.
Wait until a given condition holds.
unsigned int id() const
Get message ID.
RetrieveMessage Fawkes BlackBoard Interface Message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void unref()
Decrement reference count and conditionally delete this instance.
void remove_pointcloud(const char *id)
Remove the point cloud.
Wake threads when a blackboard interface is updated.
void set_target_frame(const char *new_target_frame)
Set target_frame value.
PclDatabaseRetrieveInterface Fawkes BlackBoard Interface.
void set_database(const char *new_database)
Set database value.
Fawkes library namespace.
void wake_all()
Wake up all waiting threads.
void set_pcl_id(const char *new_pcl_id)
Set pcl_id value.
void set_collection(const char *new_collection)
Set collection value.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
char * error() const
Get error value.
char * error() const
Get error value.
void set_timestamp(const int64_t new_timestamp)
Set timestamp value.
void set_database(const char *new_database)
Set database value.
Logger * logger
This is the Logger member used to access the logger.
MergeMessage Fawkes BlackBoard Interface Message.
uint32_t msgid() const
Get msgid value.
bool is_final() const
Get final value.
size_t maxlenof_timestamps() const
Get maximum length of timestamps value.
Thread aspect to use blocked timing.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
void set_collection(const char *new_collection)
Set collection value.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
void set_collection(const char *new_collection)
Set collection value.
void read()
Read from BlackBoard into local copy.
uint32_t msgid() const
Get msgid value.
void ref()
Increment reference count.
bool is_final() const
Get final value.
uint32_t msgid() const
Get msgid value.
bool is_final() const
Get final value.
const char * name() const
Get name of thread.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
PointCloudDBROSCommThread()
Constructor.
void wait()
Wait for the condition forever.
bool changed() const
Check if data has been changed.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
RefPtr<> is a reference-counting shared smartpointer.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
void set_timestamps(unsigned int index, const int64_t new_timestamps)
Set timestamps value at given index.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
void set_database(const char *new_database)
Set database value.
StoreMessage Fawkes BlackBoard Interface Message.
void set_original_timestamp(const bool new_original_timestamp)
Set original_timestamp value.
PclDatabaseStoreInterface Fawkes BlackBoard Interface.
Configuration * config
This is the Configuration member used to access the configuration.
virtual void loop()
Code to execute in the thread.
char * error() const
Get error value.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.