Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 <sensor_msgs/PointCloud2.h>
26 
27 using namespace fawkes;
28 
29 /** @class RosPointCloudThread "pcl_thread.h"
30  * Thread to exchange point clouds between Fawkes and ROS.
31  * @author Tim Niemueller
32  */
33 
34 /** Constructor. */
36  : Thread("RosPointCloudThread", Thread::OPMODE_WAITFORWAKEUP),
37  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
38 {
39 }
40 
41 /** Destructor. */
43 {
44 }
45 
46 
47 
48 void
50 {
51  //__pubman = new RosPointCloudPublisherManager(rosnode);
52  __adapter = new RosPointCloudAdapter(pcl_manager, logger);
53 
54  std::vector<std::string> pcls = pcl_manager->get_pointcloud_list();
55 
56  std::vector<std::string>::iterator p;
57  for (p = pcls.begin(); p != pcls.end(); ++p) {
58 
59  std::string topic_name = std::string("fawkes_pcls/") + *p;
60  std::string::size_type pos = 0;
61  while ((pos = topic_name.find("-", pos)) != std::string::npos) {
62  topic_name.replace(pos, 1, "_");
63  }
64 
65  PublisherInfo pi;
66  pi.pub = rosnode->advertise<sensor_msgs::PointCloud2>(topic_name, 1);
67 
68  logger->log_info(name(), "Publishing point cloud %s at %s", p->c_str(), topic_name.c_str());
69 
70  std::string frame_id;
71  unsigned int width, height;
72  bool is_dense;
74  __adapter->get_info(*p, width, height, frame_id, is_dense, fieldinfo);
75  pi.msg.header.frame_id = frame_id;
76  pi.msg.width = width;
77  pi.msg.height = height;
78  pi.msg.is_dense = is_dense;
79  pi.msg.fields.clear();
80  pi.msg.fields.resize(fieldinfo.size());
81  for (unsigned int i = 0; i < fieldinfo.size(); ++i) {
82  pi.msg.fields[i].name = fieldinfo[i].name;
83  pi.msg.fields[i].offset = fieldinfo[i].offset;
84  pi.msg.fields[i].datatype = fieldinfo[i].datatype;
85  pi.msg.fields[i].count = fieldinfo[i].count;
86  }
87 
88  __pubs[*p] = pi;
89  }
90 }
91 
92 
93 void
95 {
96  delete __adapter;
97 }
98 
99 
100 void
102 {
103  std::map<std::string, PublisherInfo>::iterator p;
104  for (p = __pubs.begin(); p != __pubs.end(); ++p) {
105  PublisherInfo &pi = p->second;
106  if (pi.pub.getNumSubscribers() > 0) {
107  unsigned int width, height;
108  void *point_data;
109  size_t point_size, num_points;
110  fawkes::Time time;
111  __adapter->get_data(p->first, width, height, time,
112  &point_data, point_size, num_points);
113 
114  if (pi.last_sent != time) {
115  pi.last_sent = time;
116 
117  size_t data_size = point_size * num_points;
118  pi.msg.data.resize(data_size);
119  memcpy (&pi.msg.data[0], point_data, data_size);
120 
121  pi.msg.width = width;
122  pi.msg.height = height;
123  pi.msg.header.stamp.sec = time.get_sec();
124  pi.msg.header.stamp.nsec = time.get_nsec();
125  pi.msg.point_step = point_size;
126  pi.msg.row_step = point_size * pi.msg.width;
127 
128  pi.pub.publish(pi.msg);
129  //} else {
130  // logger->log_debug(name(), "No update for %s, not sending", p->first.c_str());
131  }
132  } else {
133  __adapter->close(p->first);
134  }
135  }
136 }
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
std::vector< std::string > get_pointcloud_list() const
Get list of point cloud IDs.
RosPointCloudThread()
Constructor.
Definition: pcl_thread.cpp:35
void get_data(const std::string &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.
virtual void loop()
Code to execute in the thread.
Definition: pcl_thread.cpp:101
A class for handling time.
Definition: time.h:91
virtual ~RosPointCloudThread()
Destructor.
Definition: pcl_thread.cpp:42
Thread class encapsulation of pthreads.
Definition: thread.h:42
void get_info(std::string &id, unsigned int &width, unsigned int &height, std::string &frame_id, bool &is_dense, V_PointFieldInfo &pfi)
Get info about point cloud.
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:49
Thread aspect to use blocked timing.
Standalone PCL to ROS adapter class.
Definition: pcl_adapter.h:37
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:50
long get_sec() const
Get seconds.
Definition: time.h:110
virtual void finalize()
Finalize the thread.
Definition: pcl_thread.cpp:94
std::vector< PointFieldInfo > V_PointFieldInfo
Vector of PointFieldInfo.
Definition: pcl_adapter.h:61
const char * name() const
Get name of thread.
Definition: thread.h:95
long get_nsec() const
Get nanoseconds.
Definition: time.h:113
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:48