24#include "realsense2_thread.h"
26#include <interfaces/SwitchInterface.h>
36Realsense2Thread::Realsense2Thread()
37:
Thread(
"Realsense2Thread",
Thread::OPMODE_WAITFORWAKEUP),
47 const std::string cfg_prefix =
"/realsense2/";
52 restart_after_num_errors_ =
59 if (cfg_use_switch_) {
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);
79 rs_pipe_ =
new rs2::pipeline();
80 rs_context_ =
new rs2::context();
86 if (!camera_running_) {
87 camera_running_ = start_camera();
91 if (cfg_use_switch_) {
95 if (enable_camera_ && !depth_enabled_) {
96 enable_depth_stream();
98 }
else if (!enable_camera_ && depth_enabled_) {
99 disable_depth_stream();
101 }
else if (!depth_enabled_) {
104 if (rs_pipe_->poll_for_frames(&rs_data_)) {
105 rs2::frame depth_frame = rs_data_.first(RS2_STREAM_DEPTH);
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];
126 if (error_counter_ >= restart_after_num_errors_) {
141 realsense_depth_refptr_.reset();
150Realsense2Thread::start_camera()
154 }
catch (
const std::exception &e) {
158 if (!get_camera(rs_device_)) {
162 config.enable_stream(RS2_STREAM_DEPTH, RS2_FORMAT_Z16, frame_rate_);
163 rs2::pipeline_profile rs_pipeline_profile_ = rs_pipe_->start(
config);
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",
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(),
187 }
catch (
const std::exception &e) {
198Realsense2Thread::get_camera(rs2::device &dev)
202 rs2::device_list devlist = rs_context_->query_devices();
203 if (devlist.size() == 0) {
208 if (devlist.front().is<rs400::advanced_mode>()) {
209 dev = devlist.front().as<rs400::advanced_mode>();
211 dev = devlist.front();
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);
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);
227 logger->
log_info(
name(),
"Camera Name: %s, SN: %s", dev_name.c_str(), dev_sn.c_str());
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(),
237 }
catch (
const std::exception &e) {
247Realsense2Thread::enable_depth_stream()
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,
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;
263 depth_sensor.set_option(RS2_OPTION_LASER_POWER, laser_power_);
264 depth_enabled_ =
true;
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(),
276 }
catch (
const std::exception &e) {
286Realsense2Thread::disable_depth_stream()
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,
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);
299 depth_enabled_ =
false;
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(),
310 }
catch (
const std::exception &e) {
320Realsense2Thread::stop_camera()
322 camera_running_ =
false;
323 depth_enabled_ =
false;
326 }
catch (
const std::exception &e) {
340 enable_camera_ =
true;
342 enable_camera_ =
false;
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.
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.
Configuration * config
This is the Configuration member used to access the configuration.
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.
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...
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...
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.
void msgq_pop()
Erase first message from queue.
Message * msgq_first()
Get the first message from the message queue.
void write()
Write from local copy into BlackBoard memory.
bool msgq_empty()
Check if queue is empty.
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.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
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.
const char * name() const
Get name of thread.
A class for handling time.
Fawkes library namespace.