Fawkes API  Fawkes Development Version
pcl_thread.h
1 
2 /***************************************************************************
3  * pcl_thread.cpp - Thread to exchange point clouds
4  *
5  * Created: Mon Nov 07 02:26:35 2011
6  * Copyright 2011 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #ifndef __PLUGINS_ROS_PCL_THREAD_H_
23 #define __PLUGINS_ROS_PCL_THREAD_H_
24 
25 #include <pcl_utils/pcl_adapter.h>
26 
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>
39 
40 #include <list>
41 #include <queue>
42 
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>
48 
50 : public fawkes::Thread,
51  public fawkes::ClockAspect,
52  public fawkes::LoggingAspect,
56  public fawkes::ROSAspect
57 {
58  public:
60  virtual ~RosPointCloudThread();
61 
62  virtual void init();
63  virtual void loop();
64  virtual void finalize();
65 
66  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
67  protected: virtual void run() { Thread::run(); }
68 
69  private:
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);
75 
76  template<typename PointT>
77  void add_pointcloud(const sensor_msgs::PointCloud2ConstPtr &msg, const std::string topic_name)
78  {
80  pcl = new pcl::PointCloud<PointT>();
81  pcl::fromROSMsg(*msg, **pcl);
82  pcl_manager->add_pointcloud(topic_name.c_str(), pcl);
83  ros_pointcloud_available_ref_[topic_name] = new fawkes::pcl_utils::PointCloudStorageAdapter<PointT>(pcl);
84  }
85 
86  template<typename PointT>
87  void update_pointcloud(const sensor_msgs::PointCloud2ConstPtr &msg, const std::string topic_name)
88  {
90  pcl = dynamic_cast<fawkes::pcl_utils::PointCloudStorageAdapter<PointT> *>(ros_pointcloud_available_ref_[topic_name])->cloud;
91  pcl::fromROSMsg(*msg, **pcl);
92  }
93 
94  PointCloudAdapter *__adapter;
95 
96  /// @cond INTERNALS
97  typedef struct {
98  ros::Publisher pub;
99  sensor_msgs::PointCloud2 msg;
100  fawkes::Time last_sent;
101  } PublisherInfo;
102  /// @endcond
103  std::map<std::string, PublisherInfo> fawkes_pubs_; // the list and ref of topics from fawkes->ros
104  std::list<std::string> ros_pointcloud_available_; // the list of topics from ros->fawkes
105  std::map<std::string, fawkes::pcl_utils::StorageAdapter *> ros_pointcloud_available_ref_;// the list of refs of topics from ros->fawkes
106  std::map<std::string, ros::Subscriber> ros_pointcloud_subs_; // the list of subscribers in ros, ros_pointcloud_available that are currently used in fawkes
107 
108  fawkes::Time ros_pointcloud_last_searched_;
109 
110  float cfg_ros_research_ival_;
111 };
112 
113 #endif
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:36
Thread aspect to get access to a ROS node handle.
Definition: ros.h:39
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: pcl_thread.h:67
RosPointCloudThread()
Constructor.
Definition: pcl_thread.cpp:36
Definition: pointcloud.h:30
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:40
virtual void loop()
Code to execute in the thread.
Definition: pcl_thread.cpp:81
A class for handling time.
Definition: time.h:91
virtual ~RosPointCloudThread()
Destructor.
Definition: pcl_thread.cpp:43
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void init()
Initialize the thread.
Definition: pcl_thread.cpp:50
Adapter class for PCL point types.
Thread aspect to use blocked timing.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:50
Thread aspect to log output.
Definition: logging.h:35
virtual void finalize()
Finalize the thread.
Definition: pcl_thread.cpp:65
Thread aspect to access configuration data.
Definition: configurable.h:35
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
Point cloud adapter class.
Definition: pcl_adapter.h:38
Thread to exchange point clouds between Fawkes and ROS.
Definition: pcl_thread.h:49