22 #include "pcl_thread.h"
24 #include <core/threading/mutex_locker.h>
26 #include <sensor_msgs/PointCloud2.h>
37 :
Thread(
"RosPointCloudThread",
Thread::OPMODE_WAITFORWAKEUP),
53 cfg_ros_research_ival_ =
config->
get_float(
"/ros/pcl/ros-search-interval");
55 fawkes_pointcloud_search();
57 ros_pointcloud_search();
58 ros_pointcloud_last_searched_.
stamp();
64 for (
const std::string &item : ros_pointcloud_available_) {
67 for (
const std::pair<std::string, fawkes::pcl_utils::StorageAdapter *> &item :
68 ros_pointcloud_available_ref_) {
71 for (std::pair<std::string, ros::Subscriber> item : ros_pointcloud_subs_) {
72 item.second.shutdown();
84 >= cfg_ros_research_ival_) {
85 ros_pointcloud_last_searched_ = now;
86 ros_pointcloud_search();
87 ros_pointcloud_check_for_listener_in_fawkes();
92 fawkes_pointcloud_publish_to_ros();
97 RosPointCloudThread::ros_pointcloud_search()
99 std::list<std::string> ros_pointclouds_new;
102 ros::master::V_TopicInfo ros_topics;
103 if (!ros::master::getTopics(ros_topics)) {
109 for (
const ros::master::TopicInfo &info : ros_topics) {
111 if (0 == info.datatype.compare(
"sensor_msgs/PointCloud2")) {
113 bool topic_not_from_fawkes =
true;
114 for (
const std::pair<std::string, PublisherInfo> &fawkes_cloud : fawkes_pubs_) {
115 if (0 == info.name.compare(fawkes_cloud.second.pub.getTopic())) {
116 topic_not_from_fawkes =
false;
119 if (topic_not_from_fawkes) {
120 ros_pointclouds_new.push_back(info.name);
126 std::list<std::string> items_to_remove;
127 for (
const std::string &item_old : ros_pointcloud_available_) {
129 for (std::string item_new : ros_pointclouds_new) {
130 if (0 == item_old.compare(item_new)) {
136 items_to_remove.push_back(item_old);
139 for (
const std::string &item : items_to_remove) {
140 logger->
log_info(
name(),
"Pointcloud %s is not available from ROS anymore", item.c_str());
141 ros_pointcloud_available_.remove(item);
145 for (
const std::string &ros_topic : ros_pointclouds_new) {
147 for (
const std::string &in_list : ros_pointcloud_available_) {
148 if (0 == ros_topic.compare(in_list)) {
154 logger->
log_info(
name(),
"Pointcloud %s is now available from ROS", ros_topic.c_str());
155 ros_pointcloud_available_.push_back(ros_topic);
156 ros_pointcloud_subs_[ros_topic] =
rosnode->subscribe<sensor_msgs::PointCloud2>(
159 boost::bind(&RosPointCloudThread::ros_pointcloud_on_data_msg,
this, _1, ros_topic));
165 RosPointCloudThread::ros_pointcloud_check_for_listener_in_fawkes()
167 for (
const std::pair<std::string, fawkes::pcl_utils::StorageAdapter *> &item :
168 ros_pointcloud_available_ref_) {
169 unsigned int use_count = 0;
170 if (item.second->is_pointtype<pcl::PointXYZ>()) {
174 }
else if (item.second->is_pointtype<pcl::PointXYZRGB>()) {
178 }
else if (item.second->is_pointtype<pcl::PointXYZI>()) {
189 std::map<std::string, ros::Subscriber>::iterator element =
190 ros_pointcloud_subs_.find(item.first);
191 if (element != ros_pointcloud_subs_.end()) {
192 element->second.shutdown();
193 ros_pointcloud_subs_.erase(item.first);
196 ros_pointcloud_subs_[item.first] =
rosnode->subscribe<sensor_msgs::PointCloud2>(
199 boost::bind(&RosPointCloudThread::ros_pointcloud_on_data_msg,
this, _1, item.first));
205 RosPointCloudThread::fawkes_pointcloud_search()
209 std::vector<std::string>::iterator p;
210 for (p = pcls.begin(); p != pcls.end(); ++p) {
211 std::string topic_name = std::string(
"fawkes_pcls/") + *p;
212 std::string::size_type pos = 0;
213 while ((pos = topic_name.find(
"-", pos)) != std::string::npos) {
214 topic_name.replace(pos, 1,
"_");
218 pi.pub =
rosnode->advertise<sensor_msgs::PointCloud2>(topic_name, 1);
220 logger->
log_info(
name(),
"Publishing point cloud %s at %s", p->c_str(), topic_name.c_str());
222 std::string frame_id;
223 unsigned int width, height;
226 adapter_->
get_info(*p, width, height, frame_id, is_dense, fieldinfo);
227 pi.msg.header.frame_id = frame_id;
228 pi.msg.width = width;
229 pi.msg.height = height;
230 pi.msg.is_dense = is_dense;
231 pi.msg.fields.clear();
232 pi.msg.fields.resize(fieldinfo.size());
233 for (
unsigned int i = 0; i < fieldinfo.size(); ++i) {
234 pi.msg.fields[i].name = fieldinfo[i].name;
235 pi.msg.fields[i].offset = fieldinfo[i].offset;
236 pi.msg.fields[i].datatype = fieldinfo[i].datatype;
237 pi.msg.fields[i].count = fieldinfo[i].count;
240 fawkes_pubs_[*p] = pi;
245 RosPointCloudThread::fawkes_pointcloud_publish_to_ros()
247 std::map<std::string, PublisherInfo>::iterator p;
248 for (p = fawkes_pubs_.begin(); p != fawkes_pubs_.end(); ++p) {
249 PublisherInfo &pi = p->second;
251 unsigned int width, height;
253 size_t point_size, num_points;
256 std::string frame_id;
258 p->first, frame_id, width, height, time, &point_data, point_size, num_points);
260 if (pi.last_sent != time) {
263 size_t data_size = point_size * num_points;
264 pi.msg.data.resize(data_size);
265 memcpy(&pi.msg.data[0], point_data, data_size);
267 pi.msg.width = width;
268 pi.msg.height = height;
269 pi.msg.header.frame_id = frame_id;
270 pi.msg.header.stamp.sec = time.
get_sec();
271 pi.msg.header.stamp.nsec = time.
get_nsec();
272 pi.msg.point_step = point_size;
273 pi.msg.row_step = point_size * pi.msg.width;
275 pi.pub.publish(pi.msg);
281 adapter_->
close(p->first);
288 RosPointCloudThread::ros_pointcloud_on_data_msg(
const sensor_msgs::PointCloud2ConstPtr &msg,
289 const std::string & topic_name)
293 bool r =
false, i =
false;
294 for (
const sensor_msgs::PointField &field : msg->fields) {
296 if (0 == field.name.compare(
"r")) {
299 if (0 == field.name.compare(
"i")) {
304 logger->
log_info(
name(),
"Adding %s with type XYZ ROS -> FAWKES", topic_name.c_str());
305 add_pointcloud<pcl::PointXYZ>(msg, topic_name);
306 }
else if (r && !i) {
307 logger->
log_info(
name(),
"Adding %s with type XYZRGB ROS -> FAWKES", topic_name.c_str());
308 add_pointcloud<pcl::PointXYZRGB>(msg, topic_name);
309 }
else if (!r && i) {
310 logger->
log_info(
name(),
"Adding %s with type XYRI ROS -> FAWKES", topic_name.c_str());
311 add_pointcloud<pcl::PointXYZI>(msg, topic_name);
314 add_pointcloud<pcl::PointXYZ>(msg, topic_name);
321 update_pointcloud<pcl::PointXYZ>(msg, topic_name);
323 update_pointcloud<pcl::PointXYZRGB>(msg, topic_name);
325 update_pointcloud<pcl::PointXYZI>(msg, topic_name);
330 ros_pointcloud_check_for_listener_in_fawkes();