Fawkes API  Fawkes Development Version
realsense2_thread.cpp
1 
2 /***************************************************************************
3  * realsense2_plugin.cpp - realsense2
4  *
5  * Plugin created: Wed May 22 10:09:22 2019
6  *
7  * Copyright 2019 Christoph Gollok
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 #include "realsense2_thread.h"
25 
26 #include <interfaces/SwitchInterface.h>
27 
28 using namespace fawkes;
29 
30 /** @class Realsense2Thread 'realsense2_thread.h'
31  * Driver for the Intel RealSense Camera providing Depth Data as Pointcloud
32  * Inspired by realsense fawkes plugin
33  * @author Christoph Gollok
34  */
35 
36 Realsense2Thread::Realsense2Thread()
37 : Thread("Realsense2Thread", Thread::OPMODE_WAITFORWAKEUP),
38  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
39  switch_if_(NULL)
40 {
41 }
42 
43 void
45 {
46  // set config values
47  const std::string cfg_prefix = "/realsense2/";
48  frame_id_ = config->get_string_or_default((cfg_prefix + "frame_id").c_str(), "cam_conveyor");
49  pcl_id_ = config->get_string_or_default((cfg_prefix + "pcl_id").c_str(), "/camera/depth/points");
50  switch_if_name_ =
51  config->get_string_or_default((cfg_prefix + "switch_if_name").c_str(), "realsense2");
52  restart_after_num_errors_ =
53  config->get_uint_or_default((cfg_prefix + "restart_after_num_errors").c_str(), 50);
54  frame_rate_ = config->get_uint_or_default((cfg_prefix + "frame_rate").c_str(), 30);
55  laser_power_ = config->get_float_or_default((cfg_prefix + "laser_power").c_str(), -1);
56 
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>(switch_if_name_.c_str());
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_pipe_ = new rs2::pipeline();
80  rs_context_ = new rs2::context();
81 }
82 
83 void
85 {
86  if (!camera_running_) {
87  camera_running_ = start_camera();
88  return;
89  }
90 
91  if (cfg_use_switch_) {
92  read_switch();
93  }
94 
95  if (enable_camera_ && !depth_enabled_) {
96  enable_depth_stream();
97  return;
98  } else if (!enable_camera_ && depth_enabled_) {
99  disable_depth_stream();
100  return;
101  } else if (!depth_enabled_) {
102  return;
103  }
104  if (rs_pipe_->poll_for_frames(&rs_data_)) {
105  rs2::frame depth_frame = rs_data_.first(RS2_STREAM_DEPTH);
106  error_counter_ = 0;
107  const uint16_t *image = reinterpret_cast<const uint16_t *>(depth_frame.get_data());
108  Cloud::iterator it = realsense_depth_->begin();
109  for (int y = 0; y < intrinsics_.height; y++) {
110  for (int x = 0; x < intrinsics_.width; x++) {
111  float scaled_depth = camera_scale_ * (static_cast<float>(*image));
112  float depth_point[3];
113  float depth_pixel[2] = {static_cast<float>(x), static_cast<float>(y)};
114  rs2_deproject_pixel_to_point(depth_point, &intrinsics_, depth_pixel, scaled_depth);
115  it->x = depth_point[0];
116  it->y = depth_point[1];
117  it->z = depth_point[2];
118  ++image;
119  ++it;
120  }
121  }
122  pcl_utils::set_time(realsense_depth_refptr_, fawkes::Time(clock));
123  } else {
124  error_counter_++;
125  logger->log_warn(name(), "Poll for frames not successful ()");
126  if (error_counter_ >= restart_after_num_errors_) {
127  logger->log_warn(name(), "Polling failed, restarting device");
128  error_counter_ = 0;
129  stop_camera();
130  start_camera();
131  }
132  }
133 }
134 
135 void
137 {
138  stop_camera();
139  delete rs_pipe_;
140  delete rs_context_;
141  realsense_depth_refptr_.reset();
142  pcl_manager->remove_pointcloud(pcl_id_.c_str());
143  blackboard->close(switch_if_);
144 }
145 
146 /* Create RS context and start the depth stream
147  * @return true when succesfull
148  */
149 bool
150 Realsense2Thread::start_camera()
151 {
152  try {
153  rs_pipe_->stop();
154  } catch (const std::exception &e) {
155  }
156 
157  try {
158  if (!get_camera(rs_device_)) {
159  return false;
160  }
161  rs2::config config;
162  config.enable_stream(RS2_STREAM_DEPTH, RS2_FORMAT_Z16, frame_rate_);
163  rs2::pipeline_profile rs_pipeline_profile_ = rs_pipe_->start(config);
164  auto depth_stream =
165  rs_pipeline_profile_.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
166  intrinsics_ = depth_stream.get_intrinsics();
167  realsense_depth_->width = intrinsics_.width;
168  realsense_depth_->height = intrinsics_.height;
169  realsense_depth_->resize(intrinsics_.width * intrinsics_.height);
170  rs2::depth_sensor sensor = rs_device_.first<rs2::depth_sensor>();
171  camera_scale_ = sensor.get_depth_scale();
172  logger->log_info(name(),
173  "Height: %d Width: %d Scale: %f FPS: %d",
174  intrinsics_.height,
175  intrinsics_.width,
176  camera_scale_,
177  frame_rate_);
178 
179  return true;
180 
181  } catch (const rs2::error &e) {
182  logger->log_error(name(),
183  "RealSense error calling %s ( %s ):\n %s",
184  e.get_failed_function().c_str(),
185  e.get_failed_args().c_str(),
186  e.what());
187  } catch (const std::exception &e) {
188  logger->log_error(name(), "%s", e.what());
189  }
190 
191  return false;
192 }
193 
194 /*
195  * Get the rs_device pointer and printout camera details
196  */
197 bool
198 Realsense2Thread::get_camera(rs2::device &dev)
199 {
200  dev = nullptr;
201  try {
202  rs2::device_list devlist = rs_context_->query_devices();
203  if (devlist.size() == 0) {
204  logger->log_warn(name(), "No device connected, please connect a RealSense device");
205  return false;
206  } else {
207  logger->log_info(name(), "found devices: %d", devlist.size());
208  if (devlist.front().is<rs400::advanced_mode>()) {
209  dev = devlist.front().as<rs400::advanced_mode>();
210  } else {
211  dev = devlist.front();
212  }
213 
214  std::string dev_name = "Unknown Device";
215  if (dev.supports(RS2_CAMERA_INFO_NAME)) {
216  dev_name = dev.get_info(RS2_CAMERA_INFO_NAME);
217  } else {
218  logger->log_info(name(), "RS2Option RS2_CAMERA_INFO_NAME not supported %d", 1);
219  }
220 
221  std::string dev_sn = "########";
222  if (dev.supports(RS2_CAMERA_INFO_SERIAL_NUMBER)) {
223  dev_sn = std::string("#") + rs_device_.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
224  } else {
225  logger->log_info(name(), "RS2Option RS2_CAMERA_INFO_SERIAL_NUMBER not supported");
226  }
227  logger->log_info(name(), "Camera Name: %s, SN: %s", dev_name.c_str(), dev_sn.c_str());
228  return true;
229  }
230  } catch (const rs2::error &e) {
231  logger->log_error(name(),
232  "RealSense error calling %s ( %s ):\n %s",
233  e.get_failed_function().c_str(),
234  e.get_failed_args().c_str(),
235  e.what());
236  return false;
237  } catch (const std::exception &e) {
238  logger->log_error(name(), "%s", e.what());
239  return false;
240  }
241 }
242 
243 /*
244  * Enable the depth stream from rs_device
245  */
246 void
247 Realsense2Thread::enable_depth_stream()
248 {
249  logger->log_info(name(), "Enable depth Stream");
250 
251  try {
252  rs2::depth_sensor depth_sensor = rs_device_.first<rs2::depth_sensor>();
253  if (depth_sensor.supports(RS2_OPTION_EMITTER_ENABLED)) {
254  depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED,
255  1.f); // Enable emitter
256  depth_enabled_ = true;
257  } else if (depth_sensor.supports(RS2_OPTION_LASER_POWER)) {
258  if (laser_power_ == -1) {
259  rs2::option_range range = depth_sensor.get_option_range(RS2_OPTION_LASER_POWER);
260  laser_power_ = range.max;
261  }
262  logger->log_info(name(), "Enable depth stream with Laser Power: %f", laser_power_);
263  depth_sensor.set_option(RS2_OPTION_LASER_POWER, laser_power_); // Set max power
264  depth_enabled_ = true;
265  } else {
266  logger->log_warn(name(), "Enable depth stream not supported on device");
267  }
268 
269  } catch (const rs2::error &e) {
270  logger->log_error(name(),
271  "RealSense error calling %s ( %s ):\n %s",
272  e.get_failed_function().c_str(),
273  e.get_failed_args().c_str(),
274  e.what());
275  return;
276  } catch (const std::exception &e) {
277  logger->log_error(name(), "%s", e.what());
278  return;
279  }
280 }
281 
282 /*
283  * Disable the depth stream from rs_device
284  */
285 void
286 Realsense2Thread::disable_depth_stream()
287 {
288  logger->log_info(name(), "Disable Depth Stream");
289 
290  try {
291  rs2::depth_sensor depth_sensor = rs_device_.first<rs2::depth_sensor>();
292  if (depth_sensor.supports(RS2_OPTION_EMITTER_ENABLED)) {
293  depth_sensor.set_option(RS2_OPTION_EMITTER_ENABLED,
294  0.f); // Disable emitter
295  depth_enabled_ = false;
296  } else if (depth_sensor.supports(RS2_OPTION_LASER_POWER)) {
297  rs2::option_range range = depth_sensor.get_option_range(RS2_OPTION_LASER_POWER);
298  depth_sensor.set_option(RS2_OPTION_LASER_POWER, range.min); // Set max power
299  depth_enabled_ = false;
300  } else {
301  logger->log_warn(name(), "Disable depth stream not supported on device");
302  }
303  } catch (const rs2::error &e) {
304  logger->log_error(name(),
305  "RealSense error calling %s ( %s ):\n %s",
306  e.get_failed_function().c_str(),
307  e.get_failed_args().c_str(),
308  e.what());
309  return;
310  } catch (const std::exception &e) {
311  logger->log_error(name(), "%s", e.what());
312  return;
313  }
314 }
315 
316 /*
317  * Stop the device and delete the context properly
318  */
319 void
320 Realsense2Thread::stop_camera()
321 {
322  camera_running_ = false;
323  depth_enabled_ = false;
324  try {
325  rs_pipe_->stop();
326  } catch (const std::exception &e) {
327  }
328 }
329 
330 /**
331  * Read the switch interface and start/stop the camera if necessary.
332  * @return true iff the interface is currently enabled.
333  */
334 bool
336 {
337  while (!switch_if_->msgq_empty()) {
338  Message *msg = switch_if_->msgq_first();
339  if (dynamic_cast<SwitchInterface::EnableSwitchMessage *>(msg)) {
340  enable_camera_ = true;
341  } else if (dynamic_cast<SwitchInterface::DisableSwitchMessage *>(msg)) {
342  enable_camera_ = false;
343  }
344  switch_if_->msgq_pop();
345  }
346  switch_if_->set_enabled(enable_camera_);
347  switch_if_->write();
348  return switch_if_->is_enabled();
349 }
Realsense2Thread::init
virtual void init()
Initialize the thread.
Definition: realsense2_thread.cpp:44
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
fawkes::SwitchInterface
SwitchInterface Fawkes BlackBoard Interface.
Definition: SwitchInterface.h:34
fawkes::Configuration::get_float_or_default
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
fawkes::Message
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:45
fawkes::PointCloudManager::remove_pointcloud
void remove_pointcloud(const char *id)
Remove the point cloud.
Definition: pointcloud_manager.cpp:66
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
fawkes::SwitchInterface::DisableSwitchMessage
DisableSwitchMessage Fawkes BlackBoard Interface Message.
Definition: SwitchInterface.h:136
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
fawkes::ClockAspect::clock
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
fawkes::Configuration::get_bool_or_default
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
fawkes::SwitchInterface::EnableSwitchMessage
EnableSwitchMessage Fawkes BlackBoard Interface Message.
Definition: SwitchInterface.h:116
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
fawkes::SwitchInterface::set_enabled
void set_enabled(const bool new_enabled)
Set enabled value.
Definition: SwitchInterface.cpp:105
Realsense2Thread::loop
virtual void loop()
Code to execute in the thread.
Definition: realsense2_thread.cpp:84
fawkes
Fawkes library namespace.
pcl::PointCloud< PointType >
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
fawkes::Time
A class for handling time.
Definition: time.h:93
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::Configuration::get_uint_or_default
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
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
fawkes::PointCloudManager::add_pointcloud
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT >> cloud)
Add point cloud.
Definition: pointcloud_manager.h:78
fawkes::Configuration::get_string_or_default
virtual std::string get_string_or_default(const char *path, const std::string &default_val)
Get value from configuration which is of type string, or the given default if the path does not exist...
Definition: config.cpp:736
fawkes::SwitchInterface::is_enabled
bool is_enabled() const
Get enabled value.
Definition: SwitchInterface.cpp:83
Realsense2Thread::finalize
virtual void finalize()
Finalize the thread.
Definition: realsense2_thread.cpp:136
fawkes::PointCloudAspect::pcl_manager
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:47
fawkes::Interface::msgq_first
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1167
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
Realsense2Thread::read_switch
bool read_switch()
Read the switch interface and start/stop the camera if necessary.
Definition: realsense2_thread.cpp:335