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