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
45namespace fawkes {
46class SwitchInterface;
47}
48
56{
57public:
59
60 virtual void init();
61 virtual void finalize();
62 virtual void loop();
63
64private:
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() */
74protected:
75 virtual void
77 {
78 Thread::run();
79 }
80
81protected:
82 bool read_switch();
83
84private:
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
Driver for the Intel RealSense Camera providing Depth Data as Pointcloud Inspired by IntelĀ® RealSense...
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.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in 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
A class for handling time.
Definition: time.h:93
Fawkes library namespace.