25 #include "realsense_thread.h"
27 #include <interfaces/SwitchInterface.h>
37 RealsenseThread::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();
154 RealsenseThread::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_;
194 RealsenseThread::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;
217 RealsenseThread::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_));
237 RealsenseThread::log_error()
241 rs_free_error(rs_error_);
250 RealsenseThread::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) +
" ";
267 RealsenseThread::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;
288 if (dynamic_cast<SwitchInterface::EnableSwitchMessage *>(msg)) {
289 enable_camera_ =
true;
290 }
else if (dynamic_cast<SwitchInterface::DisableSwitchMessage *>(msg)) {
291 enable_camera_ =
false;