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 <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>
43 
44 #include <list>
45 #include <queue>
46 
48  public fawkes::ClockAspect,
49  public fawkes::LoggingAspect,
53  public fawkes::ROSAspect
54 {
55 public:
57  virtual ~RosPointCloudThread();
58 
59  virtual void init();
60  virtual void loop();
61  virtual void finalize();
62 
63  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
64 protected:
65  virtual void
66  run()
67  {
68  Thread::run();
69  }
70 
71 private:
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);
78 
79  template <typename PointT>
80  void
81  add_pointcloud(const sensor_msgs::PointCloud2ConstPtr &msg, const std::string topic_name)
82  {
84  pcl = new pcl::PointCloud<PointT>();
85  pcl::fromROSMsg(*msg, **pcl);
86  pcl_manager->add_pointcloud(topic_name.c_str(), pcl);
87  ros_pointcloud_available_ref_[topic_name] =
89  }
90 
91  template <typename PointT>
92  void
93  update_pointcloud(const sensor_msgs::PointCloud2ConstPtr &msg, const std::string topic_name)
94  {
97  ros_pointcloud_available_ref_[topic_name])
98  ->cloud;
99  pcl::fromROSMsg(*msg, **pcl);
100  }
101 
102  PointCloudAdapter *adapter_;
103 
104  /// @cond INTERNALS
105  typedef struct
106  {
107  ros::Publisher pub;
108  sensor_msgs::PointCloud2 msg;
109  fawkes::Time last_sent;
110  } PublisherInfo;
111  /// @endcond
112  std::map<std::string, PublisherInfo> fawkes_pubs_; // the list and ref of topics from fawkes->ros
113  std::list<std::string> ros_pointcloud_available_; // the list of topics from ros->fawkes
114  std::map<std::string, fawkes::pcl_utils::StorageAdapter *>
115  ros_pointcloud_available_ref_; // the list of refs of topics from ros->fawkes
116  std::map<std::string, ros::Subscriber>
117  ros_pointcloud_subs_; // the list of subscribers in ros, ros_pointcloud_available that are currently used in fawkes
118 
119  fawkes::Time ros_pointcloud_last_searched_;
120 
121  float cfg_ros_research_ival_;
122 };
123 
124 #endif
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:33
Thread aspect to get access to a ROS node handle.
Definition: ros.h:38
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: pcl_thread.h:66
RosPointCloudThread()
Constructor.
Definition: pcl_thread.cpp:36
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:37
virtual void loop()
Code to execute in the thread.
Definition: pcl_thread.cpp:78
A class for handling time.
Definition: time.h:92
virtual ~RosPointCloudThread()
Destructor.
Definition: pcl_thread.cpp:43
Thread class encapsulation of pthreads.
Definition: thread.h:45
virtual void init()
Initialize the thread.
Definition: pcl_thread.cpp:48
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:47
Thread aspect to log output.
Definition: logging.h:32
virtual void finalize()
Finalize the thread.
Definition: pcl_thread.cpp:62
Thread aspect to access configuration data.
Definition: configurable.h:32
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:47
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT >> cloud)
Add point cloud.