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 
29 using 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 
37 RealsenseThread::RealsenseThread()
38 : Thread("RealsenseThread", Thread::OPMODE_WAITFORWAKEUP),
39  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
40  switch_if_(NULL)
41 {
42 }
43 
44 void
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 
84 void
86 {
87  if (cfg_use_switch_) {
88  read_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_;
128  logger->log_warn(name(),
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 
140 void
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  */
153 bool
154 RealsenseThread::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 
175  logger->log_info(name(),
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  */
193 rs_device *
194 RealsenseThread::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
199  logger->log_info(name(),
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  */
216 void
217 RealsenseThread::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_)) {
222  logger->log_info(name(),
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  */
236 void
237 RealsenseThread::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  */
249 void
250 RealsenseThread::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  */
266 void
267 RealsenseThread::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  */
283 bool
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.