Fawkes API  Fawkes Development Version
realsense2_thread.h
1 
2 /***************************************************************************
3  * realsense2_thread.h - realsense2
4  *
5  * Plugin created: Wed May 22 10:09:22 2019
6 
7  * Copyright 2019 Christoph Gollok
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_REALSENSE2THREAD_H_
25 #define _PLUGINS_REALSENSE2THREAD_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 <librealsense2/rsutil.h>
35 #include <pcl/point_cloud.h>
36 #include <pcl/point_types.h>
37 
38 #include <librealsense2/rs.hpp>
39 #include <librealsense2/rs_advanced_mode.hpp>
40 #include <string>
41 #include <thread>
42 
43 namespace fawkes {
44 class SwitchInterface;
45 }
46 
49  public fawkes::LoggingAspect,
53  public fawkes::ClockAspect
54 {
55 public:
57 
58  virtual void init();
59  virtual void finalize();
60  virtual void loop();
61 
62 private:
63  bool start_camera();
64  bool get_camera(rs2::device &dev);
65  void enable_depth_stream();
66  void disable_depth_stream();
67  void stop_camera();
68 
69  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
70 protected:
71  virtual void
72  run()
73  {
74  Thread::run();
75  }
76 
77 protected:
78  bool read_switch();
79 
80 private:
81  fawkes::SwitchInterface *switch_if_;
82  bool cfg_use_switch_;
83 
84  typedef pcl::PointXYZ PointType;
85  typedef pcl::PointCloud<PointType> Cloud;
86 
87  typedef Cloud::Ptr CloudPtr;
88  typedef Cloud::ConstPtr CloudConstPtr;
89 
90  fawkes::RefPtr<Cloud> realsense_depth_refptr_;
91  CloudPtr realsense_depth_;
92 
93  rs2::pipeline *rs_pipe_;
94  rs2::context * rs_context_;
95  rs2::device rs_device_;
96  rs2::frameset rs_data_;
97  rs2_intrinsics intrinsics_;
98 
99  float camera_scale_;
100  std::string frame_id_;
101  std::string pcl_id_;
102  std::string switch_if_name_;
103  uint frame_rate_;
104  float laser_power_;
105  bool camera_running_ = false;
106  bool enable_camera_ = true;
107  bool depth_enabled_ = false;
108  uint restart_after_num_errors_;
109  uint error_counter_ = 0;
110 };
111 
112 #endif
Driver for the Intel RealSense Camera providing Depth Data as Pointcloud Inspired by realsense fawkes...
virtual void run()
Stub to see name in backtrace for easier debugging.
virtual void init()
Initialize the thread.
bool read_switch()
Read the switch interface and start/stop the camera if necessary.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
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
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
SwitchInterface Fawkes BlackBoard Interface.
Thread class encapsulation of pthreads.
Definition: thread.h:46
Fawkes library namespace.