22 #include "pcl_thread.h"
24 #include <core/threading/mutex_locker.h>
25 #include <sensor_msgs/PointCloud2.h>
27 using namespace fawkes;
36 :
Thread(
"RosPointCloudThread",
Thread::OPMODE_WAITFORWAKEUP),
56 std::vector<std::string>::iterator p;
57 for (p = pcls.begin(); p != pcls.end(); ++p) {
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,
"_");
66 pi.pub =
rosnode->advertise<sensor_msgs::PointCloud2>(topic_name, 1);
68 logger->
log_info(
name(),
"Publishing point cloud %s at %s", p->c_str(), topic_name.c_str());
71 unsigned int width, height;
74 __adapter->
get_info(*p, width, height, frame_id, is_dense, fieldinfo);
75 pi.msg.header.frame_id = frame_id;
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;
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;
109 size_t point_size, num_points;
111 __adapter->
get_data(p->first, width, height, time,
112 &point_data, point_size, num_points);
114 if (pi.last_sent != time) {
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);
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;
128 pi.pub.publish(pi.msg);
133 __adapter->
close(p->first);