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
54{
55public:
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() */
64protected:
65 virtual void
67 {
68 Thread::run();
69 }
70
71private:
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
Point cloud adapter class.
Definition: pcl_adapter.h:39
Thread to exchange point clouds between Fawkes and ROS.
Definition: pcl_thread.h:54
virtual void loop()
Code to execute in the thread.
Definition: pcl_thread.cpp:77
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: pcl_thread.h:66
virtual void finalize()
Finalize the thread.
Definition: pcl_thread.cpp:62
RosPointCloudThread()
Constructor.
Definition: pcl_thread.cpp:36
virtual void init()
Initialize the thread.
Definition: pcl_thread.cpp:48
virtual ~RosPointCloudThread()
Destructor.
Definition: pcl_thread.cpp:43
Thread aspect to use blocked timing.
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
Thread aspect to access configuration data.
Definition: configurable.h:33
Thread aspect to log output.
Definition: logging.h:33
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:38
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:47
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread aspect to get access to a ROS node handle.
Definition: ros.h:39
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
Thread class encapsulation of pthreads.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
Adapter class for PCL point types.