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 
47 class Realsense2Thread : public fawkes::Thread,
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
Realsense2Thread::init
virtual void init()
Initialize the thread.
Definition: realsense2_thread.cpp:43
fawkes::SwitchInterface
Definition: SwitchInterface.h:37
fawkes::RefPtr
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:55
Realsense2Thread::run
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: realsense2_thread.h:71
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:54
Realsense2Thread::loop
virtual void loop()
Code to execute in the thread.
Definition: realsense2_thread.cpp:83
fawkes::BlackBoardAspect
Definition: blackboard.h:36
fawkes::PointCloudAspect
Definition: pointcloud.h:36
fawkes
fawkes::LoggingAspect
Definition: logging.h:36
pcl::PointCloud< PointType >
fawkes::Thread
Definition: thread.h:44
Realsense2Thread::finalize
virtual void finalize()
Finalize the thread.
Definition: realsense2_thread.cpp:135
fawkes::ConfigurableAspect
Definition: configurable.h:36
Realsense2Thread
Definition: realsense2_thread.h:46
fawkes::ClockAspect
Definition: clock.h:38
Realsense2Thread::read_switch
bool read_switch()
Read the switch interface and start/stop the camera if necessary.
Definition: realsense2_thread.cpp:334