22 #ifndef _PLUGINS_ROS_PCL_THREAD_H_
23 #define _PLUGINS_ROS_PCL_THREAD_H_
25 #include <aspect/blocked_timing.h>
26 #include <aspect/clock.h>
27 #include <aspect/configurable.h>
28 #include <aspect/logging.h>
29 #include <aspect/pointcloud.h>
30 #include <blackboard/interface_listener.h>
31 #include <blackboard/interface_observer.h>
32 #include <core/threading/mutex.h>
33 #include <core/threading/thread.h>
34 #include <interfaces/TransformInterface.h>
35 #include <pcl/point_cloud.h>
36 #include <pcl/point_types.h>
37 #include <pcl_conversions/pcl_conversions.h>
38 #include <pcl_utils/pcl_adapter.h>
39 #include <plugins/ros/aspect/ros.h>
40 #include <ros/node_handle.h>
41 #include <sensor_msgs/PointCloud2.h>
42 #include <utils/time/time.h>
72 void ros_pointcloud_search();
73 void ros_pointcloud_check_for_listener_in_fawkes();
74 void fawkes_pointcloud_publish_to_ros();
75 void fawkes_pointcloud_search();
76 void ros_pointcloud_on_data_msg(
const sensor_msgs::PointCloud2ConstPtr &msg,
77 const std::string & topic_name);
79 template <
typename Po
intT>
81 add_pointcloud(
const sensor_msgs::PointCloud2ConstPtr &msg,
const std::string topic_name)
85 pcl::fromROSMsg(*msg, **pcl);
87 ros_pointcloud_available_ref_[topic_name] =
91 template <
typename Po
intT>
93 update_pointcloud(
const sensor_msgs::PointCloud2ConstPtr &msg,
const std::string topic_name)
97 ros_pointcloud_available_ref_[topic_name])
99 pcl::fromROSMsg(*msg, **pcl);
108 sensor_msgs::PointCloud2 msg;
112 std::map<std::string, PublisherInfo> fawkes_pubs_;
113 std::list<std::string> ros_pointcloud_available_;
114 std::map<std::string, fawkes::pcl_utils::StorageAdapter *>
115 ros_pointcloud_available_ref_;
116 std::map<std::string, ros::Subscriber>
117 ros_pointcloud_subs_;
121 float cfg_ros_research_ival_;