25#include "realsense_thread.h"
27#include <interfaces/SwitchInterface.h>
37RealsenseThread::RealsenseThread()
38:
Thread(
"RealsenseThread",
Thread::OPMODE_WAITFORWAKEUP),
48 const std::string cfg_prefix =
"/realsense/";
51 laser_power_ =
config->
get_int(cfg_prefix +
"device_options/laser_power");
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_stream_type_ = RS_STREAM_DEPTH;
80 connect_and_start_camera();
87 if (cfg_use_switch_) {
90 if (enable_camera_ && !camera_running_) {
91 connect_and_start_camera();
94 }
else if (!enable_camera_) {
95 if (camera_running_) {
105 if (rs_poll_for_frames(rs_device_, &rs_error_) == 1) {
107 const uint16_t *image =
108 reinterpret_cast<const uint16_t *
>(rs_get_frame_data(rs_device_, rs_stream_type_, NULL));
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];
129 "Poll for frames not successful (%s)",
130 rs_get_error_message(rs_error_));
131 if (error_counter_ >= restart_after_num_errors_) {
135 connect_and_start_camera();
143 realsense_depth_refptr_.reset();
154RealsenseThread::connect_and_start_camera()
156 rs_context_ = rs_create_context(RS_API_VERSION, &rs_error_);
158 num_of_cameras_ = rs_get_device_count(rs_context_, &rs_error_);
160 if (num_of_cameras_ < 1) {
162 rs_delete_context(rs_context_, &rs_error_);
163 camera_running_ =
false;
164 return camera_running_;
167 rs_device_ = get_camera();
168 rs_set_device_option(rs_device_, RS_OPTION_F200_LASER_POWER, laser_power_, &rs_error_);
170 enable_depth_stream();
172 rs_start_device(rs_device_, &rs_error_);
178 rs_get_stream_format(rs_device_, rs_stream_type_, &rs_error_)));
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_;
194RealsenseThread::get_camera()
197 rs_device *rs_detected_device = rs_get_device(rs_context_, 0, &rs_error_);
200 "\n\nDetected Device:\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_));
210 return rs_detected_device;
217RealsenseThread::enable_depth_stream()
219 rs_enable_stream_preset(rs_device_, rs_stream_type_, RS_PRESET_BEST_QUALITY, &rs_error_);
221 if (rs_is_stream_enabled(rs_device_, rs_stream_type_, &rs_error_)) {
223 "Depth stream enabled! Streaming with %i fps",
224 rs_get_stream_framerate(rs_device_, rs_stream_type_, &rs_error_));
228 throw Exception(
"Couldn't start depth stream! Stream type: %s",
229 rs_stream_to_string(rs_stream_type_));
237RealsenseThread::log_error()
241 rs_free_error(rs_error_);
250RealsenseThread::log_depths(
const uint16_t *image)
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) +
" ";
267RealsenseThread::stop_camera()
269 if (camera_running_) {
271 rs_stop_device(rs_device_, &rs_error_);
272 rs_delete_context(rs_context_, &rs_error_);
275 camera_running_ =
false;
289 enable_camera_ =
true;
291 enable_camera_ =
false;
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.
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 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.
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.
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.