22 #include "pcl_thread.h" 24 #include <core/threading/mutex_locker.h> 26 #include <sensor_msgs/PointCloud2.h> 37 :
Thread(
"RosPointCloudThread",
Thread::OPMODE_WAITFORWAKEUP),
55 cfg_ros_research_ival_ =
config->
get_float(
"/ros/pcl/ros-search-interval");
57 fawkes_pointcloud_search();
59 ros_pointcloud_search();
60 ros_pointcloud_last_searched_.
stamp();
67 for (
const std::string &item : ros_pointcloud_available_) {
70 for (
const std::pair<std::string, fawkes::pcl_utils::StorageAdapter *> &item : ros_pointcloud_available_ref_) {
73 for (std::pair<std::string, ros::Subscriber> item : ros_pointcloud_subs_) {
74 item.second.shutdown();
87 ros_pointcloud_last_searched_ = now;
88 ros_pointcloud_search();
89 ros_pointcloud_check_for_listener_in_fawkes();
94 fawkes_pointcloud_publish_to_ros();
99 RosPointCloudThread::ros_pointcloud_search()
101 std::list<std::string> ros_pointclouds_new;
104 ros::master::V_TopicInfo ros_topics;
105 if ( ! ros::master::getTopics(ros_topics) ) {
111 for (
const ros::master::TopicInfo &info : ros_topics ) {
113 if ( 0 == info.datatype.compare(
"sensor_msgs/PointCloud2") ) {
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;
121 if (topic_not_from_fawkes) {
122 ros_pointclouds_new.push_back(info.name);
128 std::list<std::string> items_to_remove;
129 for (
const std::string &item_old : ros_pointcloud_available_) {
131 for (std::string item_new : ros_pointclouds_new) {
132 if (0 == item_old.compare(item_new)) {
138 items_to_remove.push_back(item_old);
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);
147 for (
const std::string &ros_topic : ros_pointclouds_new) {
149 for (
const std::string &in_list : ros_pointcloud_available_) {
150 if (0 == ros_topic.compare(in_list)) {
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)
166 RosPointCloudThread::ros_pointcloud_check_for_listener_in_fawkes()
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>()) {
172 }
else if (item.second->is_pointtype<pcl::PointXYZRGB>()) {
174 }
else if (item.second->is_pointtype<pcl::PointXYZI>()) {
180 if ( use_count <= 2 ) {
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);
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)
195 RosPointCloudThread::fawkes_pointcloud_search()
199 std::vector<std::string>::iterator p;
200 for (p = pcls.begin(); p != pcls.end(); ++p) {
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,
"_");
209 pi.pub =
rosnode->advertise<sensor_msgs::PointCloud2>(topic_name, 1);
211 logger->
log_info(
name(),
"Publishing point cloud %s at %s", p->c_str(), topic_name.c_str());
213 std::string frame_id;
214 unsigned int width, height;
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;
231 fawkes_pubs_[*p] = pi;
236 RosPointCloudThread::fawkes_pointcloud_publish_to_ros()
238 std::map<std::string, PublisherInfo>::iterator p;
239 for (p = fawkes_pubs_.begin(); p != fawkes_pubs_.end(); ++p) {
240 PublisherInfo &pi = p->second;
242 unsigned int width, height;
244 size_t point_size, num_points;
247 std::string frame_id;
248 __adapter->
get_data(p->first, frame_id, width, height, time,
249 &point_data, point_size, num_points);
251 if (pi.last_sent != time) {
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);
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;
266 pi.pub.publish(pi.msg);
272 __adapter->
close(p->first);
279 RosPointCloudThread::ros_pointcloud_on_data_msg(
const sensor_msgs::PointCloud2ConstPtr &msg,
const std::string topic_name)
283 bool r =
false, i =
false;
284 for (
const sensor_msgs::PointField &field : msg->fields) {
286 if ( 0 == field.name.compare(
"r") ) { r =
true; }
287 if ( 0 == field.name.compare(
"i") ) { i =
true; }
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);
300 add_pointcloud<pcl::PointXYZ>(msg, topic_name);
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);
316 ros_pointcloud_check_for_listener_in_fawkes();
std::vector< PointFieldInfo > V_PointFieldInfo
Vector of PointFieldInfo.
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.
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.
Fawkes library namespace.
virtual void loop()
Code to execute in the thread.
A class for handling time.
virtual ~RosPointCloudThread()
Destructor.
Thread class encapsulation of pthreads.
Logger * logger
This is the Logger member used to access the logger.
virtual void init()
Initialize the thread.
Adapter class for PCL point types.
Thread aspect to use blocked timing.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
double time_diff_sec(const timeval &a, const timeval &b)
Calculate time difference of two time structs.
long get_nsec() const
Get nanoseconds.
const char * name() const
Get name of thread.
virtual void finalize()
Finalize the thread.
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.
Point cloud adapter class.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Time & stamp()
Set this time to the current time.
Configuration * config
This is the Configuration member used to access the configuration.
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.