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  cfg_use_switch_(true)
42 {
43 }
44 
45 void
47 {
48  //set config values
49  const std::string cfg_prefix = "/realsense/";
50  frame_id_ = config->get_string(cfg_prefix + "frame_id");
51  pcl_id_ = config->get_string(cfg_prefix + "pcl_id");
52  laser_power_ = config->get_int(cfg_prefix + "device_options/laser_power");
53 
54  try {
55  cfg_use_switch_ = config->get_bool((cfg_prefix + "use_switch").c_str());
56  } catch (Exception &e) {
57  } // ignore, use default
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  rs_stream_type_ = RS_STREAM_DEPTH;
70  connect_and_start_camera();
71 
72  camera_scale_ = rs_get_device_depth_scale(rs_device_, NULL);
73 
74  rs_get_stream_intrinsics(rs_device_, rs_stream_type_, &z_intrinsic_, &rs_error_);
75  logger->log_info(name(), "Height: %i, Width: %i", z_intrinsic_.height, z_intrinsic_.width);
76 
77  //initalize pointcloud
78  realsense_depth_refptr_ = new Cloud();
79  realsense_depth_ = pcl_utils::cloudptr_from_refptr(realsense_depth_refptr_);
80  realsense_depth_->header.frame_id = frame_id_;
81  realsense_depth_->width = z_intrinsic_.width;
82  realsense_depth_->height = z_intrinsic_.height;
83  pcl_manager->add_pointcloud(pcl_id_.c_str(), realsense_depth_refptr_);
84  //fill pointcloud with empty points
85  for (int i = 0; i < z_intrinsic_.height; i++) {
86  for (int j = 0; j < z_intrinsic_.width; j++) {
87  realsense_depth_->push_back(PointType(0, 0, 0));
88  }
89  }
90  realsense_depth_->resize(z_intrinsic_.width * z_intrinsic_.height);
91 }
92 
93 void
95 {
96  if (cfg_use_switch_ && !read_switch()) {
97  return;
98  }
99  if (rs_poll_for_frames(rs_device_, &rs_error_) == 1) {
100  const uint16_t *image =
101  reinterpret_cast<const uint16_t *>(rs_get_frame_data(rs_device_, rs_stream_type_, NULL));
102  log_error();
103  Cloud::iterator it = realsense_depth_->begin();
104  for (int y = 0; y < z_intrinsic_.height; y++) {
105  for (int x = 0; x < z_intrinsic_.width; x++) {
106  float scaled_depth = camera_scale_ * ((float)*image);
107  float depth_point[3];
108  float depth_pixel[2] = {(float)x, (float)y};
109  rs_deproject_pixel_to_point(depth_point, &z_intrinsic_, depth_pixel, scaled_depth);
110  it->x = depth_point[0];
111  it->y = depth_point[1];
112  it->z = depth_point[2];
113  ++image;
114  ++it;
115  }
116  }
117  pcl_utils::set_time(realsense_depth_refptr_, fawkes::Time(clock));
118  } else {
119  logger->log_warn(name(),
120  "Poll for frames not successful (%s)",
121  rs_get_error_message(rs_error_));
122  }
123 }
124 
125 void
127 {
128  realsense_depth_refptr_.reset();
129  pcl_manager->remove_pointcloud(pcl_id_.c_str());
130  stop_camera();
131  blackboard->close(switch_if_);
132  //TODO Documentation with librealsense
133 }
134 
135 /* Create RS context and start the depth stream
136  * @return true when succesfull
137  */
138 bool
139 RealsenseThread::connect_and_start_camera()
140 {
141  rs_context_ = rs_create_context(RS_API_VERSION, &rs_error_);
142  log_error();
143  num_of_cameras_ = rs_get_device_count(rs_context_, &rs_error_);
144  logger->log_info(name(), "No. of cameras: %i ", num_of_cameras_);
145  if (num_of_cameras_ < 1) {
146  throw Exception("No camera detected!");
147  }
148 
149  rs_device_ = get_camera();
150  rs_set_device_option(rs_device_, RS_OPTION_F200_LASER_POWER, laser_power_, &rs_error_);
151  log_error();
152  enable_depth_stream();
153 
154  rs_start_device(rs_device_, &rs_error_);
155  log_error();
156 
157  logger->log_info(name(),
158  "Stream format: %s",
159  rs_format_to_string(
160  rs_get_stream_format(rs_device_, rs_stream_type_, &rs_error_)));
161 
162  camera_started_ = true;
163  return true;
164 }
165 
166 /* Get the rs_device pointer and printout camera details
167  * @return rs_device
168  */
169 rs_device *
170 RealsenseThread::get_camera()
171 {
172  //assume we only have one camera connected so take index 0
173  rs_device *rs_detected_device = rs_get_device(rs_context_, 0, &rs_error_);
174  //print device details
175  logger->log_info(name(),
176  "\n\nDetected Device:\n"
177  "Serial No: %s\n"
178  "Firmware %s\n"
179  "Name %s\n"
180  "USB Port ID %s\n",
181  rs_get_device_serial(rs_detected_device, &rs_error_),
182  rs_get_device_firmware_version(rs_detected_device, &rs_error_),
183  rs_get_device_name(rs_detected_device, &rs_error_),
184  rs_get_device_usb_port_id(rs_detected_device, &rs_error_));
185  log_error();
186  return rs_detected_device;
187 }
188 
189 /*
190  * Enable the depth stream from rs_device
191  */
192 void
193 RealsenseThread::enable_depth_stream()
194 {
195  rs_enable_stream_preset(rs_device_, rs_stream_type_, RS_PRESET_BEST_QUALITY, &rs_error_);
196  log_error();
197  if (rs_is_stream_enabled(rs_device_, rs_stream_type_, &rs_error_)) {
198  logger->log_info(name(),
199  "Depth stream enabled! Streaming with %i fps",
200  rs_get_stream_framerate(rs_device_, rs_stream_type_, &rs_error_));
201  log_error();
202  } else {
203  log_error();
204  throw Exception("Couldn't start depth stream! Stream type: %s",
205  rs_stream_to_string(rs_stream_type_));
206  }
207 }
208 
209 /*
210  * printout and free the rs_error if available
211  */
212 void
213 RealsenseThread::log_error()
214 {
215  if (rs_error_) {
216  logger->log_warn(name(), "Realsense Error: %s", rs_get_error_message(rs_error_));
217  rs_free_error(rs_error_);
218  rs_error_ = nullptr;
219  }
220 }
221 
222 /*
223  * Testing function to log the depth pixel distancens
224  */
225 void
226 RealsenseThread::log_depths(const uint16_t *image)
227 {
228  std::string out;
229  for (uint16_t i = 0; i < rs_get_stream_height(rs_device_, rs_stream_type_, NULL); i++) {
230  for (uint16_t i = 0; i < rs_get_stream_width(rs_device_, rs_stream_type_, NULL); i++) {
231  float depth_in_meters = camera_scale_ * image[i];
232  out += std::to_string(depth_in_meters) + " ";
233  }
234  out += "\n";
235  }
236  logger->log_info(name(), "%s\n\n\n\n\n", out.c_str());
237 }
238 
239 /*
240  * Stop the device and delete the context properly
241  */
242 void
243 RealsenseThread::stop_camera()
244 {
245  if (camera_started_) {
246  logger->log_info(name(), "Stopping realsense camera ...");
247  rs_stop_device(rs_device_, &rs_error_);
248  rs_delete_context(rs_context_, &rs_error_);
249  log_error();
250  logger->log_info(name(), "Realsense camera stopped!");
251  camera_started_ = false;
252  }
253 }
254 
255 /**
256  * Read the switch interface and start/stop the camera if necessary.
257  * @return true iff the interface is currently enabled.
258  */
259 bool
261 {
262  bool enable_camera = false;
263  bool disable_camera = false;
264  while (!switch_if_->msgq_empty()) {
265  Message *msg = switch_if_->msgq_first();
266  if (dynamic_cast<SwitchInterface::EnableSwitchMessage *>(msg)) {
267  disable_camera = false;
268  enable_camera = true;
269  } else if (dynamic_cast<SwitchInterface::DisableSwitchMessage *>(msg)) {
270  disable_camera = true;
271  enable_camera = false;
272  }
273  switch_if_->msgq_pop();
274  }
275  if (camera_started_ && disable_camera) {
276  stop_camera();
277  switch_if_->set_enabled(false);
278  } else if (!camera_started_ && enable_camera) {
279  connect_and_start_camera();
280  switch_if_->set_enabled(true);
281  }
282  switch_if_->write();
283  return switch_if_->is_enabled();
284 }
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:41
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1026
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void remove_pointcloud(const char *id)
Remove the point cloud.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual void init()
Initialize the thread.
A class for handling time.
Definition: time.h:92
Thread class encapsulation of pthreads.
Definition: thread.h:45
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1179
SwitchInterface Fawkes BlackBoard Interface.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:47
Base class for exceptions in Fawkes.
Definition: exception.h:35
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1164
void set_enabled(const bool new_enabled)
Set enabled value.
const char * name() const
Get name of thread.
Definition: thread.h:100
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
bool is_enabled() const
Get enabled value.
bool read_switch()
Read the switch interface and start/stop the camera if necessary.
virtual void finalize()
Finalize the thread.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT >> cloud)
Add point cloud.
virtual void loop()
Code to execute in the thread.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.