25 #include "realsense_thread.h" 27 #include <interfaces/SwitchInterface.h> 37 RealsenseThread::RealsenseThread()
38 :
Thread(
"RealsenseThread",
Thread::OPMODE_WAITFORWAKEUP),
49 const std::string cfg_prefix =
"/realsense/";
52 laser_power_ =
config->
get_int(cfg_prefix +
"device_options/laser_power");
55 cfg_use_switch_ =
config->
get_bool((cfg_prefix +
"use_switch").c_str());
59 if (cfg_use_switch_) {
69 rs_stream_type_ = RS_STREAM_DEPTH;
70 connect_and_start_camera();
72 camera_scale_ = rs_get_device_depth_scale(rs_device_, NULL);
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);
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;
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));
90 realsense_depth_->resize(z_intrinsic_.width * z_intrinsic_.height);
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));
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];
120 "Poll for frames not successful (%s)",
121 rs_get_error_message(rs_error_));
128 realsense_depth_refptr_.reset();
139 RealsenseThread::connect_and_start_camera()
141 rs_context_ = rs_create_context(RS_API_VERSION, &rs_error_);
143 num_of_cameras_ = rs_get_device_count(rs_context_, &rs_error_);
145 if (num_of_cameras_ < 1) {
149 rs_device_ = get_camera();
150 rs_set_device_option(rs_device_, RS_OPTION_F200_LASER_POWER, laser_power_, &rs_error_);
152 enable_depth_stream();
154 rs_start_device(rs_device_, &rs_error_);
160 rs_get_stream_format(rs_device_, rs_stream_type_, &rs_error_)));
162 camera_started_ =
true;
170 RealsenseThread::get_camera()
173 rs_device *rs_detected_device = rs_get_device(rs_context_, 0, &rs_error_);
176 "\n\nDetected Device:\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_));
186 return rs_detected_device;
193 RealsenseThread::enable_depth_stream()
195 rs_enable_stream_preset(rs_device_, rs_stream_type_, RS_PRESET_BEST_QUALITY, &rs_error_);
197 if (rs_is_stream_enabled(rs_device_, rs_stream_type_, &rs_error_)) {
199 "Depth stream enabled! Streaming with %i fps",
200 rs_get_stream_framerate(rs_device_, rs_stream_type_, &rs_error_));
204 throw Exception(
"Couldn't start depth stream! Stream type: %s",
205 rs_stream_to_string(rs_stream_type_));
213 RealsenseThread::log_error()
217 rs_free_error(rs_error_);
226 RealsenseThread::log_depths(
const uint16_t *image)
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) +
" ";
243 RealsenseThread::stop_camera()
245 if (camera_started_) {
247 rs_stop_device(rs_device_, &rs_error_);
248 rs_delete_context(rs_context_, &rs_error_);
251 camera_started_ =
false;
262 bool enable_camera =
false;
263 bool disable_camera =
false;
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;
275 if (camera_started_ && disable_camera) {
278 }
else if (!camera_started_ && enable_camera) {
279 connect_and_start_camera();
Base class for all messages passed through interfaces in Fawkes BlackBoard.
bool msgq_empty()
Check if queue is empty.
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.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
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.
Clock * clock
By means of this member access to the clock is given.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
SwitchInterface Fawkes BlackBoard Interface.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Base class for exceptions in Fawkes.
Message * msgq_first()
Get the first message from the message queue.
void set_enabled(const bool new_enabled)
Set enabled value.
const char * name() const
Get name of thread.
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.
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.
virtual void close(Interface *interface)=0
Close interface.