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 
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 camera_started_ = false;
107  int laser_power_;
108 };
109 
110 #endif
Thread aspect to access to BlackBoard.
Definition: blackboard.h:33
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:33
Fawkes library namespace.
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:37
virtual void init()
Initialize the thread.
Thread class encapsulation of pthreads.
Definition: thread.h:45
Thread aspect to use blocked timing.
SwitchInterface Fawkes BlackBoard Interface.
Driver for the Intel RealSense Camera providing Depth Data as Pointcloud Inspired by IntelĀ® RealSens...
Thread aspect to log output.
Definition: logging.h:32
virtual void run()
Stub to see name in backtrace for easier debugging.
bool read_switch()
Read the switch interface and start/stop the camera if necessary.
Thread aspect to access configuration data.
Definition: configurable.h:32
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.