Fawkes API Fawkes Development Version
realsense_thread.cpp
1
2/***************************************************************************
3 * realsense_thread.cpp - realsense
4 *
5 * Plugin created: Mon Jun 13 17:09:44 2016
6
7 * Copyright 2016 Johannes Rothe
8 * 2017 Till Hofmann
9 *
10 ****************************************************************************/
11
12/* This program is free software; you can redistribute it and/or modify
13 * it under the terms of the GNU General Public License as published by
14 * the Free Software Foundation; either version 2 of the License, or
15 * (at your option) any later version.
16 *
17 * This program is distributed in the hope that it will be useful,
18 * but WITHOUT ANY WARRANTY; without even the implied warranty of
19 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20 * GNU Library General Public License for more details.
21 *
22 * Read the full text in the LICENSE.GPL file in the doc directory.
23 */
24
25#include "realsense_thread.h"
26
27#include <interfaces/SwitchInterface.h>
28
29using namespace fawkes;
30
31/** @class RealsenseThread 'realsense_thread.h'
32 * Driver for the Intel RealSense Camera providing Depth Data as Pointcloud
33 * Inspired by IntelĀ® RealSenseā„¢ Camera - F200 ROS Nodelet
34 * @author Johannes Rothe
35 */
36
37RealsenseThread::RealsenseThread()
38: Thread("RealsenseThread", Thread::OPMODE_WAITFORWAKEUP),
39 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
40 switch_if_(NULL)
41{
42}
43
44void
46{
47 //set config values
48 const std::string cfg_prefix = "/realsense/";
49 frame_id_ = config->get_string(cfg_prefix + "frame_id");
50 pcl_id_ = config->get_string(cfg_prefix + "pcl_id");
51 laser_power_ = config->get_int(cfg_prefix + "device_options/laser_power");
52 restart_after_num_errors_ =
53 config->get_uint_or_default(std::string(cfg_prefix + "restart_after_num_errors").c_str(), 50);
54 cfg_poll_delay_ =
55 config->get_float_or_default(std::string(cfg_prefix + "delay_between_poll_errors").c_str(),
56 1.0);
57 cfg_use_switch_ = config->get_bool_or_default((cfg_prefix + "use_switch").c_str(), true);
58
59 if (cfg_use_switch_) {
60 logger->log_info(name(), "Switch enabled");
61 } else {
62 logger->log_info(name(), "Switch will be ignored");
63 }
64
65 switch_if_ = blackboard->open_for_writing<SwitchInterface>("realsense");
66 switch_if_->set_enabled(true);
67 switch_if_->write();
68
69 camera_scale_ = 1;
70 //initalize pointcloud
71 realsense_depth_refptr_ = new Cloud();
72 realsense_depth_ = pcl_utils::cloudptr_from_refptr(realsense_depth_refptr_);
73 realsense_depth_->header.frame_id = frame_id_;
74 realsense_depth_->width = 0;
75 realsense_depth_->height = 0;
76 realsense_depth_->resize(0);
77 pcl_manager->add_pointcloud(pcl_id_.c_str(), realsense_depth_refptr_);
78
79 rs_stream_type_ = RS_STREAM_DEPTH;
80 connect_and_start_camera();
81 next_poll_time_ = fawkes::Time(clock);
82}
83
84void
86{
87 if (cfg_use_switch_) {
89 }
90 if (enable_camera_ && !camera_running_) {
91 connect_and_start_camera();
92 // Start reading in the next loop
93 return;
94 } else if (!enable_camera_) {
95 if (camera_running_) {
96 stop_camera();
97 }
98 return;
99 }
100
101 if (error_counter_ > 0 && next_poll_time_ > fawkes::Time(clock)) {
102 return;
103 }
104
105 if (rs_poll_for_frames(rs_device_, &rs_error_) == 1) {
106 error_counter_ = 0;
107 const uint16_t *image =
108 reinterpret_cast<const uint16_t *>(rs_get_frame_data(rs_device_, rs_stream_type_, NULL));
109 log_error();
110 Cloud::iterator it = realsense_depth_->begin();
111 for (int y = 0; y < z_intrinsic_.height; y++) {
112 for (int x = 0; x < z_intrinsic_.width; x++) {
113 float scaled_depth = camera_scale_ * ((float)*image);
114 float depth_point[3];
115 float depth_pixel[2] = {(float)x, (float)y};
116 rs_deproject_pixel_to_point(depth_point, &z_intrinsic_, depth_pixel, scaled_depth);
117 it->x = depth_point[0];
118 it->y = depth_point[1];
119 it->z = depth_point[2];
120 ++image;
121 ++it;
122 }
123 }
124 pcl_utils::set_time(realsense_depth_refptr_, fawkes::Time(clock));
125 } else {
126 error_counter_++;
127 next_poll_time_ = fawkes::Time(clock) + cfg_poll_delay_;
129 "Poll for frames not successful (%s)",
130 rs_get_error_message(rs_error_));
131 if (error_counter_ >= restart_after_num_errors_) {
132 logger->log_warn(name(), "Polling failed, restarting device");
133 error_counter_ = 0;
134 stop_camera();
135 connect_and_start_camera();
136 }
137 }
138}
139
140void
142{
143 realsense_depth_refptr_.reset();
144 pcl_manager->remove_pointcloud(pcl_id_.c_str());
145 stop_camera();
146 blackboard->close(switch_if_);
147 //TODO Documentation with librealsense
148}
149
150/* Create RS context and start the depth stream
151 * @return true when succesfull
152 */
153bool
154RealsenseThread::connect_and_start_camera()
155{
156 rs_context_ = rs_create_context(RS_API_VERSION, &rs_error_);
157 log_error();
158 num_of_cameras_ = rs_get_device_count(rs_context_, &rs_error_);
159 logger->log_info(name(), "No. of cameras: %i ", num_of_cameras_);
160 if (num_of_cameras_ < 1) {
161 logger->log_error(name(), "No camera detected!");
162 rs_delete_context(rs_context_, &rs_error_);
163 camera_running_ = false;
164 return camera_running_;
165 }
166
167 rs_device_ = get_camera();
168 rs_set_device_option(rs_device_, RS_OPTION_F200_LASER_POWER, laser_power_, &rs_error_);
169 log_error();
170 enable_depth_stream();
171
172 rs_start_device(rs_device_, &rs_error_);
173 log_error();
174
176 "Stream format: %s",
177 rs_format_to_string(
178 rs_get_stream_format(rs_device_, rs_stream_type_, &rs_error_)));
179
180 camera_running_ = true;
181 camera_scale_ = rs_get_device_depth_scale(rs_device_, NULL);
182 rs_get_stream_intrinsics(rs_device_, rs_stream_type_, &z_intrinsic_, &rs_error_);
183 realsense_depth_->width = z_intrinsic_.width;
184 realsense_depth_->height = z_intrinsic_.height;
185 realsense_depth_->resize(z_intrinsic_.width * z_intrinsic_.height);
186 logger->log_info(name(), "Height: %i, Width: %i", z_intrinsic_.height, z_intrinsic_.width);
187 return camera_running_;
188}
189
190/* Get the rs_device pointer and printout camera details
191 * @return rs_device
192 */
193rs_device *
194RealsenseThread::get_camera()
195{
196 //assume we only have one camera connected so take index 0
197 rs_device *rs_detected_device = rs_get_device(rs_context_, 0, &rs_error_);
198 //print device details
200 "\n\nDetected Device:\n"
201 "Serial No: %s\n"
202 "Firmware %s\n"
203 "Name %s\n"
204 "USB Port ID %s\n",
205 rs_get_device_serial(rs_detected_device, &rs_error_),
206 rs_get_device_firmware_version(rs_detected_device, &rs_error_),
207 rs_get_device_name(rs_detected_device, &rs_error_),
208 rs_get_device_usb_port_id(rs_detected_device, &rs_error_));
209 log_error();
210 return rs_detected_device;
211}
212
213/*
214 * Enable the depth stream from rs_device
215 */
216void
217RealsenseThread::enable_depth_stream()
218{
219 rs_enable_stream_preset(rs_device_, rs_stream_type_, RS_PRESET_BEST_QUALITY, &rs_error_);
220 log_error();
221 if (rs_is_stream_enabled(rs_device_, rs_stream_type_, &rs_error_)) {
223 "Depth stream enabled! Streaming with %i fps",
224 rs_get_stream_framerate(rs_device_, rs_stream_type_, &rs_error_));
225 log_error();
226 } else {
227 log_error();
228 throw Exception("Couldn't start depth stream! Stream type: %s",
229 rs_stream_to_string(rs_stream_type_));
230 }
231}
232
233/*
234 * printout and free the rs_error if available
235 */
236void
237RealsenseThread::log_error()
238{
239 if (rs_error_) {
240 logger->log_warn(name(), "Realsense Error: %s", rs_get_error_message(rs_error_));
241 rs_free_error(rs_error_);
242 rs_error_ = nullptr;
243 }
244}
245
246/*
247 * Testing function to log the depth pixel distancens
248 */
249void
250RealsenseThread::log_depths(const uint16_t *image)
251{
252 std::string out;
253 for (uint16_t i = 0; i < rs_get_stream_height(rs_device_, rs_stream_type_, NULL); i++) {
254 for (uint16_t i = 0; i < rs_get_stream_width(rs_device_, rs_stream_type_, NULL); i++) {
255 float depth_in_meters = camera_scale_ * image[i];
256 out += std::to_string(depth_in_meters) + " ";
257 }
258 out += "\n";
259 }
260 logger->log_info(name(), "%s\n\n\n\n\n", out.c_str());
261}
262
263/*
264 * Stop the device and delete the context properly
265 */
266void
267RealsenseThread::stop_camera()
268{
269 if (camera_running_) {
270 logger->log_info(name(), "Stopping realsense camera ...");
271 rs_stop_device(rs_device_, &rs_error_);
272 rs_delete_context(rs_context_, &rs_error_);
273 log_error();
274 logger->log_info(name(), "Realsense camera stopped!");
275 camera_running_ = false;
276 }
277}
278
279/**
280 * Read the switch interface and start/stop the camera if necessary.
281 * @return true iff the interface is currently enabled.
282 */
283bool
285{
286 while (!switch_if_->msgq_empty()) {
287 Message *msg = switch_if_->msgq_first();
288 if (dynamic_cast<SwitchInterface::EnableSwitchMessage *>(msg)) {
289 enable_camera_ = true;
290 } else if (dynamic_cast<SwitchInterface::DisableSwitchMessage *>(msg)) {
291 enable_camera_ = false;
292 }
293 switch_if_->msgq_pop();
294 }
295 switch_if_->set_enabled(enable_camera_);
296 switch_if_->write();
297 return switch_if_->is_enabled();
298}
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.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual float get_float_or_default(const char *path, const float &default_val)
Get value from configuration which is of type float, or the given default if the path does not exist.
Definition: config.cpp:696
virtual unsigned int get_uint_or_default(const char *path, const unsigned int &default_val)
Get value from configuration which is of type unsigned int, or the given default if the path does not...
Definition: config.cpp:706
virtual bool get_bool_or_default(const char *path, const bool &default_val)
Get value from configuration which is of type bool, or the given default if the path does not exist.
Definition: config.cpp:726
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:47
void remove_pointcloud(const char *id)
Remove the point cloud.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
SwitchInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled value.
bool is_enabled() const
Get enabled value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
Fawkes library namespace.