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
28using 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
36Realsense2Thread::Realsense2Thread()
37: Thread("Realsense2Thread", Thread::OPMODE_WAITFORWAKEUP),
38 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE),
39 switch_if_(NULL)
40{
41}
42
43void
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
83void
85{
86 if (!camera_running_) {
87 camera_running_ = start_camera();
88 return;
89 }
90
91 if (cfg_use_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
135void
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 */
149bool
150Realsense2Thread::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();
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) {
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 */
197bool
198Realsense2Thread::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) {
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 */
246void
247Realsense2Thread::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) {
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 */
285void
286Realsense2Thread::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) {
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 */
319void
320Realsense2Thread::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 */
334bool
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}
virtual void init()
Initialize the thread.
bool read_switch()
Read the switch interface and start/stop the camera if necessary.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize 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 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
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
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.