22 #ifndef __PLUGINS_ROS_PCL_THREAD_H_ 23 #define __PLUGINS_ROS_PCL_THREAD_H_ 25 #include <pcl_utils/pcl_adapter.h> 27 #include <core/threading/thread.h> 28 #include <aspect/blocked_timing.h> 29 #include <aspect/clock.h> 30 #include <aspect/configurable.h> 31 #include <aspect/logging.h> 32 #include <aspect/pointcloud.h> 33 #include <plugins/ros/aspect/ros.h> 34 #include <blackboard/interface_listener.h> 35 #include <blackboard/interface_observer.h> 36 #include <interfaces/TransformInterface.h> 37 #include <core/threading/mutex.h> 38 #include <utils/time/time.h> 43 #include <ros/node_handle.h> 44 #include <pcl/point_cloud.h> 45 #include <pcl/point_types.h> 46 #include <sensor_msgs/PointCloud2.h> 47 #include <pcl_conversions/pcl_conversions.h> 67 protected:
virtual void run() { Thread::run(); }
70 void ros_pointcloud_search();
71 void ros_pointcloud_check_for_listener_in_fawkes();
72 void fawkes_pointcloud_publish_to_ros();
73 void fawkes_pointcloud_search();
74 void ros_pointcloud_on_data_msg(
const sensor_msgs::PointCloud2ConstPtr &msg,
const std::string topic_name);
76 template<
typename Po
intT>
77 void add_pointcloud(
const sensor_msgs::PointCloud2ConstPtr &msg,
const std::string topic_name)
81 pcl::fromROSMsg(*msg, **pcl);
86 template<
typename Po
intT>
87 void update_pointcloud(
const sensor_msgs::PointCloud2ConstPtr &msg,
const std::string topic_name)
91 pcl::fromROSMsg(*msg, **pcl);
99 sensor_msgs::PointCloud2 msg;
103 std::map<std::string, PublisherInfo> fawkes_pubs_;
104 std::list<std::string> ros_pointcloud_available_;
105 std::map<std::string, fawkes::pcl_utils::StorageAdapter *> ros_pointcloud_available_ref_;
106 std::map<std::string, ros::Subscriber> ros_pointcloud_subs_;
110 float cfg_ros_research_ival_;
Thread aspect that allows to obtain the current time from the clock.
Thread aspect to get access to a ROS node handle.
virtual void run()
Stub to see name in backtrace for easier debugging.
RosPointCloudThread()
Constructor.
Thread aspect to provide and access point clouds.
virtual void loop()
Code to execute in the thread.
A class for handling time.
virtual ~RosPointCloudThread()
Destructor.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
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.
Thread aspect to log output.
virtual void finalize()
Finalize the thread.
Thread aspect to access configuration data.
RefPtr<> is a reference-counting shared smartpointer.
Point cloud adapter class.
Thread to exchange point clouds between Fawkes and ROS.