24 #include "realsense2_thread.h"
26 #include <interfaces/SwitchInterface.h>
36 Realsense2Thread::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();
150 Realsense2Thread::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) {
198 Realsense2Thread::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) {
247 Realsense2Thread::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) {
286 Realsense2Thread::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) {
320 Realsense2Thread::stop_camera()
322 camera_running_ =
false;
323 depth_enabled_ =
false;
326 }
catch (
const std::exception &e) {
339 if (dynamic_cast<SwitchInterface::EnableSwitchMessage *>(msg)) {
340 enable_camera_ =
true;
341 }
else if (dynamic_cast<SwitchInterface::DisableSwitchMessage *>(msg)) {
342 enable_camera_ =
false;