Fawkes API  Fawkes Development Version
pcl_thread.cpp
1 
2 /***************************************************************************
3  * pcl_thread.cpp - Thread to exchange point clouds
4  *
5  * Created: Mon Nov 07 02:58:40 2011
6  * Copyright 2011 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "pcl_thread.h"
23 
24 #include <core/threading/mutex_locker.h>
25 #include <ros/ros.h>
26 #include <sensor_msgs/PointCloud2.h>
27 
28 using namespace fawkes;
29 
30 /** @class RosPointCloudThread "pcl_thread.h"
31  * Thread to exchange point clouds between Fawkes and ROS.
32  * @author Tim Niemueller
33  */
34 
35 /** Constructor. */
37  : Thread("RosPointCloudThread", Thread::OPMODE_WAITFORWAKEUP),
38  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)
39 {
40 }
41 
42 /** Destructor. */
44 {
45 }
46 
47 
48 
49 void
51 {
52  //__pubman = new RosPointCloudPublisherManager(rosnode);
53  __adapter = new PointCloudAdapter(pcl_manager, logger);
54 
55  cfg_ros_research_ival_ = config->get_float("/ros/pcl/ros-search-interval");
56 
57  fawkes_pointcloud_search();
58 
59  ros_pointcloud_search();
60  ros_pointcloud_last_searched_.stamp();
61 }
62 
63 
64 void
66 {
67  for (const std::string &item : ros_pointcloud_available_) {
68  pcl_manager->remove_pointcloud(item.c_str());
69  }
70  for (const std::pair<std::string, fawkes::pcl_utils::StorageAdapter *> &item : ros_pointcloud_available_ref_) {
71  delete item.second;
72  }
73  for (std::pair<std::string, ros::Subscriber> item : ros_pointcloud_subs_) {
74  item.second.shutdown();
75  }
76  delete __adapter;
77 }
78 
79 
80 void
82 {
83  // search ever n sec for new clouds on ROS side
84  fawkes::Time now;
85  now.stamp();
86  if ( fawkes::time_diff_sec(*now.get_timeval(), *ros_pointcloud_last_searched_.get_timeval()) >= cfg_ros_research_ival_ ) {
87  ros_pointcloud_last_searched_ = now;
88  ros_pointcloud_search();
89  ros_pointcloud_check_for_listener_in_fawkes();
90  //TODO if fawkes_pointcloud_search() would be called here, a check for clouds from fawkes need to be implemented
91  }
92 
93  // publish clouds fawkes->ros
94  fawkes_pointcloud_publish_to_ros();
95  // publish clouds ros->fawkes (this is done in callbacks)
96 }
97 
98 void
99 RosPointCloudThread::ros_pointcloud_search()
100 {
101  std::list<std::string> ros_pointclouds_new;
102 
103  // get all ROS topics
104  ros::master::V_TopicInfo ros_topics;
105  if ( ! ros::master::getTopics(ros_topics) ) {
106  logger->log_info(name(), "Coulnd't get available ROS topics");
107  return;
108  }
109 
110  // iterate through all topics
111  for ( const ros::master::TopicInfo &info : ros_topics ) {
112  // only topics of type sensor_msgs/PointCloud2 are important
113  if ( 0 == info.datatype.compare("sensor_msgs/PointCloud2") ) {
114  // check if this is a topic comming from fawkes
115  bool topic_not_from_fawkes = true;
116  for ( const std::pair<std::string, PublisherInfo> &fawkes_cloud : fawkes_pubs_) {
117  if ( 0 == info.name.compare( fawkes_cloud.second.pub.getTopic() ) ) {
118  topic_not_from_fawkes = false;
119  }
120  }
121  if (topic_not_from_fawkes) {
122  ros_pointclouds_new.push_back(info.name);
123  }
124  }
125  }
126 
127  // check for removed clouds
128  std::list<std::string> items_to_remove;
129  for (const std::string &item_old : ros_pointcloud_available_) {
130  bool exists = false;
131  for (std::string item_new : ros_pointclouds_new) {
132  if (0 == item_old.compare(item_new)) {
133  exists = true;
134  break;
135  }
136  }
137  if ( ! exists ) {
138  items_to_remove.push_back(item_old);
139  }
140  }
141  for (const std::string &item : items_to_remove) {
142  logger->log_info(name(), "Pointcloud %s is not available from ROS anymore", item.c_str());
143  ros_pointcloud_available_.remove(item);
144  }
145 
146  // check for new clouds
147  for (const std::string &ros_topic : ros_pointclouds_new) {
148  bool exists = false;
149  for (const std::string &in_list : ros_pointcloud_available_) {
150  if (0 == ros_topic.compare(in_list)) {
151  exists = true;
152  break;
153  }
154  }
155  if ( ! exists ) {
156  logger->log_info(name(), "Pointcloud %s is now available from ROS", ros_topic.c_str());
157  ros_pointcloud_available_.push_back(ros_topic);
158  ros_pointcloud_subs_[ros_topic] = rosnode->subscribe<sensor_msgs::PointCloud2>(ros_topic, 1,
159  boost::bind(&RosPointCloudThread::ros_pointcloud_on_data_msg, this, _1, ros_topic)
160  );
161  }
162  }
163 }
164 
165 void
166 RosPointCloudThread::ros_pointcloud_check_for_listener_in_fawkes()
167 {
168  for (const std::pair<std::string, fawkes::pcl_utils::StorageAdapter *> &item : ros_pointcloud_available_ref_) {
169  unsigned int use_count = 0;
170  if (item.second->is_pointtype<pcl::PointXYZ>()) {
171  use_count = dynamic_cast<fawkes::pcl_utils::PointCloudStorageAdapter<pcl::PointXYZ> *>(item.second)->cloud.use_count();
172  } else if (item.second->is_pointtype<pcl::PointXYZRGB>()) {
173  use_count = dynamic_cast<fawkes::pcl_utils::PointCloudStorageAdapter<pcl::PointXYZRGB> *>(item.second)->cloud.use_count();
174  } else if (item.second->is_pointtype<pcl::PointXYZI>()) {
175  use_count = dynamic_cast<fawkes::pcl_utils::PointCloudStorageAdapter<pcl::PointXYZI> *>(item.second)->cloud.use_count();
176  } else {
177  logger->log_error(name(), "Can't detect cloud type");
178  }
179 
180  if ( use_count <= 2 ) { // my internal list, this ref and the pcl_manager have copys of this pointer, if more are used, otheres are listening too
181  std::map<std::string, ros::Subscriber>::iterator element = ros_pointcloud_subs_.find(item.first);
182  if (element != ros_pointcloud_subs_.end()) {
183  element->second.shutdown();
184  ros_pointcloud_subs_.erase(item.first);
185  }
186  } else {
187  ros_pointcloud_subs_[item.first] = rosnode->subscribe<sensor_msgs::PointCloud2>(item.first, 1,
188  boost::bind(&RosPointCloudThread::ros_pointcloud_on_data_msg, this, _1, item.first)
189  );
190  }
191  }
192 }
193 
194 void
195 RosPointCloudThread::fawkes_pointcloud_search()
196 {
197  std::vector<std::string> pcls = pcl_manager->get_pointcloud_list();
198 
199  std::vector<std::string>::iterator p;
200  for (p = pcls.begin(); p != pcls.end(); ++p) {
201 
202  std::string topic_name = std::string("fawkes_pcls/") + *p;
203  std::string::size_type pos = 0;
204  while ((pos = topic_name.find("-", pos)) != std::string::npos) {
205  topic_name.replace(pos, 1, "_");
206  }
207 
208  PublisherInfo pi;
209  pi.pub = rosnode->advertise<sensor_msgs::PointCloud2>(topic_name, 1);
210 
211  logger->log_info(name(), "Publishing point cloud %s at %s", p->c_str(), topic_name.c_str());
212 
213  std::string frame_id;
214  unsigned int width, height;
215  bool is_dense;
217  __adapter->get_info(*p, width, height, frame_id, is_dense, fieldinfo);
218  pi.msg.header.frame_id = frame_id;
219  pi.msg.width = width;
220  pi.msg.height = height;
221  pi.msg.is_dense = is_dense;
222  pi.msg.fields.clear();
223  pi.msg.fields.resize(fieldinfo.size());
224  for (unsigned int i = 0; i < fieldinfo.size(); ++i) {
225  pi.msg.fields[i].name = fieldinfo[i].name;
226  pi.msg.fields[i].offset = fieldinfo[i].offset;
227  pi.msg.fields[i].datatype = fieldinfo[i].datatype;
228  pi.msg.fields[i].count = fieldinfo[i].count;
229  }
230 
231  fawkes_pubs_[*p] = pi;
232  }
233 }
234 
235 void
236 RosPointCloudThread::fawkes_pointcloud_publish_to_ros()
237 {
238  std::map<std::string, PublisherInfo>::iterator p;
239  for (p = fawkes_pubs_.begin(); p != fawkes_pubs_.end(); ++p) {
240  PublisherInfo &pi = p->second;
241  if (pi.pub.getNumSubscribers() > 0 && pcl_manager->exists_pointcloud(p->first.c_str())) {
242  unsigned int width, height;
243  void *point_data;
244  size_t point_size, num_points;
245  fawkes::Time time;
246  fawkes::Time now(time);
247  std::string frame_id;
248  __adapter->get_data(p->first, frame_id, width, height, time,
249  &point_data, point_size, num_points);
250 
251  if (pi.last_sent != time) {
252  pi.last_sent = time;
253 
254  size_t data_size = point_size * num_points;
255  pi.msg.data.resize(data_size);
256  memcpy (&pi.msg.data[0], point_data, data_size);
257 
258  pi.msg.width = width;
259  pi.msg.height = height;
260  pi.msg.header.frame_id = frame_id;
261  pi.msg.header.stamp.sec = time.get_sec();
262  pi.msg.header.stamp.nsec = time.get_nsec();
263  pi.msg.point_step = point_size;
264  pi.msg.row_step = point_size * pi.msg.width;
265 
266  pi.pub.publish(pi.msg);
267  //} else {
268  // logger->log_debug(name(), "No update for %s, not sending", p->first.c_str());
269  }
270  } else {
271  if (pcl_manager->exists_pointcloud(p->first.c_str())) {
272  __adapter->close(p->first);
273  }
274  }
275  }
276 }
277 
278 void
279 RosPointCloudThread::ros_pointcloud_on_data_msg(const sensor_msgs::PointCloud2ConstPtr &msg, const std::string topic_name)
280 {
281  // if this is the first time, I need the meta infos, what point-type is send
282  if ( ! pcl_manager->exists_pointcloud( topic_name.c_str() ) ) {
283  bool r = false, i = false;
284  for (const sensor_msgs::PointField &field : msg->fields) {
285 // logger->log_info(name(), "%s: %s", topic_name.c_str(), field.name.c_str());
286  if ( 0 == field.name.compare("r") ) { r = true; }
287  if ( 0 == field.name.compare("i") ) { i = true; }
288  }
289  if ( !r && !i ) {
290  logger->log_info(name(), "Adding %s with type XYZ ROS -> FAWKES", topic_name.c_str());
291  add_pointcloud<pcl::PointXYZ>(msg, topic_name);
292  } else if ( r && !i ) {
293  logger->log_info(name(), "Adding %s with type XYZRGB ROS -> FAWKES", topic_name.c_str());
294  add_pointcloud<pcl::PointXYZRGB>(msg, topic_name);
295  } else if ( !r && i) {
296  logger->log_info(name(), "Adding %s with type XYRI ROS -> FAWKES", topic_name.c_str());
297  add_pointcloud<pcl::PointXYZI>(msg, topic_name);
298  } else {
299  logger->log_error(name(), "%s: can't detect point type, using XYZ", topic_name.c_str());
300  add_pointcloud<pcl::PointXYZ>(msg, topic_name);
301  }
302  }
303 
304  // copy data
305  const pcl_utils::StorageAdapter* sa = pcl_manager->get_storage_adapter(topic_name.c_str());
306  if (sa->is_pointtype<pcl::PointXYZ>()) {
307  update_pointcloud<pcl::PointXYZ>(msg, topic_name);
308  } else if (sa->is_pointtype<pcl::PointXYZRGB>()) {
309  update_pointcloud<pcl::PointXYZRGB>(msg, topic_name);
310  } else if (sa->is_pointtype<pcl::PointXYZI>()) {
311  update_pointcloud<pcl::PointXYZI>(msg, topic_name);
312  } else {
313  logger->log_error(name(), "Can't detect cloud type");
314  }
315 
316  ros_pointcloud_check_for_listener_in_fawkes();
317 }
318 
319 
320 
321 
322 
323 
324 
325 
326 
327 
328 
std::vector< PointFieldInfo > V_PointFieldInfo
Vector of PointFieldInfo.
Definition: pcl_adapter.h:62
std::vector< std::string > get_pointcloud_list() const
Get list of point cloud IDs.
void close(const std::string &id)
Close an adapter.
const timeval * get_timeval() const
Obtain the timeval where the time is stored.
Definition: time.h:109
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void remove_pointcloud(const char *id)
Remove the point cloud.
RosPointCloudThread()
Constructor.
Definition: pcl_thread.cpp:36
Fawkes library namespace.
virtual void loop()
Code to execute in the thread.
Definition: pcl_thread.cpp:81
A class for handling time.
Definition: time.h:91
virtual ~RosPointCloudThread()
Destructor.
Definition: pcl_thread.cpp:43
Thread class encapsulation of pthreads.
Definition: thread.h:42
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual void init()
Initialize the thread.
Definition: pcl_thread.cpp:50
Adapter class for PCL point types.
Thread aspect to use blocked timing.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:50
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.
Definition: time.h:40
long get_nsec() const
Get nanoseconds.
Definition: time.h:113
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void finalize()
Finalize the thread.
Definition: pcl_thread.cpp:65
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
void get_info(const std::string &id, unsigned int &width, unsigned int &height, std::string &frame_id, bool &is_dense, V_PointFieldInfo &pfi)
Get info about point cloud.
long get_sec() const
Get seconds.
Definition: time.h:110
Point cloud adapter class.
Definition: pcl_adapter.h:38
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:48
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
void get_data(const std::string &id, std::string &frame_id, unsigned int &width, unsigned int &height, fawkes::Time &time, void **data_ptr, size_t &point_size, size_t &num_points)
Get current data of point cloud.
const pcl_utils::StorageAdapter * get_storage_adapter(const char *id)
Get a storage adapter.
bool exists_pointcloud(const char *id)
Check if point cloud exists.