Fawkes API  Fawkes Development Version
realsense_thread.h
1 
2 /***************************************************************************
3  * realsense_thread.h - realsense
4  *
5  * Plugin created: Mon Jun 13 17:09:44 2016
6 
7  * Copyright 2016 Johannes Rothe
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #ifndef _PLUGINS_REALSENSETHREAD_H_
25 #define _PLUGINS_REALSENSETHREAD_H_
26 
27 #include <aspect/blackboard.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 <core/threading/thread.h>
34 #include <pcl/point_cloud.h>
35 #include <pcl/point_types.h>
36 
37 #ifdef HAVE_REALSENSE1
38 # include <librealsense1/rs.hpp>
39 #else
40 # include <librealsense/rs.hpp>
41 #endif
42 
43 #include <string>
44 
45 namespace fawkes {
46 class SwitchInterface;
47 }
48 
49 class RealsenseThread : public fawkes::Thread,
51  public fawkes::LoggingAspect,
55  public fawkes::ClockAspect
56 {
57 public:
59 
60  virtual void init();
61  virtual void finalize();
62  virtual void loop();
63 
64 private:
65  bool connect_and_start_camera();
66  rs_device *get_camera();
67  void enable_depth_stream();
68  void log_error();
69  void log_depths(const uint16_t *image);
70  void fill_pointcloud();
71  void stop_camera();
72 
73  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
74 protected:
75  virtual void
76  run()
77  {
78  Thread::run();
79  }
80 
81 protected:
82  bool read_switch();
83 
84 private:
85  fawkes::SwitchInterface *switch_if_;
86  bool cfg_use_switch_;
87 
88  typedef pcl::PointXYZ PointType;
89  typedef pcl::PointCloud<PointType> Cloud;
90 
91  typedef Cloud::Ptr CloudPtr;
92  typedef Cloud::ConstPtr CloudConstPtr;
93 
94  fawkes::RefPtr<Cloud> realsense_depth_refptr_;
95  CloudPtr realsense_depth_;
96 
97  rs_error * rs_error_ = 0;
98  rs_context * rs_context_;
99  rs_device * rs_device_;
100  rs_intrinsics z_intrinsic_;
101  rs_stream rs_stream_type_;
102  int num_of_cameras_;
103  float camera_scale_;
104  std::string frame_id_;
105  std::string pcl_id_;
106  bool enable_camera_ = true;
107  bool camera_running_ = false;
108  int laser_power_;
109  uint restart_after_num_errors_;
110  uint error_counter_ = 0;
111  fawkes::Time next_poll_time_;
112  float cfg_poll_delay_;
113 };
114 
115 #endif
fawkes::SwitchInterface
Definition: SwitchInterface.h:37
fawkes::RefPtr
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:55
RealsenseThread::run
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: realsense_thread.h:75
RealsenseThread::read_switch
bool read_switch()
Read the switch interface and start/stop the camera if necessary.
Definition: realsense_thread.cpp:283
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:54
RealsenseThread
Definition: realsense_thread.h:48
fawkes::BlackBoardAspect
Definition: blackboard.h:36
fawkes::PointCloudAspect
Definition: pointcloud.h:36
fawkes
fawkes::LoggingAspect
Definition: logging.h:36
pcl::PointCloud< PointType >
RealsenseThread::init
virtual void init()
Initialize the thread.
Definition: realsense_thread.cpp:44
RealsenseThread::loop
virtual void loop()
Code to execute in the thread.
Definition: realsense_thread.cpp:84
RealsenseThread::finalize
virtual void finalize()
Finalize the thread.
Definition: realsense_thread.cpp:140
fawkes::Time
Definition: time.h:96
fawkes::Thread
Definition: thread.h:44
fawkes::ConfigurableAspect
Definition: configurable.h:36
fawkes::ClockAspect
Definition: clock.h:38