base_realsense_node.cpp
Go to the documentation of this file.
1 #include "../include/base_realsense_node.h"
2 #include "assert.h"
3 #include <boost/algorithm/string.hpp>
4 #include <algorithm>
5 #include <cctype>
6 #include <mutex>
7 
8 #include <dynamic_reconfigure/IntParameter.h>
9 #include <dynamic_reconfigure/Reconfigure.h>
10 #include <dynamic_reconfigure/Config.h>
11 
12 using namespace realsense2_camera;
13 using namespace ddynamic_reconfigure;
14 
15 // stream_index_pair sip{stream_type, stream_index};
16 #define STREAM_NAME(sip) (static_cast<std::ostringstream&&>(std::ostringstream() << _stream_name[sip.first] << ((sip.second>0) ? std::to_string(sip.second) : ""))).str()
17 #define FRAME_ID(sip) (static_cast<std::ostringstream&&>(std::ostringstream() << "camera_" << STREAM_NAME(sip) << "_frame")).str()
18 #define OPTICAL_FRAME_ID(sip) (static_cast<std::ostringstream&&>(std::ostringstream() << "camera_" << STREAM_NAME(sip) << "_optical_frame")).str()
19 #define ALIGNED_DEPTH_TO_FRAME_ID(sip) (static_cast<std::ostringstream&&>(std::ostringstream() << "camera_aligned_depth_to_" << STREAM_NAME(sip) << "_frame")).str()
20 
21 SyncedImuPublisher::SyncedImuPublisher(ros::Publisher imu_publisher, std::size_t waiting_list_size):
22  _publisher(imu_publisher), _pause_mode(false),
23  _waiting_list_size(waiting_list_size)
24  {}
25 
26 SyncedImuPublisher::~SyncedImuPublisher()
27 {
28  PublishPendingMessages();
29 }
30 
31 void SyncedImuPublisher::Publish(sensor_msgs::Imu imu_msg)
32 {
33  std::lock_guard<std::mutex> lock_guard(_mutex);
34  if (_pause_mode)
35  {
36  if (_pending_messages.size() >= _waiting_list_size)
37  {
38  throw std::runtime_error("SyncedImuPublisher inner list reached maximum size of " + std::to_string(_pending_messages.size()));
39  }
40  _pending_messages.push(imu_msg);
41  }
42  else
43  {
44  _publisher.publish(imu_msg);
45  // ROS_INFO_STREAM("iid1:" << imu_msg.header.seq << ", time: " << std::setprecision (20) << imu_msg.header.stamp.toSec());
46  }
47  return;
48 }
49 
50 void SyncedImuPublisher::Pause()
51 {
52  if (!_is_enabled) return;
53  std::lock_guard<std::mutex> lock_guard(_mutex);
54  _pause_mode = true;
55 }
56 
57 void SyncedImuPublisher::Resume()
58 {
59  std::lock_guard<std::mutex> lock_guard(_mutex);
60  PublishPendingMessages();
61  _pause_mode = false;
62 }
63 
64 void SyncedImuPublisher::PublishPendingMessages()
65 {
66  // ROS_INFO_STREAM("publish imu: " << _pending_messages.size());
67  while (!_pending_messages.empty())
68  {
69  const sensor_msgs::Imu &imu_msg = _pending_messages.front();
70  _publisher.publish(imu_msg);
71  // ROS_INFO_STREAM("iid2:" << imu_msg.header.seq << ", time: " << std::setprecision (20) << imu_msg.header.stamp.toSec());
72  _pending_messages.pop();
73  }
74 }
75 
76 std::string BaseRealSenseNode::getNamespaceStr()
77 {
78  auto ns = ros::this_node::getNamespace();
79  ns.erase(std::remove(ns.begin(), ns.end(), '/'), ns.end());
80  return ns;
81 }
82 
83 BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle,
84  ros::NodeHandle& privateNodeHandle,
85  rs2::device dev,
86  const std::string& serial_no) :
87  _is_running(true), _base_frame_id(""), _node_handle(nodeHandle),
88  _pnh(privateNodeHandle), _dev(dev), _json_file_path(""),
89  _serial_no(serial_no),
90  _is_initialized_time_base(false),
91  _namespace(getNamespaceStr())
92 {
93  // Types for depth stream
94  _format[RS2_STREAM_DEPTH] = RS2_FORMAT_Z16;
95  _image_format[RS2_STREAM_DEPTH] = CV_16UC1; // CVBridge type
96  _encoding[RS2_STREAM_DEPTH] = sensor_msgs::image_encodings::TYPE_16UC1; // ROS message type
97  _unit_step_size[RS2_STREAM_DEPTH] = sizeof(uint16_t); // sensor_msgs::ImagePtr row step size
98  _stream_name[RS2_STREAM_DEPTH] = "depth";
99 
100  // Types for confidence stream
101  _image_format[RS2_STREAM_CONFIDENCE] = CV_8UC1; // CVBridge type
102  _encoding[RS2_STREAM_CONFIDENCE] = sensor_msgs::image_encodings::MONO8; // ROS message type
103  _unit_step_size[RS2_STREAM_CONFIDENCE] = sizeof(uint8_t); // sensor_msgs::ImagePtr row step size
104  _stream_name[RS2_STREAM_CONFIDENCE] = "confidence";
105 
106  // Infrared stream
107  _format[RS2_STREAM_INFRARED] = RS2_FORMAT_Y8;
108 
109  _image_format[RS2_STREAM_INFRARED] = CV_8UC1; // CVBridge type
110  _encoding[RS2_STREAM_INFRARED] = sensor_msgs::image_encodings::MONO8; // ROS message type
111  _unit_step_size[RS2_STREAM_INFRARED] = sizeof(uint8_t); // sensor_msgs::ImagePtr row step size
112  _stream_name[RS2_STREAM_INFRARED] = "infra";
113 
114  // Types for color stream
115  _image_format[RS2_STREAM_COLOR] = CV_8UC3; // CVBridge type
116  _encoding[RS2_STREAM_COLOR] = sensor_msgs::image_encodings::RGB8; // ROS message type
117  _unit_step_size[RS2_STREAM_COLOR] = 3; // sensor_msgs::ImagePtr row step size
118  _stream_name[RS2_STREAM_COLOR] = "color";
119  _depth_aligned_encoding[RS2_STREAM_COLOR] = sensor_msgs::image_encodings::TYPE_16UC1;
120 
121  // Types for fisheye stream
122  _image_format[RS2_STREAM_FISHEYE] = CV_8UC1; // CVBridge type
123  _encoding[RS2_STREAM_FISHEYE] = sensor_msgs::image_encodings::MONO8; // ROS message type
124  _unit_step_size[RS2_STREAM_FISHEYE] = sizeof(uint8_t); // sensor_msgs::ImagePtr row step size
125  _stream_name[RS2_STREAM_FISHEYE] = "fisheye";
126 
127  // Types for Motion-Module streams
128  _stream_name[RS2_STREAM_GYRO] = "gyro";
129 
130  _stream_name[RS2_STREAM_ACCEL] = "accel";
131 
132  _stream_name[RS2_STREAM_POSE] = "pose";
133 
134  _monitor_options = {RS2_OPTION_ASIC_TEMPERATURE, RS2_OPTION_PROJECTOR_TEMPERATURE};
135 }
136 
137 BaseRealSenseNode::~BaseRealSenseNode()
138 {
139  // Kill dynamic transform thread
140  _is_running = false;
141  _cv_tf.notify_one();
142  if (_tf_t && _tf_t->joinable())
143  _tf_t->join();
144  if (_update_functions_t && _update_functions_t->joinable())
145  _update_functions_t->join();
146 
147  _cv_monitoring.notify_one();
148  if (_monitoring_t && _monitoring_t->joinable())
149  {
150  _monitoring_t->join();
151  }
152 
153  std::set<std::string> module_names;
154  for (const std::pair<stream_index_pair, std::vector<rs2::stream_profile>>& profile : _enabled_profiles)
155  {
156  try
157  {
158  std::string module_name = _sensors[profile.first].get_info(RS2_CAMERA_INFO_NAME);
159  std::pair< std::set<std::string>::iterator, bool> res = module_names.insert(module_name);
160  if (res.second)
161  {
162  _sensors[profile.first].stop();
163  _sensors[profile.first].close();
164  }
165  }
166  catch (const rs2::error& e)
167  {
168  ROS_ERROR_STREAM("Exception: " << e.what());
169  }
170  }
171 }
172 
173 void BaseRealSenseNode::toggleSensors(bool enabled)
174 {
175  if(enabled)
176  {
177  std::map<std::string, std::vector<rs2::stream_profile> > profiles;
178  std::map<std::string, rs2::sensor> active_sensors;
179  for (const std::pair<stream_index_pair, std::vector<rs2::stream_profile>>& profile : _enabled_profiles)
180  {
181  std::string module_name = _sensors[profile.first].get_info(RS2_CAMERA_INFO_NAME);
182  ROS_INFO_STREAM("insert " << rs2_stream_to_string(profile.second.begin()->stream_type())
183  << " to " << module_name);
184  profiles[module_name].insert(profiles[module_name].begin(),
185  profile.second.begin(),
186  profile.second.end());
187  active_sensors[module_name] = _sensors[profile.first];
188  }
189 
190  for (const std::pair<std::string, std::vector<rs2::stream_profile> >& sensor_profile : profiles)
191  {
192  std::string module_name = sensor_profile.first;
193  rs2::sensor sensor = active_sensors[module_name];
194  sensor.open(sensor_profile.second);
195  sensor.start(_sensors_callback[module_name]);
196  if (sensor.is<rs2::depth_sensor>())
197  {
198  _depth_scale_meters = sensor.as<rs2::depth_sensor>().get_depth_scale();
199  }
200  }
201  }
202  else
203  {
204  std::set<std::string> module_names;
205  for (const std::pair<stream_index_pair, std::vector<rs2::stream_profile>>& profile : _enabled_profiles)
206  {
207  std::string module_name = _sensors[profile.first].get_info(RS2_CAMERA_INFO_NAME);
208  std::pair< std::set<std::string>::iterator, bool> res = module_names.insert(module_name);
209  if (res.second)
210  {
211  _sensors[profile.first].stop();
212  _sensors[profile.first].close();
213  }
214  }
215  }
216 }
217 
218 void BaseRealSenseNode::setupErrorCallback()
219 {
220  for (auto&& s : _dev.query_sensors())
221  {
222  s.set_notifications_callback([&](const rs2::notification& n)
223  {
224  std::vector<std::string> error_strings({"RT IC2 Config error",
225  "Left IC2 Config error"});
226  if (n.get_severity() >= RS2_LOG_SEVERITY_ERROR)
227  {
228  ROS_WARN_STREAM("Hardware Notification:" << n.get_description() << "," << n.get_timestamp() << "," << n.get_severity() << "," << n.get_category());
229  }
230  if (error_strings.end() != find_if(error_strings.begin(), error_strings.end(), [&n] (std::string err)
231  {return (n.get_description().find(err) != std::string::npos); }))
232  {
233  ROS_ERROR_STREAM("Performing Hardware Reset.");
234  _dev.hardware_reset();
235  }
236  });
237  }
238 }
239 
240 void BaseRealSenseNode::publishTopics()
241 {
242  getParameters();
243  setupDevice();
244  setupFilters();
245  registerHDRoptions();
246  registerDynamicReconfigCb(_node_handle);
247  setupErrorCallback();
248  enable_devices();
249  setupPublishers();
250  setupStreams();
251  SetBaseStream();
252  registerAutoExposureROIOptions(_node_handle);
253  publishStaticTransforms();
254  publishIntrinsics();
255  startMonitoring();
256  ROS_INFO_STREAM("RealSense Node Is Up!");
257 }
258 
259 void BaseRealSenseNode::runFirstFrameInitialization(rs2_stream stream_type)
260 {
261  if (_is_first_frame[stream_type])
262  {
263  ROS_DEBUG_STREAM("runFirstFrameInitialization: " << _video_functions_stack.size() << ", " << rs2_stream_to_string(stream_type));
264  _is_first_frame[stream_type] = false;
265  if (!_video_functions_stack[stream_type].empty())
266  {
267  std::thread t = std::thread([=]()
268  {
269  while (!_video_functions_stack[stream_type].empty())
270  {
271  _video_functions_stack[stream_type].back()();
272  _video_functions_stack[stream_type].pop_back();
273  }
274  });
275  t.detach();
276  }
277  }
278 }
279 
280 bool is_checkbox(rs2::options sensor, rs2_option option)
281 {
282  rs2::option_range op_range = sensor.get_option_range(option);
283  return op_range.max == 1.0f &&
284  op_range.min == 0.0f &&
285  op_range.step == 1.0f;
286 }
287 
288 bool is_enum_option(rs2::options sensor, rs2_option option)
289 {
290  static const int MAX_ENUM_OPTION_VALUES(100);
291  static const float EPSILON(0.05);
292 
293  rs2::option_range op_range = sensor.get_option_range(option);
294  if (abs((op_range.step - 1)) > EPSILON || (op_range.max > MAX_ENUM_OPTION_VALUES)) return false;
295  for (auto i = op_range.min; i <= op_range.max; i += op_range.step)
296  {
297  if (sensor.get_option_value_description(option, i) == nullptr)
298  continue;
299  return true;
300  }
301  return false;
302 }
303 
304 bool is_int_option(rs2::options sensor, rs2_option option)
305 {
306  rs2::option_range op_range = sensor.get_option_range(option);
307  return (op_range.step == 1.0);
308 }
309 
310 std::map<std::string, int> get_enum_method(rs2::options sensor, rs2_option option)
311 {
312  std::map<std::string, int> dict; // An enum to set size
313  if (is_enum_option(sensor, option))
314  {
315  rs2::option_range op_range = sensor.get_option_range(option);
316  const auto op_range_min = int(op_range.min);
317  const auto op_range_max = int(op_range.max);
318  const auto op_range_step = int(op_range.step);
319  for (auto val = op_range_min; val <= op_range_max; val += op_range_step)
320  {
321  if (sensor.get_option_value_description(option, val) == nullptr)
322  continue;
323  dict[sensor.get_option_value_description(option, val)] = val;
324  }
325  }
326  return dict;
327 }
328 
329 namespace realsense2_camera
330 {
331 
332 template <typename K, typename V>
333 std::ostream& operator<<(std::ostream& os, const std::map<K, V>& m)
334 {
335  os << '{';
336  for (const auto& kv : m)
337  {
338  os << " {" << kv.first << ": " << kv.second << '}';
339  }
340  os << " }";
341  return os;
342 }
343 
344 }
345 
349 bool isValidCharInName(char c)
350 {
351  return std::isalnum(c) || c == '/' || c == '_';
352 }
353 
358 std::string create_graph_resource_name(const std::string &original_name)
359 {
360  std::string fixed_name = original_name;
361  std::transform(fixed_name.begin(), fixed_name.end(), fixed_name.begin(),
362  [](unsigned char c) { return std::tolower(c); });
363  std::replace_if(fixed_name.begin(), fixed_name.end(), [](const char c) { return !isValidCharInName(c); },
364  '_');
365  return fixed_name;
366 }
367 
368 void BaseRealSenseNode::set_auto_exposure_roi(const std::string option_name, rs2::sensor sensor, int new_value)
369 {
370  rs2::region_of_interest& auto_exposure_roi(_auto_exposure_roi[sensor.get_info(RS2_CAMERA_INFO_NAME)]);
371  if (option_name == "left")
372  auto_exposure_roi.min_x = new_value;
373  else if (option_name == "right")
374  auto_exposure_roi.max_x = new_value;
375  else if (option_name == "top")
376  auto_exposure_roi.min_y = new_value;
377  else if (option_name == "bottom")
378  auto_exposure_roi.max_y = new_value;
379  else
380  {
381  ROS_WARN_STREAM("Invalid option_name: " << option_name << " while setting auto exposure ROI.");
382  return;
383  }
384  set_sensor_auto_exposure_roi(sensor);
385 }
386 
387 void BaseRealSenseNode::set_sensor_auto_exposure_roi(rs2::sensor sensor)
388 {
389  const rs2::region_of_interest& auto_exposure_roi(_auto_exposure_roi[sensor.get_info(RS2_CAMERA_INFO_NAME)]);
390  try
391  {
392  sensor.as<rs2::roi_sensor>().set_region_of_interest(auto_exposure_roi);
393  }
394  catch(const std::runtime_error& e)
395  {
396  ROS_ERROR_STREAM(e.what());
397  }
398 }
399 
400 void BaseRealSenseNode::readAndSetDynamicParam(ros::NodeHandle& nh1, std::shared_ptr<ddynamic_reconfigure::DDynamicReconfigure> ddynrec,
401  const std::string option_name, const int min_val, const int max_val, rs2::sensor sensor,
402  int* option_value)
403 {
404  nh1.param(option_name, *option_value, *option_value); //param (const std::string &param_name, T &param_val, const T &default_val) const
405  if (*option_value < min_val) *option_value = min_val;
406  if (*option_value > max_val) *option_value = max_val;
407 
408  ddynrec->registerVariable<int>(
409  option_name, *option_value, [this, sensor, option_name](int new_value){set_auto_exposure_roi(option_name, sensor, new_value);},
410  "auto-exposure " + option_name + " coordinate", min_val, max_val);
411 }
412 
413 void BaseRealSenseNode::registerAutoExposureROIOptions(ros::NodeHandle& nh)
414 {
415  for (const std::pair<stream_index_pair, std::vector<rs2::stream_profile>>& profile : _enabled_profiles)
416  {
417  rs2::sensor sensor = _sensors[profile.first];
418  std::string module_base_name(sensor.get_info(RS2_CAMERA_INFO_NAME));
419  if (sensor.is<rs2::roi_sensor>() && _auto_exposure_roi.find(module_base_name) == _auto_exposure_roi.end())
420  {
421  int max_x(_width[profile.first]-1);
422  int max_y(_height[profile.first]-1);
423 
424  std::string module_name = create_graph_resource_name(module_base_name) +"/auto_exposure_roi";
425  ros::NodeHandle nh1(nh, module_name);
426  std::shared_ptr<ddynamic_reconfigure::DDynamicReconfigure> ddynrec = std::make_shared<ddynamic_reconfigure::DDynamicReconfigure>(nh1);
427 
428  _auto_exposure_roi[module_base_name] = {0, 0, max_x, max_y};
429  rs2::region_of_interest& auto_exposure_roi(_auto_exposure_roi[module_base_name]);
430  readAndSetDynamicParam(nh1, ddynrec, "left", 0, max_x, sensor, &(auto_exposure_roi.min_x));
431  readAndSetDynamicParam(nh1, ddynrec, "right", 0, max_x, sensor, &(auto_exposure_roi.max_x));
432  readAndSetDynamicParam(nh1, ddynrec, "top", 0, max_y, sensor, &(auto_exposure_roi.min_y));
433  readAndSetDynamicParam(nh1, ddynrec, "bottom", 0, max_y, sensor, &(auto_exposure_roi.max_y));
434 
435  ddynrec->publishServicesTopics();
436  _ddynrec.push_back(ddynrec);
437 
438  // Initiate the call to set_sensor_auto_exposure_roi, after the first frame arrive.
439  rs2_stream stream_type = profile.first.first;
440  _video_functions_stack[stream_type].push_back([this, sensor](){set_sensor_auto_exposure_roi(sensor);});
441  _is_first_frame[stream_type] = true;
442  }
443  }
444 }
445 
446 void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor, std::string& module_name)
447 {
448  ros::NodeHandle nh1(nh, module_name);
449  std::shared_ptr<ddynamic_reconfigure::DDynamicReconfigure> ddynrec = std::make_shared<ddynamic_reconfigure::DDynamicReconfigure>(nh1);
450  for (auto i = 0; i < RS2_OPTION_COUNT; i++)
451  {
452  rs2_option option = static_cast<rs2_option>(i);
453  const std::string option_name(create_graph_resource_name(rs2_option_to_string(option)));
454  try
455  {
456  if (!sensor.supports(option) || sensor.is_option_read_only(option))
457  {
458  continue;
459  }
460  if (is_checkbox(sensor, option))
461  {
462  auto option_value = bool(sensor.get_option(option));
463  if (nh1.param(option_name, option_value, option_value))
464  {
465  sensor.set_option(option, option_value);
466  }
467  ddynrec->registerVariable<bool>(
468  option_name, option_value,
469  [option, sensor](bool new_value) { sensor.set_option(option, new_value); },
470  sensor.get_option_description(option));
471  continue;
472  }
473  const auto enum_dict = get_enum_method(sensor, option);
474  if (enum_dict.empty())
475  {
476  rs2::option_range op_range = sensor.get_option_range(option);
477  const auto sensor_option_value = sensor.get_option(option);
478  auto option_value = sensor_option_value;
479  if (nh1.param(option_name, option_value, option_value))
480  {
481  if (option_value < op_range.min || op_range.max < option_value)
482  {
483  ROS_WARN_STREAM("Param '" << nh1.resolveName(option_name) << "' has value " << option_value
484  << " outside the range [" << op_range.min << ", " << op_range.max
485  << "]. Using current sensor value " << sensor_option_value << " instead.");
486  option_value = sensor_option_value;
487  }
488  else
489  {
490  sensor.set_option(option, option_value);
491  }
492  }
493  if (option_value < op_range.min || op_range.max < option_value)
494  {
495  ROS_WARN_STREAM("Param '" << nh1.resolveName(option_name) << "' has value " << option_value
496  << " that is not in range [" << op_range.min << ", " << op_range.max << "]"
497  << ". Removing this parameter from dynamic reconfigure options.");
498  continue;
499  }
500  if (is_int_option(sensor, option))
501  {
502  ddynrec->registerVariable<int>(
503  option_name, int(option_value),
504  [option, sensor](int new_value) { sensor.set_option(option, new_value); },
505  sensor.get_option_description(option), int(op_range.min), int(op_range.max));
506  }
507  else
508  {
509  if (i == RS2_OPTION_DEPTH_UNITS)
510  {
511  if (ROS_DEPTH_SCALE >= op_range.min && ROS_DEPTH_SCALE <= op_range.max)
512  {
513  sensor.set_option(option, ROS_DEPTH_SCALE);
514  op_range.min = ROS_DEPTH_SCALE;
515  op_range.max = ROS_DEPTH_SCALE;
516 
517  _depth_scale_meters = ROS_DEPTH_SCALE;
518  }
519  }
520  else
521  {
522  ddynrec->registerVariable<double>(
523  option_name, option_value,
524  [option, sensor](double new_value) { sensor.set_option(option, new_value); },
525  sensor.get_option_description(option), double(op_range.min), double(op_range.max));
526  }
527  }
528  }
529  else
530  {
531  const auto sensor_option_value = sensor.get_option(option);
532  auto option_value = int(sensor_option_value);
533  if (nh1.param(option_name, option_value, option_value))
534  {
535  if (std::find_if(enum_dict.cbegin(), enum_dict.cend(),
536  [&option_value](const std::pair<std::string, int>& kv) {
537  return kv.second == option_value;
538  }) == enum_dict.cend())
539  {
540  ROS_WARN_STREAM("Param '" << nh1.resolveName(option_name) << "' has value " << option_value
541  << " that is not in the enum " << enum_dict
542  << ". Using current sensor value " << sensor_option_value << " instead.");
543  option_value = sensor_option_value;
544  }
545  else
546  {
547  sensor.set_option(option, option_value);
548  }
549  }
550  if (std::find_if(enum_dict.cbegin(), enum_dict.cend(),
551  [&option_value](const std::pair<std::string, int>& kv) {
552  return kv.second == option_value;
553  }) == enum_dict.cend())
554  {
555  ROS_WARN_STREAM("Param '" << nh1.resolveName(option_name) << "' has value " << option_value
556  << " that is not in the enum " << enum_dict
557  << ". Removing this parameter from dynamic reconfigure options.");
558  continue;
559  }
560  if (module_name == "stereo_module" && i == RS2_OPTION_SEQUENCE_ID)
561  {
562  ddynrec->registerEnumVariable<int>(
563  option_name, option_value,
564  [this, option, sensor, module_name](int new_value)
565  {
566  sensor.set_option(option, new_value);
567  _update_functions_v.push_back([this, module_name, sensor]()
568  {set_sensor_parameter_to_ros(module_name, sensor, RS2_OPTION_EXPOSURE);});
569  _update_functions_v.push_back([this, module_name, sensor]()
570  {set_sensor_parameter_to_ros(module_name, sensor, RS2_OPTION_GAIN);});
571  _update_functions_cv.notify_one();
572  },
573  sensor.get_option_description(option), enum_dict);
574  }
575  else
576  {
577  ddynrec->registerEnumVariable<int>(
578  option_name, option_value,
579  [option, sensor](int new_value) { sensor.set_option(option, new_value); },
580  sensor.get_option_description(option), enum_dict);
581  }
582  }
583  }
584  catch(const rs2::backend_error& e)
585  {
586  ROS_WARN_STREAM("Failed to set option: " << option_name << ": " << e.what());
587  }
588  catch(const std::exception& e)
589  {
590  std::cerr << e.what() << '\n';
591  }
592 
593  }
594  ddynrec->publishServicesTopics();
595  _ddynrec.push_back(ddynrec);
596 }
597 
598 void BaseRealSenseNode::registerDynamicReconfigCb(ros::NodeHandle& nh)
599 {
600  ROS_INFO("Setting Dynamic reconfig parameters.");
601 
602  for(rs2::sensor sensor : _dev_sensors)
603  {
604  std::string module_name = create_graph_resource_name(sensor.get_info(RS2_CAMERA_INFO_NAME));
605  ROS_DEBUG_STREAM("module_name:" << module_name);
606  registerDynamicOption(nh, sensor, module_name);
607  }
608 
609  for (NamedFilter nfilter : _filters)
610  {
611  std::string module_name = nfilter._name;
612  auto sensor = *(nfilter._filter);
613  ROS_DEBUG_STREAM("module_name:" << module_name);
614  registerDynamicOption(nh, sensor, module_name);
615  }
616  ROS_INFO("Done Setting Dynamic reconfig parameters.");
617 }
618 
619 void BaseRealSenseNode::registerHDRoptions()
620 {
621  if (std::find_if(std::begin(_filters), std::end(_filters), [](NamedFilter f){return f._name == "hdr_merge";}) == std::end(_filters))
622  return;
623 
624  std::string module_name;
625  std::vector<rs2_option> options{RS2_OPTION_EXPOSURE, RS2_OPTION_GAIN};
626  for(rs2::sensor sensor : _dev_sensors)
627  {
628  if (!sensor.is<rs2::depth_sensor>()) continue;
629  std::string module_name = create_graph_resource_name(sensor.get_info(RS2_CAMERA_INFO_NAME));
630  // Read initialization parameters and initialize hdr_merge filter:
631  unsigned int seq_size = sensor.get_option(RS2_OPTION_SEQUENCE_SIZE);
632  for (unsigned int seq_id = 1; seq_id <= seq_size; seq_id++ )
633  {
634  sensor.set_option(RS2_OPTION_SEQUENCE_ID, seq_id);
635  for (rs2_option& option : options)
636  {
637  std::stringstream param_name_str;
638  param_name_str << module_name << "/" << create_graph_resource_name(rs2_option_to_string(option)) << "/" << seq_id;
639  std::string param_name(param_name_str.str());
640  ROS_INFO_STREAM("Reading option: " << param_name);
641  int option_value = sensor.get_option(option);
642 
643  int user_set_option_value;
644  _pnh.param(param_name, user_set_option_value, option_value);
645  _pnh.deleteParam(param_name);
646  if (option_value != user_set_option_value)
647  {
648  ROS_INFO_STREAM("Set " << rs2_option_to_string(option) << " to " << user_set_option_value);
649  sensor.set_option(option, user_set_option_value);
650  }
651  }
652  }
653  sensor.set_option(RS2_OPTION_SEQUENCE_ID, 0); // Set back to default.
654  sensor.set_option(RS2_OPTION_HDR_ENABLED, true);
655 
656  monitor_update_functions(); // Start parameters update thread
657 
658  break;
659  }
660 }
661 
662 void BaseRealSenseNode::set_sensor_parameter_to_ros(const std::string& module_name, rs2::options sensor, rs2_option option)
663 {
664  dynamic_reconfigure::ReconfigureRequest srv_req;
665  dynamic_reconfigure::ReconfigureResponse srv_resp;
666  dynamic_reconfigure::IntParameter int_param;
667  dynamic_reconfigure::Config conf;
668 
669  int_param.name = create_graph_resource_name(rs2_option_to_string(option)); //"sequence_id"
670  int_param.value = sensor.get_option(option);
671  conf.ints.push_back(int_param);
672 
673  srv_req.config = conf;
674 
675  std::string service_name = module_name + "/set_parameters";
676  if (ros::service::call(service_name, srv_req, srv_resp)) {
677  ROS_INFO_STREAM( "call to set " << service_name << "/" << int_param.name << " to " << int_param.value << " succeeded");
678  } else {
679  ROS_ERROR_STREAM( "call to set " << service_name << "/" << int_param.name << " to " << int_param.value << " failed");
680  }
681 }
682 
683 void BaseRealSenseNode::monitor_update_functions()
684 {
685  int time_interval(1000);
686  std::function<void()> func = [this, time_interval](){
687  std::mutex mu;
688  std::unique_lock<std::mutex> lock(mu);
689  while(_is_running) {
690  _update_functions_cv.wait_for(lock, std::chrono::milliseconds(time_interval), [&]{return !_is_running || !_update_functions_v.empty();});
691  while (!_update_functions_v.empty())
692  {
693  _update_functions_v.back()();
694  _update_functions_v.pop_back();
695  }
696  }
697  };
698  _update_functions_t = std::make_shared<std::thread>(func);
699 }
700 
701 rs2_stream BaseRealSenseNode::rs2_string_to_stream(std::string str)
702 {
703  if (str == "RS2_STREAM_ANY")
704  return RS2_STREAM_ANY;
705  if (str == "RS2_STREAM_COLOR")
706  return RS2_STREAM_COLOR;
707  if (str == "RS2_STREAM_INFRARED")
708  return RS2_STREAM_INFRARED;
709  if (str == "RS2_STREAM_FISHEYE")
710  return RS2_STREAM_FISHEYE;
711  throw std::runtime_error("Unknown stream string " + str);
712 }
713 
714 void BaseRealSenseNode::getParameters()
715 {
716  ROS_INFO("getParameters...");
717 
718  // Setup system to use RGB image from the infra stream if configured by user
719  bool infra_rgb;
720  _pnh.param("infra_rgb", infra_rgb, false);
721  if (infra_rgb)
722  {
723  _format[RS2_STREAM_INFRARED] = RS2_FORMAT_RGB8;
724  _image_format[RS2_STREAM_INFRARED] = CV_8UC3; // CVBridge type
725  _encoding[RS2_STREAM_INFRARED] = sensor_msgs::image_encodings::RGB8; // ROS message type
726  _unit_step_size[RS2_STREAM_INFRARED] = 3 * sizeof(uint8_t); // sensor_msgs::ImagePtr row step size
727  ROS_INFO_STREAM("Infrared RGB stream enabled");
728  }
729 
730  _pnh.param("align_depth", _align_depth, ALIGN_DEPTH);
731  _pnh.param("enable_pointcloud", _pointcloud, POINTCLOUD);
732  std::string pc_texture_stream("");
733  int pc_texture_idx;
734  _pnh.param("pointcloud_texture_stream", pc_texture_stream, std::string("RS2_STREAM_COLOR"));
735  _pnh.param("pointcloud_texture_index", pc_texture_idx, 0);
736  _pointcloud_texture = stream_index_pair{rs2_string_to_stream(pc_texture_stream), pc_texture_idx};
737 
738  _pnh.param("filters", _filters_str, DEFAULT_FILTERS);
739  _pointcloud |= (_filters_str.find("pointcloud") != std::string::npos);
740 
741  _pnh.param("publish_tf", _publish_tf, PUBLISH_TF);
742  _pnh.param("tf_publish_rate", _tf_publish_rate, TF_PUBLISH_RATE);
743 
744  _pnh.param("enable_sync", _sync_frames, SYNC_FRAMES);
745  if (_pointcloud || _align_depth || _filters_str.size() > 0)
746  _sync_frames = true;
747 
748  _pnh.param("json_file_path", _json_file_path, std::string(""));
749 
750  for (auto& stream : IMAGE_STREAMS)
751  {
752  std::string param_name(_stream_name[stream.first] + "_width");
753  _pnh.param(param_name, _width[stream], IMAGE_WIDTH);
754  ROS_DEBUG_STREAM("parameter:" << param_name << " = " << _width[stream]);
755  param_name = _stream_name[stream.first] + "_height";
756  _pnh.param(param_name, _height[stream], IMAGE_HEIGHT);
757  ROS_DEBUG_STREAM("parameter:" << param_name << " = " << _height[stream]);
758  param_name = _stream_name[stream.first] + "_fps";
759  _pnh.param(param_name, _fps[stream], IMAGE_FPS);
760  ROS_DEBUG_STREAM("parameter:" << param_name << " = " << _fps[stream]);
761  param_name = "enable_" + STREAM_NAME(stream);
762  _pnh.param(param_name, _enable[stream], true);
763  ROS_DEBUG_STREAM("parameter:" << param_name << " = " << _enable[stream]);
764  }
765 
766  for (auto& stream : HID_STREAMS)
767  {
768  std::string param_name(_stream_name[stream.first] + "_fps");
769  ROS_DEBUG_STREAM("reading parameter:" << param_name);
770  _pnh.param(param_name, _fps[stream], IMU_FPS);
771  param_name = "enable_" + STREAM_NAME(stream);
772  _pnh.param(param_name, _enable[stream], ENABLE_IMU);
773  ROS_DEBUG_STREAM("_enable[" << _stream_name[stream.first] << "]:" << _enable[stream]);
774  }
775  _pnh.param("base_frame_id", _base_frame_id, DEFAULT_BASE_FRAME_ID);
776  _pnh.param("odom_frame_id", _odom_frame_id, DEFAULT_ODOM_FRAME_ID);
777 
778  std::vector<stream_index_pair> streams(IMAGE_STREAMS);
779  streams.insert(streams.end(), HID_STREAMS.begin(), HID_STREAMS.end());
780  for (auto& stream : streams)
781  {
782  std::string param_name(static_cast<std::ostringstream&&>(std::ostringstream() << STREAM_NAME(stream) << "_frame_id").str());
783  _pnh.param(param_name, _frame_id[stream], FRAME_ID(stream));
784  ROS_DEBUG_STREAM("frame_id: reading parameter:" << param_name << " : " << _frame_id[stream]);
785  param_name = static_cast<std::ostringstream&&>(std::ostringstream() << STREAM_NAME(stream) << "_optical_frame_id").str();
786  _pnh.param(param_name, _optical_frame_id[stream], OPTICAL_FRAME_ID(stream));
787  ROS_DEBUG_STREAM("optical: reading parameter:" << param_name << " : " << _optical_frame_id[stream]);
788  }
789 
790  std::string unite_imu_method_str("");
791  _pnh.param("unite_imu_method", unite_imu_method_str, DEFAULT_UNITE_IMU_METHOD);
792  if (unite_imu_method_str == "linear_interpolation")
793  _imu_sync_method = imu_sync_method::LINEAR_INTERPOLATION;
794  else if (unite_imu_method_str == "copy")
795  _imu_sync_method = imu_sync_method::COPY;
796  else
797  _imu_sync_method = imu_sync_method::NONE;
798 
799  if (_imu_sync_method > imu_sync_method::NONE)
800  {
801  _pnh.param("imu_optical_frame_id", _optical_frame_id[GYRO], DEFAULT_IMU_OPTICAL_FRAME_ID);
802  }
803 
804  {
805  stream_index_pair stream(COLOR);
806  std::string param_name(static_cast<std::ostringstream&&>(std::ostringstream() << "aligned_depth_to_" << STREAM_NAME(stream) << "_frame_id").str());
807  _pnh.param(param_name, _depth_aligned_frame_id[stream], ALIGNED_DEPTH_TO_FRAME_ID(stream));
808  }
809 
810  _pnh.param("allow_no_texture_points", _allow_no_texture_points, ALLOW_NO_TEXTURE_POINTS);
811  _pnh.param("ordered_pc", _ordered_pc, ORDERED_POINTCLOUD);
812  _pnh.param("clip_distance", _clipping_distance, static_cast<float>(-1.0));
813  _pnh.param("linear_accel_cov", _linear_accel_cov, static_cast<double>(0.01));
814  _pnh.param("angular_velocity_cov", _angular_velocity_cov, static_cast<double>(0.01));
815  _pnh.param("hold_back_imu_for_frames", _hold_back_imu_for_frames, HOLD_BACK_IMU_FOR_FRAMES);
816  _pnh.param("publish_odom_tf", _publish_odom_tf, PUBLISH_ODOM_TF);
817 }
818 
819 void BaseRealSenseNode::setupDevice()
820 {
821  ROS_INFO("setupDevice...");
822  try{
823  if (!_json_file_path.empty())
824  {
825  if (_dev.is<rs2::serializable_device>())
826  {
827  std::stringstream ss;
828  std::ifstream in(_json_file_path);
829  if (in.is_open())
830  {
831  ss << in.rdbuf();
832  std::string json_file_content = ss.str();
833 
834  auto adv = _dev.as<rs2::serializable_device>();
835  adv.load_json(json_file_content);
836  ROS_INFO_STREAM("JSON file is loaded! (" << _json_file_path << ")");
837  }
838  else
839  ROS_WARN_STREAM("JSON file provided doesn't exist! (" << _json_file_path << ")");
840  }
841  else
842  ROS_WARN("Device does not support advanced settings!");
843  }
844  else
845  ROS_INFO("JSON file is not provided");
846 
847  ROS_INFO_STREAM("ROS Node Namespace: " << _namespace);
848 
849  auto camera_name = _dev.get_info(RS2_CAMERA_INFO_NAME);
850  ROS_INFO_STREAM("Device Name: " << camera_name);
851 
852  ROS_INFO_STREAM("Device Serial No: " << _serial_no);
853 
854  auto camera_id = _dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT);
855 
856  ROS_INFO_STREAM("Device physical port: " << camera_id);
857 
858  auto fw_ver = _dev.get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION);
859  ROS_INFO_STREAM("Device FW version: " << fw_ver);
860 
861  auto pid = _dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
862  ROS_INFO_STREAM("Device Product ID: 0x" << pid);
863 
864  ROS_INFO_STREAM("Enable PointCloud: " << ((_pointcloud)?"On":"Off"));
865  ROS_INFO_STREAM("Align Depth: " << ((_align_depth)?"On":"Off"));
866  ROS_INFO_STREAM("Sync Mode: " << ((_sync_frames)?"On":"Off"));
867 
868  _dev_sensors = _dev.query_sensors();
869 
870 
871  std::function<void(rs2::frame)> frame_callback_function, imu_callback_function;
872  if (_sync_frames)
873  {
874  frame_callback_function = _syncer;
875 
876  auto frame_callback_inner = [this](rs2::frame frame){
877  frame_callback(frame);
878  };
879  _syncer.start(frame_callback_inner);
880  }
881  else
882  {
883  frame_callback_function = [this](rs2::frame frame){frame_callback(frame);};
884  }
885 
886  if (_imu_sync_method == imu_sync_method::NONE)
887  {
888  imu_callback_function = [this](rs2::frame frame){imu_callback(frame);};
889  }
890  else
891  {
892  imu_callback_function = [this](rs2::frame frame){imu_callback_sync(frame, _imu_sync_method);};
893  }
894  std::function<void(rs2::frame)> multiple_message_callback_function = [this](rs2::frame frame){multiple_message_callback(frame, _imu_sync_method);};
895 
896  ROS_INFO_STREAM("Device Sensors: ");
897  for(auto&& sensor : _dev_sensors)
898  {
899  for (auto& profile : sensor.get_stream_profiles())
900  {
901  auto video_profile = profile.as<rs2::video_stream_profile>();
902  stream_index_pair sip(video_profile.stream_type(), video_profile.stream_index());
903  if (_sensors.find( sip ) != _sensors.end())
904  continue;
905  _sensors[sip] = sensor;
906  }
907 
908  std::string module_name = sensor.get_info(RS2_CAMERA_INFO_NAME);
909  if (sensor.is<rs2::depth_sensor>())
910  {
911  _sensors_callback[module_name] = frame_callback_function;
912  }
913  else if (sensor.is<rs2::color_sensor>())
914  {
915  _sensors_callback[module_name] = frame_callback_function;
916  }
917  else if (sensor.is<rs2::fisheye_sensor>())
918  {
919  _sensors_callback[module_name] = frame_callback_function;
920  }
921  else if (sensor.is<rs2::motion_sensor>())
922  {
923  _sensors_callback[module_name] = imu_callback_function;
924  }
925  else if (sensor.is<rs2::pose_sensor>())
926  {
927  _sensors_callback[module_name] = multiple_message_callback_function;
928  }
929  else
930  {
931  ROS_ERROR_STREAM("Module Name \"" << module_name << "\" isn't supported by LibRealSense! Terminating RealSense Node...");
932  ros::shutdown();
933  exit(1);
934  }
935  ROS_INFO_STREAM(module_name << " was found.");
936  }
937 
938  // Update "enable" map
939  for (std::pair<stream_index_pair, bool> const& enable : _enable )
940  {
941  const stream_index_pair& stream_index(enable.first);
942  if (enable.second && _sensors.find(stream_index) == _sensors.end())
943  {
944  ROS_INFO_STREAM("(" << rs2_stream_to_string(stream_index.first) << ", " << stream_index.second << ") sensor isn't supported by current device! -- Skipping...");
945  _enable[enable.first] = false;
946  }
947  }
948  }
949  catch(const std::exception& ex)
950  {
951  ROS_ERROR_STREAM("An exception has been thrown: " << ex.what());
952  throw;
953  }
954  catch(...)
955  {
956  ROS_ERROR_STREAM("Unknown exception has occured!");
957  throw;
958  }
959 }
960 
961 void BaseRealSenseNode::setupPublishers()
962 {
963  ROS_INFO("setupPublishers...");
964  image_transport::ImageTransport image_transport(_node_handle);
965 
966  for (auto& stream : IMAGE_STREAMS)
967  {
968  if (_enable[stream])
969  {
970  std::stringstream image_raw, camera_info;
971  bool rectified_image = false;
972  if (stream == DEPTH || stream == CONFIDENCE || stream == INFRA1 || stream == INFRA2)
973  rectified_image = true;
974 
975  std::string stream_name(STREAM_NAME(stream));
976  image_raw << stream_name << "/image_" << ((rectified_image)?"rect_":"") << "raw";
977  camera_info << stream_name << "/camera_info";
978 
979  std::shared_ptr<FrequencyDiagnostics> frequency_diagnostics(new FrequencyDiagnostics(_fps[stream], stream_name, _serial_no));
980  _image_publishers[stream] = {image_transport.advertise(image_raw.str(), 1), frequency_diagnostics};
981  _info_publisher[stream] = _node_handle.advertise<sensor_msgs::CameraInfo>(camera_info.str(), 1);
982 
983  if (_align_depth && stream == COLOR)
984  {
985  std::stringstream aligned_image_raw, aligned_camera_info;
986  aligned_image_raw << "aligned_depth_to_" << stream_name << "/image_raw";
987  aligned_camera_info << "aligned_depth_to_" << stream_name << "/camera_info";
988 
989  std::string aligned_stream_name = "aligned_depth_to_" + stream_name;
990  std::shared_ptr<FrequencyDiagnostics> frequency_diagnostics(new FrequencyDiagnostics(_fps[stream], aligned_stream_name, _serial_no));
991  _depth_aligned_image_publishers[stream] = {image_transport.advertise(aligned_image_raw.str(), 1), frequency_diagnostics};
992  _depth_aligned_info_publisher[stream] = _node_handle.advertise<sensor_msgs::CameraInfo>(aligned_camera_info.str(), 1);
993  }
994 
995  if (stream == DEPTH && _pointcloud)
996  {
997  _pointcloud_publisher = _node_handle.advertise<sensor_msgs::PointCloud2>("depth/color/points", 1);
998  }
999  }
1000  }
1001 
1002  _synced_imu_publisher = std::make_shared<SyncedImuPublisher>();
1003  if (_imu_sync_method > imu_sync_method::NONE && _enable[GYRO] && _enable[ACCEL])
1004  {
1005  ROS_INFO("Start publisher IMU");
1006  _synced_imu_publisher = std::make_shared<SyncedImuPublisher>(_node_handle.advertise<sensor_msgs::Imu>("imu", 5));
1007  _synced_imu_publisher->Enable(_hold_back_imu_for_frames);
1008  }
1009  else
1010  {
1011  if (_enable[GYRO])
1012  {
1013  _imu_publishers[GYRO] = _node_handle.advertise<sensor_msgs::Imu>("gyro/sample", 100);
1014  }
1015 
1016  if (_enable[ACCEL])
1017  {
1018  _imu_publishers[ACCEL] = _node_handle.advertise<sensor_msgs::Imu>("accel/sample", 100);
1019  }
1020  }
1021  if (_enable[POSE])
1022  {
1023  _imu_publishers[POSE] = _node_handle.advertise<nav_msgs::Odometry>("odom/sample", 100);
1024  }
1025 
1026 
1027  if (_enable[FISHEYE] &&
1028  _enable[DEPTH])
1029  {
1030  _depth_to_other_extrinsics_publishers[FISHEYE] = _node_handle.advertise<Extrinsics>("extrinsics/depth_to_fisheye", 1, true);
1031  }
1032 
1033  if (_enable[COLOR] &&
1034  _enable[DEPTH])
1035  {
1036  _depth_to_other_extrinsics_publishers[COLOR] = _node_handle.advertise<Extrinsics>("extrinsics/depth_to_color", 1, true);
1037  }
1038 
1039  if (_enable[INFRA1] &&
1040  _enable[DEPTH])
1041  {
1042  _depth_to_other_extrinsics_publishers[INFRA1] = _node_handle.advertise<Extrinsics>("extrinsics/depth_to_infra1", 1, true);
1043  }
1044 
1045  if (_enable[INFRA2] &&
1046  _enable[DEPTH])
1047  {
1048  _depth_to_other_extrinsics_publishers[INFRA2] = _node_handle.advertise<Extrinsics>("extrinsics/depth_to_infra2", 1, true);
1049  }
1050 }
1051 
1052 void BaseRealSenseNode::enable_devices()
1053 {
1054  for (auto& elem : IMAGE_STREAMS)
1055  {
1056  if (_enable[elem])
1057  {
1058  auto& sens = _sensors[elem];
1059  auto profiles = sens.get_stream_profiles();
1060  rs2::stream_profile default_profile, selected_profile;
1061  for (auto& profile : profiles)
1062  {
1063  auto video_profile = profile.as<rs2::video_stream_profile>();
1064  ROS_DEBUG_STREAM("Sensor profile: " <<
1065  "stream_type: " << rs2_stream_to_string(video_profile.stream_type()) << "(" << video_profile.stream_index() << ")" <<
1066  "Format: " << video_profile.format() <<
1067  ", Width: " << video_profile.width() <<
1068  ", Height: " << video_profile.height() <<
1069  ", FPS: " << video_profile.fps());
1070 
1071  if (profile.stream_type() == elem.first && profile.stream_index() == elem.second)
1072  {
1073  if (profile.is_default())
1074  {
1075  default_profile = profile;
1076  }
1077  if ((_width[elem] == 0 || video_profile.width() == _width[elem]) &&
1078  (_height[elem] == 0 || video_profile.height() == _height[elem]) &&
1079  (_fps[elem] == 0 || video_profile.fps() == _fps[elem]) &&
1080  (_format.find(elem.first) == _format.end() || video_profile.format() == _format[elem.first] ))
1081  {
1082  selected_profile = profile;
1083  break;
1084  }
1085  }
1086 
1087  }
1088  if (!selected_profile)
1089  {
1090  ROS_WARN_STREAM_COND((_width[elem]!=-1 && _height[elem]!=-1 && _fps[elem]!=-1), "Given stream configuration is not supported by the device! " <<
1091  " Stream: " << rs2_stream_to_string(elem.first) <<
1092  ", Stream Index: " << elem.second <<
1093  ", Width: " << _width[elem] <<
1094  ", Height: " << _height[elem] <<
1095  ", FPS: " << _fps[elem] <<
1096  ", Format: " << ((_format.find(elem.first) == _format.end())? "None":rs2_format_to_string(rs2_format(_format[elem.first]))));
1097  if (default_profile)
1098  {
1099  ROS_WARN_STREAM_COND((_width[elem]!=-1 && _height[elem]!=-1 && _fps[elem]!=-1), "Using default profile instead.");
1100  selected_profile = default_profile;
1101  }
1102  }
1103  if (selected_profile)
1104  {
1105  auto video_profile = selected_profile.as<rs2::video_stream_profile>();
1106  _width[elem] = video_profile.width();
1107  _height[elem] = video_profile.height();
1108  _fps[elem] = video_profile.fps();
1109  _enabled_profiles[elem].push_back(selected_profile);
1110  _image[elem] = cv::Mat(_height[elem], _width[elem], _image_format[elem.first], cv::Scalar(0, 0, 0));
1111  ROS_INFO_STREAM(STREAM_NAME(elem) << " stream is enabled - width: " << _width[elem] << ", height: " << _height[elem] << ", fps: " << _fps[elem] << ", " << "Format: " << video_profile.format());
1112  }
1113  else
1114  {
1115  _enable[elem] = false;
1116  }
1117  }
1118  }
1119  if (_align_depth)
1120  {
1121  for (auto& profiles : _enabled_profiles)
1122  {
1123  _depth_aligned_image[profiles.first] = cv::Mat(_height[DEPTH], _width[DEPTH], _image_format[DEPTH.first], cv::Scalar(0, 0, 0));
1124  _depth_scaled_image[profiles.first] = cv::Mat(_height[DEPTH], _width[DEPTH], _image_format[DEPTH.first], cv::Scalar(0, 0, 0));
1125  }
1126  }
1127 
1128  // Streaming HID
1129  for (auto& elem : HID_STREAMS)
1130  {
1131  if (_enable[elem])
1132  {
1133  auto& sens = _sensors[elem];
1134  auto profiles = sens.get_stream_profiles();
1135  rs2::stream_profile default_profile, selected_profile;
1136  ROS_DEBUG_STREAM("Available profiles:");
1137  for (rs2::stream_profile& profile : profiles)
1138  {
1139  ROS_DEBUG_STREAM("type:" << rs2_stream_to_string(profile.stream_type()) <<
1140  " fps: " << profile.fps() << ". format: " << profile.format());
1141  }
1142  for (rs2::stream_profile& profile : profiles)
1143  {
1144  if (profile.stream_type() == elem.first)
1145  {
1146  if (profile.is_default())
1147  default_profile = profile;
1148  if (_fps[elem] == 0 || profile.fps() == _fps[elem])
1149  {
1150  selected_profile = profile;
1151  break;
1152  }
1153  }
1154  }
1155  if (!selected_profile)
1156  {
1157  std::string stream_name(STREAM_NAME(elem));
1158  ROS_WARN_STREAM_COND((_fps[elem]!=-1), "No mathcing profile found for " << stream_name << " with fps=" << _fps[elem]);
1159  if (default_profile)
1160  {
1161  ROS_WARN_STREAM_COND((_fps[elem]!=-1), "Using default profile instead.");
1162  selected_profile = default_profile;
1163  }
1164  }
1165  if(selected_profile)
1166  {
1167  _fps[elem] = selected_profile.fps();
1168  _enabled_profiles[elem].push_back(selected_profile);
1169  ROS_INFO_STREAM(STREAM_NAME(elem) << " stream is enabled - fps: " << _fps[elem]);
1170  }
1171  else
1172  {
1173  _enable[elem] = false;
1174  }
1175  }
1176  }
1177 }
1178 
1179 void BaseRealSenseNode::setupFilters()
1180 {
1181  std::vector<std::string> filters_str;
1182  boost::split(filters_str, _filters_str, [](char c){return c == ',';});
1183  bool use_disparity_filter(false);
1184  bool use_colorizer_filter(false);
1185  bool use_decimation_filter(false);
1186  bool use_hdr_filter(false);
1187  for (std::vector<std::string>::const_iterator s_iter=filters_str.begin(); s_iter!=filters_str.end(); s_iter++)
1188  {
1189  if ((*s_iter) == "colorizer")
1190  {
1191  use_colorizer_filter = true;
1192  }
1193  else if ((*s_iter) == "disparity")
1194  {
1195  use_disparity_filter = true;
1196  }
1197  else if ((*s_iter) == "spatial")
1198  {
1199  ROS_INFO("Add Filter: spatial");
1200  _filters.push_back(NamedFilter("spatial", std::make_shared<rs2::spatial_filter>()));
1201  }
1202  else if ((*s_iter) == "temporal")
1203  {
1204  ROS_INFO("Add Filter: temporal");
1205  _filters.push_back(NamedFilter("temporal", std::make_shared<rs2::temporal_filter>()));
1206  }
1207  else if ((*s_iter) == "hole_filling")
1208  {
1209  ROS_INFO("Add Filter: hole_filling");
1210  _filters.push_back(NamedFilter("hole_filling", std::make_shared<rs2::hole_filling_filter>()));
1211  }
1212  else if ((*s_iter) == "decimation")
1213  {
1214  use_decimation_filter = true;
1215  }
1216  else if ((*s_iter) == "pointcloud")
1217  {
1218  assert(_pointcloud); // For now, it is set in getParameters()..
1219  }
1220  else if ((*s_iter) == "hdr_merge")
1221  {
1222  use_hdr_filter = true;
1223  }
1224  else if ((*s_iter).size() > 0)
1225  {
1226  ROS_ERROR_STREAM("Unknown Filter: " << (*s_iter));
1227  throw;
1228  }
1229  }
1230  if (use_disparity_filter)
1231  {
1232  ROS_INFO("Add Filter: disparity");
1233  _filters.insert(_filters.begin(), NamedFilter("disparity_start", std::make_shared<rs2::disparity_transform>()));
1234  _filters.push_back(NamedFilter("disparity_end", std::make_shared<rs2::disparity_transform>(false)));
1235  ROS_INFO("Done Add Filter: disparity");
1236  }
1237  if (use_hdr_filter)
1238  {
1239  ROS_INFO("Add Filter: hdr_merge");
1240  _filters.insert(_filters.begin(),NamedFilter("hdr_merge", std::make_shared<rs2::hdr_merge>()));
1241  ROS_INFO("Add Filter: sequence_id_filter");
1242  _filters.insert(_filters.begin(),NamedFilter("sequence_id_filter", std::make_shared<rs2::sequence_id_filter>()));
1243  }
1244  if (use_decimation_filter)
1245  {
1246  ROS_INFO("Add Filter: decimation");
1247  _filters.insert(_filters.begin(),NamedFilter("decimation", std::make_shared<rs2::decimation_filter>()));
1248  }
1249  if (_align_depth)
1250  {
1251  _filters.push_back(NamedFilter("align_to_color", std::make_shared<rs2::align>(RS2_STREAM_COLOR)));
1252  }
1253  if (use_colorizer_filter)
1254  {
1255  ROS_INFO("Add Filter: colorizer");
1256  _colorizer = std::make_shared<rs2::colorizer>();
1257  _filters.push_back(NamedFilter("colorizer", _colorizer));
1258  // Types for depth stream
1259  _image_format[DEPTH.first] = _image_format[COLOR.first]; // CVBridge type
1260  _encoding[DEPTH.first] = _encoding[COLOR.first]; // ROS message type
1261  _unit_step_size[DEPTH.first] = _unit_step_size[COLOR.first]; // sensor_msgs::ImagePtr row step size
1262  _depth_aligned_encoding[RS2_STREAM_COLOR] = _encoding[COLOR.first]; // ROS message type
1263 
1264  _width[DEPTH] = _width[COLOR];
1265  _height[DEPTH] = _height[COLOR];
1266  _image[DEPTH] = cv::Mat(std::max(0, _height[DEPTH]), std::max(0, _width[DEPTH]), _image_format[DEPTH.first], cv::Scalar(0, 0, 0));
1267  }
1268  if (_pointcloud)
1269  {
1270  ROS_INFO("Add Filter: pointcloud");
1271  _pointcloud_filter = std::make_shared<rs2::pointcloud>(_pointcloud_texture.first, _pointcloud_texture.second);
1272  _filters.push_back(NamedFilter("pointcloud", _pointcloud_filter));
1273  }
1274  ROS_INFO("num_filters: %d", static_cast<int>(_filters.size()));
1275 }
1276 
1277 cv::Mat& BaseRealSenseNode::fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image)
1278 {
1279  static const float meter_to_mm = 0.001f;
1280  if (fabs(_depth_scale_meters - meter_to_mm) < 1e-6)
1281  {
1282  to_image = from_image;
1283  return to_image;
1284  }
1285 
1286  if (to_image.size() != from_image.size())
1287  {
1288  to_image.create(from_image.rows, from_image.cols, from_image.type());
1289  }
1290 
1291  CV_Assert(from_image.depth() == _image_format[RS2_STREAM_DEPTH]);
1292 
1293  int nRows = from_image.rows;
1294  int nCols = from_image.cols;
1295 
1296  if (from_image.isContinuous())
1297  {
1298  nCols *= nRows;
1299  nRows = 1;
1300  }
1301 
1302  int i,j;
1303  const uint16_t* p_from;
1304  uint16_t* p_to;
1305  for( i = 0; i < nRows; ++i)
1306  {
1307  p_from = from_image.ptr<uint16_t>(i);
1308  p_to = to_image.ptr<uint16_t>(i);
1309  for ( j = 0; j < nCols; ++j)
1310  {
1311  p_to[j] = p_from[j] * _depth_scale_meters / meter_to_mm;
1312  }
1313  }
1314  return to_image;
1315 }
1316 
1317 void BaseRealSenseNode::clip_depth(rs2::depth_frame depth_frame, float clipping_dist)
1318 {
1319  uint16_t* p_depth_frame = reinterpret_cast<uint16_t*>(const_cast<void*>(depth_frame.get_data()));
1320  uint16_t clipping_value = static_cast<uint16_t>(clipping_dist / _depth_scale_meters);
1321 
1322  int width = depth_frame.get_width();
1323  int height = depth_frame.get_height();
1324 
1325  #ifdef _OPENMP
1326  #pragma omp parallel for schedule(dynamic) //Using OpenMP to try to parallelise the loop
1327  #endif
1328  for (int y = 0; y < height; y++)
1329  {
1330  auto depth_pixel_index = y * width;
1331  for (int x = 0; x < width; x++, ++depth_pixel_index)
1332  {
1333  // Check if the depth value is greater than the threashold
1334  if (p_depth_frame[depth_pixel_index] > clipping_value)
1335  {
1336  p_depth_frame[depth_pixel_index] = 0; //Set to invalid (<=0) value.
1337  }
1338  }
1339  }
1340 }
1341 
1342 sensor_msgs::Imu BaseRealSenseNode::CreateUnitedMessage(const CimuData accel_data, const CimuData gyro_data)
1343 {
1344  sensor_msgs::Imu imu_msg;
1345  ros::Time t(gyro_data.m_time);
1346  imu_msg.header.seq = 0;
1347  imu_msg.header.stamp = t;
1348 
1349  imu_msg.angular_velocity.x = gyro_data.m_data.x();
1350  imu_msg.angular_velocity.y = gyro_data.m_data.y();
1351  imu_msg.angular_velocity.z = gyro_data.m_data.z();
1352 
1353  imu_msg.linear_acceleration.x = accel_data.m_data.x();
1354  imu_msg.linear_acceleration.y = accel_data.m_data.y();
1355  imu_msg.linear_acceleration.z = accel_data.m_data.z();
1356  return imu_msg;
1357 }
1358 
1359 template <typename T> T lerp(const T &a, const T &b, const double t) {
1360  return a * (1.0 - t) + b * t;
1361 }
1362 
1363 void BaseRealSenseNode::FillImuData_LinearInterpolation(const CimuData imu_data, std::deque<sensor_msgs::Imu>& imu_msgs)
1364 {
1365  static std::deque<CimuData> _imu_history;
1366  _imu_history.push_back(imu_data);
1367  stream_index_pair type(imu_data.m_type);
1368  imu_msgs.clear();
1369 
1370  if ((type != ACCEL) || _imu_history.size() < 3)
1371  return;
1372 
1373  std::deque<CimuData> gyros_data;
1374  CimuData accel0, accel1, crnt_imu;
1375 
1376  while (_imu_history.size())
1377  {
1378  crnt_imu = _imu_history.front();
1379  _imu_history.pop_front();
1380  if (!accel0.is_set() && crnt_imu.m_type == ACCEL)
1381  {
1382  accel0 = crnt_imu;
1383  }
1384  else if (accel0.is_set() && crnt_imu.m_type == ACCEL)
1385  {
1386  accel1 = crnt_imu;
1387  const double dt = accel1.m_time - accel0.m_time;
1388 
1389  while (gyros_data.size())
1390  {
1391  CimuData crnt_gyro = gyros_data.front();
1392  gyros_data.pop_front();
1393  const double alpha = (crnt_gyro.m_time - accel0.m_time) / dt;
1394  CimuData crnt_accel(ACCEL, lerp(accel0.m_data, accel1.m_data, alpha), crnt_gyro.m_time);
1395  imu_msgs.push_back(CreateUnitedMessage(crnt_accel, crnt_gyro));
1396  }
1397  accel0 = accel1;
1398  }
1399  else if (accel0.is_set() && crnt_imu.m_time >= accel0.m_time && crnt_imu.m_type == GYRO)
1400  {
1401  gyros_data.push_back(crnt_imu);
1402  }
1403  }
1404  _imu_history.push_back(crnt_imu);
1405  return;
1406 }
1407 
1408 void BaseRealSenseNode::FillImuData_Copy(const CimuData imu_data, std::deque<sensor_msgs::Imu>& imu_msgs)
1409 {
1410  stream_index_pair type(imu_data.m_type);
1411 
1412  static CimuData _accel_data(ACCEL, {0,0,0}, -1.0);
1413  if (ACCEL == type)
1414  {
1415  _accel_data = imu_data;
1416  return;
1417  }
1418  if (_accel_data.m_time < 0)
1419  return;
1420 
1421  imu_msgs.push_back(CreateUnitedMessage(_accel_data, imu_data));
1422 }
1423 
1424 void BaseRealSenseNode::ImuMessage_AddDefaultValues(sensor_msgs::Imu& imu_msg)
1425 {
1426  imu_msg.header.frame_id = _optical_frame_id[GYRO];
1427  imu_msg.orientation.x = 0.0;
1428  imu_msg.orientation.y = 0.0;
1429  imu_msg.orientation.z = 0.0;
1430  imu_msg.orientation.w = 0.0;
1431 
1432  imu_msg.orientation_covariance = { -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
1433  imu_msg.linear_acceleration_covariance = { _linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov};
1434  imu_msg.angular_velocity_covariance = { _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov};
1435 }
1436 
1437 void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync_method)
1438 {
1439  static std::mutex m_mutex;
1440  static int seq = 0;
1441 
1442  m_mutex.lock();
1443 
1444  auto stream = frame.get_profile().stream_type();
1445  auto stream_index = (stream == GYRO.first)?GYRO:ACCEL;
1446  double frame_time = frame.get_timestamp();
1447 
1448  bool placeholder_false(false);
1449  if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) )
1450  {
1451  _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain());
1452  }
1453 
1454  seq += 1;
1455 
1456  if (0 != _synced_imu_publisher->getNumSubscribers())
1457  {
1458  auto crnt_reading = *(reinterpret_cast<const float3*>(frame.get_data()));
1459  Eigen::Vector3d v(crnt_reading.x, crnt_reading.y, crnt_reading.z);
1460  CimuData imu_data(stream_index, v, frameSystemTimeSec(frame));
1461  std::deque<sensor_msgs::Imu> imu_msgs;
1462  switch (sync_method)
1463  {
1464  case NONE: //Cannot really be NONE. Just to avoid compilation warning.
1465  case COPY:
1466  FillImuData_Copy(imu_data, imu_msgs);
1467  break;
1468  case LINEAR_INTERPOLATION:
1469  FillImuData_LinearInterpolation(imu_data, imu_msgs);
1470  break;
1471  }
1472  while (imu_msgs.size())
1473  {
1474  sensor_msgs::Imu imu_msg = imu_msgs.front();
1475  imu_msg.header.seq = seq;
1476  ImuMessage_AddDefaultValues(imu_msg);
1477  _synced_imu_publisher->Publish(imu_msg);
1478  ROS_DEBUG("Publish united %s stream", rs2_stream_to_string(frame.get_profile().stream_type()));
1479  imu_msgs.pop_front();
1480  }
1481  }
1482  m_mutex.unlock();
1483 };
1484 
1485 void BaseRealSenseNode::imu_callback(rs2::frame frame)
1486 {
1487  auto stream = frame.get_profile().stream_type();
1488  double frame_time = frame.get_timestamp();
1489  bool placeholder_false(false);
1490  if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) )
1491  {
1492  _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain());
1493  }
1494 
1495  ROS_DEBUG("Frame arrived: stream: %s ; index: %d ; Timestamp Domain: %s",
1496  rs2_stream_to_string(frame.get_profile().stream_type()),
1497  frame.get_profile().stream_index(),
1498  rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain()));
1499 
1500  auto stream_index = (stream == GYRO.first)?GYRO:ACCEL;
1501  if (0 != _imu_publishers[stream_index].getNumSubscribers())
1502  {
1503  ros::Time t(frameSystemTimeSec(frame));
1504 
1505  auto imu_msg = sensor_msgs::Imu();
1506  ImuMessage_AddDefaultValues(imu_msg);
1507  imu_msg.header.frame_id = _optical_frame_id[stream_index];
1508 
1509  auto crnt_reading = *(reinterpret_cast<const float3*>(frame.get_data()));
1510  if (GYRO == stream_index)
1511  {
1512  imu_msg.angular_velocity.x = crnt_reading.x;
1513  imu_msg.angular_velocity.y = crnt_reading.y;
1514  imu_msg.angular_velocity.z = crnt_reading.z;
1515  }
1516  else if (ACCEL == stream_index)
1517  {
1518  imu_msg.linear_acceleration.x = crnt_reading.x;
1519  imu_msg.linear_acceleration.y = crnt_reading.y;
1520  imu_msg.linear_acceleration.z = crnt_reading.z;
1521  }
1522  _seq[stream_index] += 1;
1523  imu_msg.header.seq = _seq[stream_index];
1524  imu_msg.header.stamp = t;
1525  _imu_publishers[stream_index].publish(imu_msg);
1526  ROS_DEBUG("Publish %s stream", rs2_stream_to_string(frame.get_profile().stream_type()));
1527  }
1528 }
1529 
1530 void BaseRealSenseNode::pose_callback(rs2::frame frame)
1531 {
1532  double frame_time = frame.get_timestamp();
1533  bool placeholder_false(false);
1534  if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) )
1535  {
1536  _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain());
1537  }
1538 
1539  ROS_DEBUG("Frame arrived: stream: %s ; index: %d ; Timestamp Domain: %s",
1540  rs2_stream_to_string(frame.get_profile().stream_type()),
1541  frame.get_profile().stream_index(),
1542  rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain()));
1543  const auto& stream_index(POSE);
1544  rs2_pose pose = frame.as<rs2::pose_frame>().get_pose_data();
1545  ros::Time t(frameSystemTimeSec(frame));
1546 
1547  geometry_msgs::PoseStamped pose_msg;
1548  pose_msg.pose.position.x = -pose.translation.z;
1549  pose_msg.pose.position.y = -pose.translation.x;
1550  pose_msg.pose.position.z = pose.translation.y;
1551  pose_msg.pose.orientation.x = -pose.rotation.z;
1552  pose_msg.pose.orientation.y = -pose.rotation.x;
1553  pose_msg.pose.orientation.z = pose.rotation.y;
1554  pose_msg.pose.orientation.w = pose.rotation.w;
1555 
1556  static tf2_ros::TransformBroadcaster br;
1557  geometry_msgs::TransformStamped msg;
1558  msg.header.stamp = t;
1559  msg.header.frame_id = _odom_frame_id;
1560  msg.child_frame_id = _frame_id[POSE];
1561  msg.transform.translation.x = pose_msg.pose.position.x;
1562  msg.transform.translation.y = pose_msg.pose.position.y;
1563  msg.transform.translation.z = pose_msg.pose.position.z;
1564  msg.transform.rotation.x = pose_msg.pose.orientation.x;
1565  msg.transform.rotation.y = pose_msg.pose.orientation.y;
1566  msg.transform.rotation.z = pose_msg.pose.orientation.z;
1567  msg.transform.rotation.w = pose_msg.pose.orientation.w;
1568 
1569  if (_publish_odom_tf) br.sendTransform(msg);
1570 
1571  if (0 != _imu_publishers[stream_index].getNumSubscribers())
1572  {
1573  double cov_pose(_linear_accel_cov * pow(10, 3-(int)pose.tracker_confidence));
1574  double cov_twist(_angular_velocity_cov * pow(10, 1-(int)pose.tracker_confidence));
1575 
1576  geometry_msgs::Vector3Stamped v_msg;
1577  v_msg.vector.x = -pose.velocity.z;
1578  v_msg.vector.y = -pose.velocity.x;
1579  v_msg.vector.z = pose.velocity.y;
1580  tf::Vector3 tfv;
1581  tf::vector3MsgToTF(v_msg.vector,tfv);
1582  tf::Quaternion q(-msg.transform.rotation.x,-msg.transform.rotation.y,-msg.transform.rotation.z,msg.transform.rotation.w);
1583  tfv=tf::quatRotate(q,tfv);
1584  tf::vector3TFToMsg(tfv,v_msg.vector);
1585 
1586  geometry_msgs::Vector3Stamped om_msg;
1587  om_msg.vector.x = -pose.angular_velocity.z;
1588  om_msg.vector.y = -pose.angular_velocity.x;
1589  om_msg.vector.z = pose.angular_velocity.y;
1590  tf::vector3MsgToTF(om_msg.vector,tfv);
1591  tfv=tf::quatRotate(q,tfv);
1592  tf::vector3TFToMsg(tfv,om_msg.vector);
1593 
1594 
1595  nav_msgs::Odometry odom_msg;
1596  _seq[stream_index] += 1;
1597 
1598  odom_msg.header.frame_id = _odom_frame_id;
1599  odom_msg.child_frame_id = _frame_id[POSE];
1600  odom_msg.header.stamp = t;
1601  odom_msg.header.seq = _seq[stream_index];
1602  odom_msg.pose.pose = pose_msg.pose;
1603  odom_msg.pose.covariance = {cov_pose, 0, 0, 0, 0, 0,
1604  0, cov_pose, 0, 0, 0, 0,
1605  0, 0, cov_pose, 0, 0, 0,
1606  0, 0, 0, cov_twist, 0, 0,
1607  0, 0, 0, 0, cov_twist, 0,
1608  0, 0, 0, 0, 0, cov_twist};
1609  odom_msg.twist.twist.linear = v_msg.vector;
1610  odom_msg.twist.twist.angular = om_msg.vector;
1611  odom_msg.twist.covariance ={cov_pose, 0, 0, 0, 0, 0,
1612  0, cov_pose, 0, 0, 0, 0,
1613  0, 0, cov_pose, 0, 0, 0,
1614  0, 0, 0, cov_twist, 0, 0,
1615  0, 0, 0, 0, cov_twist, 0,
1616  0, 0, 0, 0, 0, cov_twist};
1617  _imu_publishers[stream_index].publish(odom_msg);
1618  ROS_DEBUG("Publish %s stream", rs2_stream_to_string(frame.get_profile().stream_type()));
1619  }
1620 }
1621 
1622 void BaseRealSenseNode::frame_callback(rs2::frame frame)
1623 {
1624  _synced_imu_publisher->Pause();
1625 
1626  try{
1627  double frame_time = frame.get_timestamp();
1628 
1629  // We compute a ROS timestamp which is based on an initial ROS time at point of first frame,
1630  // and the incremental timestamp from the camera.
1631  // In sync mode the timestamp is based on ROS time
1632  bool placeholder_false(false);
1633  if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) )
1634  {
1635  _is_initialized_time_base = setBaseTime(frame_time, frame.get_frame_timestamp_domain());
1636  }
1637 
1638  ros::Time t(frameSystemTimeSec(frame));
1639  if (frame.is<rs2::frameset>())
1640  {
1641  ROS_DEBUG("Frameset arrived.");
1642  auto frameset = frame.as<rs2::frameset>();
1643  ROS_DEBUG("List of frameset before applying filters: size: %d", static_cast<int>(frameset.size()));
1644  for (auto it = frameset.begin(); it != frameset.end(); ++it)
1645  {
1646  auto f = (*it);
1647  auto stream_type = f.get_profile().stream_type();
1648  auto stream_index = f.get_profile().stream_index();
1649  auto stream_format = f.get_profile().format();
1650  auto stream_unique_id = f.get_profile().unique_id();
1651 
1652  ROS_DEBUG("Frameset contain (%s, %d, %s %d) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu",
1653  rs2_stream_to_string(stream_type), stream_index, rs2_format_to_string(stream_format), stream_unique_id, frame.get_frame_number(), frame_time, t.toNSec());
1654  runFirstFrameInitialization(stream_type);
1655  }
1656  // Clip depth_frame for max range:
1657  rs2::depth_frame original_depth_frame = frameset.get_depth_frame();
1658  bool is_color_frame(frameset.get_color_frame());
1659  if (original_depth_frame && _clipping_distance > 0)
1660  {
1661  clip_depth(original_depth_frame, _clipping_distance);
1662  }
1663 
1664  ROS_DEBUG("num_filters: %d", static_cast<int>(_filters.size()));
1665  for (std::vector<NamedFilter>::const_iterator filter_it = _filters.begin(); filter_it != _filters.end(); filter_it++)
1666  {
1667  ROS_DEBUG("Applying filter: %s", filter_it->_name.c_str());
1668  if ((filter_it->_name == "pointcloud") && (!original_depth_frame))
1669  continue;
1670  if ((filter_it->_name == "align_to_color") && (!is_color_frame))
1671  continue;
1672  frameset = filter_it->_filter->process(frameset);
1673  }
1674 
1675  ROS_DEBUG("List of frameset after applying filters: size: %d", static_cast<int>(frameset.size()));
1676  bool sent_depth_frame(false);
1677  for (auto it = frameset.begin(); it != frameset.end(); ++it)
1678  {
1679  auto f = (*it);
1680  auto stream_type = f.get_profile().stream_type();
1681  auto stream_index = f.get_profile().stream_index();
1682  auto stream_format = f.get_profile().format();
1683  stream_index_pair sip{stream_type,stream_index};
1684 
1685  ROS_DEBUG("Frameset contain (%s, %d, %s) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu",
1686  rs2_stream_to_string(stream_type), stream_index, rs2_format_to_string(stream_format), f.get_frame_number(), frame_time, t.toNSec());
1687  if (f.is<rs2::video_frame>())
1688  ROS_DEBUG_STREAM("frame: " << f.as<rs2::video_frame>().get_width() << " x " << f.as<rs2::video_frame>().get_height());
1689 
1690  if (f.is<rs2::points>())
1691  {
1692  publishPointCloud(f.as<rs2::points>(), t, frameset);
1693  continue;
1694  }
1695  if (stream_type == RS2_STREAM_DEPTH)
1696  {
1697  if (sent_depth_frame) continue;
1698  sent_depth_frame = true;
1699  if (_align_depth && is_color_frame)
1700  {
1701  publishFrame(f, t, COLOR,
1702  _depth_aligned_image,
1703  _depth_aligned_info_publisher,
1704  _depth_aligned_image_publishers, _depth_aligned_seq,
1705  _depth_aligned_camera_info,
1706  _depth_aligned_encoding);
1707  continue;
1708  }
1709  }
1710  publishFrame(f, t,
1711  sip,
1712  _image,
1713  _info_publisher,
1714  _image_publishers, _seq,
1715  _camera_info,
1716  _encoding);
1717  }
1718  if (original_depth_frame && _align_depth)
1719  {
1720  rs2::frame frame_to_send;
1721  if (_colorizer)
1722  frame_to_send = _colorizer->process(original_depth_frame);
1723  else
1724  frame_to_send = original_depth_frame;
1725 
1726  publishFrame(frame_to_send, t,
1727  DEPTH,
1728  _image,
1729  _info_publisher,
1730  _image_publishers, _seq,
1731  _camera_info,
1732  _encoding);
1733  }
1734  }
1735  else if (frame.is<rs2::video_frame>())
1736  {
1737  auto stream_type = frame.get_profile().stream_type();
1738  auto stream_index = frame.get_profile().stream_index();
1739  ROS_DEBUG("Single video frame arrived (%s, %d). frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu",
1740  rs2_stream_to_string(stream_type), stream_index, frame.get_frame_number(), frame_time, t.toNSec());
1741  runFirstFrameInitialization(stream_type);
1742 
1743  stream_index_pair sip{stream_type,stream_index};
1744  if (frame.is<rs2::depth_frame>())
1745  {
1746  if (_clipping_distance > 0)
1747  {
1748  clip_depth(frame, _clipping_distance);
1749  }
1750  }
1751  publishFrame(frame, t,
1752  sip,
1753  _image,
1754  _info_publisher,
1755  _image_publishers, _seq,
1756  _camera_info,
1757  _encoding);
1758  }
1759  }
1760  catch(const std::exception& ex)
1761  {
1762  ROS_ERROR_STREAM("An error has occurred during frame callback: " << ex.what());
1763  }
1764  _synced_imu_publisher->Resume();
1765 }; // frame_callback
1766 
1767 void BaseRealSenseNode::multiple_message_callback(rs2::frame frame, imu_sync_method sync_method)
1768 {
1769  auto stream = frame.get_profile().stream_type();
1770  switch (stream)
1771  {
1772  case RS2_STREAM_GYRO:
1773  case RS2_STREAM_ACCEL:
1774  if (sync_method > imu_sync_method::NONE) imu_callback_sync(frame, sync_method);
1775  else imu_callback(frame);
1776  break;
1777  case RS2_STREAM_POSE:
1778  pose_callback(frame);
1779  break;
1780  default:
1781  frame_callback(frame);
1782  }
1783 }
1784 
1785 bool BaseRealSenseNode::setBaseTime(double frame_time, rs2_timestamp_domain time_domain)
1786 {
1787  ROS_WARN_ONCE(time_domain == RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME ? "Frame metadata isn't available! (frame_timestamp_domain = RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME)" : "");
1788  if (time_domain == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK)
1789  {
1790  ROS_WARN("frame's time domain is HARDWARE_CLOCK. Timestamps may reset periodically.");
1791  _ros_time_base = ros::Time::now();
1792  _camera_time_base = frame_time;
1793  return true;
1794  }
1795  return false;
1796 }
1797 
1798 double BaseRealSenseNode::frameSystemTimeSec(rs2::frame frame)
1799 {
1800  if (frame.get_frame_timestamp_domain() == RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK)
1801  {
1802  double elapsed_camera_ms = (/*ms*/ frame.get_timestamp() - /*ms*/ _camera_time_base) / 1000.0;
1803  return (_ros_time_base.toSec() + elapsed_camera_ms);
1804  }
1805  else
1806  {
1807  return (frame.get_timestamp() / 1000.0);
1808  }
1809 }
1810 
1811 void BaseRealSenseNode::setupStreams()
1812 {
1813  ROS_INFO("setupStreams...");
1814  try{
1815  // Publish image stream info
1816  for (auto& profiles : _enabled_profiles)
1817  {
1818  for (auto& profile : profiles.second)
1819  {
1820  if (profile.is<rs2::video_stream_profile>())
1821  {
1822  auto video_profile = profile.as<rs2::video_stream_profile>();
1823  updateStreamCalibData(video_profile);
1824  }
1825  }
1826  }
1827 
1828  // Streaming IMAGES
1829  std::map<std::string, std::vector<rs2::stream_profile> > profiles;
1830  std::map<std::string, rs2::sensor> active_sensors;
1831  for (const std::pair<stream_index_pair, std::vector<rs2::stream_profile>>& profile : _enabled_profiles)
1832  {
1833  std::string module_name = _sensors[profile.first].get_info(RS2_CAMERA_INFO_NAME);
1834  ROS_INFO_STREAM("insert " << rs2_stream_to_string(profile.second.begin()->stream_type())
1835  << " to " << module_name);
1836  profiles[module_name].insert(profiles[module_name].begin(),
1837  profile.second.begin(),
1838  profile.second.end());
1839  active_sensors[module_name] = _sensors[profile.first];
1840  }
1841 
1842  for (const std::pair<std::string, std::vector<rs2::stream_profile> >& sensor_profile : profiles)
1843  {
1844  std::string module_name = sensor_profile.first;
1845  rs2::sensor sensor = active_sensors[module_name];
1846  sensor.open(sensor_profile.second);
1847  sensor.start(_sensors_callback[module_name]);
1848  if (sensor.is<rs2::depth_sensor>())
1849  {
1850  _depth_scale_meters = sensor.as<rs2::depth_sensor>().get_depth_scale();
1851  }
1852  }
1853  }
1854  catch(const std::exception& ex)
1855  {
1856  ROS_ERROR_STREAM("An exception has been thrown: " << ex.what());
1857  throw;
1858  }
1859  catch(...)
1860  {
1861  ROS_ERROR_STREAM("Unknown exception has occured!");
1862  throw;
1863  }
1864 }
1865 
1866 void BaseRealSenseNode::updateStreamCalibData(const rs2::video_stream_profile& video_profile)
1867 {
1868  stream_index_pair stream_index{video_profile.stream_type(), video_profile.stream_index()};
1869  auto intrinsic = video_profile.get_intrinsics();
1870  _stream_intrinsics[stream_index] = intrinsic;
1871  _camera_info[stream_index].width = intrinsic.width;
1872  _camera_info[stream_index].height = intrinsic.height;
1873  _camera_info[stream_index].header.frame_id = _optical_frame_id[stream_index];
1874 
1875  _camera_info[stream_index].K.at(0) = intrinsic.fx;
1876  _camera_info[stream_index].K.at(2) = intrinsic.ppx;
1877  _camera_info[stream_index].K.at(4) = intrinsic.fy;
1878  _camera_info[stream_index].K.at(5) = intrinsic.ppy;
1879  _camera_info[stream_index].K.at(8) = 1;
1880 
1881  _camera_info[stream_index].P.at(0) = _camera_info[stream_index].K.at(0);
1882  _camera_info[stream_index].P.at(1) = 0;
1883  _camera_info[stream_index].P.at(2) = _camera_info[stream_index].K.at(2);
1884  _camera_info[stream_index].P.at(3) = 0;
1885  _camera_info[stream_index].P.at(4) = 0;
1886  _camera_info[stream_index].P.at(5) = _camera_info[stream_index].K.at(4);
1887  _camera_info[stream_index].P.at(6) = _camera_info[stream_index].K.at(5);
1888  _camera_info[stream_index].P.at(7) = 0;
1889  _camera_info[stream_index].P.at(8) = 0;
1890  _camera_info[stream_index].P.at(9) = 0;
1891  _camera_info[stream_index].P.at(10) = 1;
1892  _camera_info[stream_index].P.at(11) = 0;
1893 
1894  // Set Tx, Ty for right camera
1895  if (stream_index.second == 2)
1896  {
1897  stream_index_pair sip1{stream_index.first, 1};
1898  if (_enable[sip1])
1899  {
1900  const auto& ex = getAProfile(stream_index).get_extrinsics_to(getAProfile(sip1));
1901  _camera_info[stream_index].header.frame_id = _optical_frame_id[sip1];
1902  _camera_info[stream_index].P.at(3) = -intrinsic.fx * ex.translation[0] + 0.0; // Tx - avoid -0.0 values.
1903  _camera_info[stream_index].P.at(7) = -intrinsic.fy * ex.translation[1] + 0.0; // Ty - avoid -0.0 values.
1904  }
1905  }
1906 
1907  if (intrinsic.model == RS2_DISTORTION_KANNALA_BRANDT4)
1908  {
1909  _camera_info[stream_index].distortion_model = "equidistant";
1910  } else {
1911  _camera_info[stream_index].distortion_model = "plumb_bob";
1912  }
1913 
1914  // set R (rotation matrix) values to identity matrix
1915  _camera_info[stream_index].R.at(0) = 1.0;
1916  _camera_info[stream_index].R.at(1) = 0.0;
1917  _camera_info[stream_index].R.at(2) = 0.0;
1918  _camera_info[stream_index].R.at(3) = 0.0;
1919  _camera_info[stream_index].R.at(4) = 1.0;
1920  _camera_info[stream_index].R.at(5) = 0.0;
1921  _camera_info[stream_index].R.at(6) = 0.0;
1922  _camera_info[stream_index].R.at(7) = 0.0;
1923  _camera_info[stream_index].R.at(8) = 1.0;
1924 
1925  _camera_info[stream_index].D.resize(5);
1926  for (int i = 0; i < 5; i++)
1927  {
1928  _camera_info[stream_index].D.at(i) = intrinsic.coeffs[i];
1929  }
1930 
1931  if (stream_index == DEPTH && _enable[DEPTH] && _enable[COLOR])
1932  {
1933  _camera_info[stream_index].P.at(3) = 0; // Tx
1934  _camera_info[stream_index].P.at(7) = 0; // Ty
1935  }
1936 
1937  if (_align_depth)
1938  {
1939  for (auto& profiles : _enabled_profiles)
1940  {
1941  for (auto& profile : profiles.second)
1942  {
1943  auto video_profile = profile.as<rs2::video_stream_profile>();
1944  stream_index_pair stream_index{video_profile.stream_type(), video_profile.stream_index()};
1945  _depth_aligned_camera_info[stream_index] = _camera_info[stream_index];
1946  }
1947  }
1948  }
1949 }
1950 
1951 tf::Quaternion BaseRealSenseNode::rotationMatrixToQuaternion(const float rotation[9]) const
1952 {
1953  Eigen::Matrix3f m;
1954  // We need to be careful about the order, as RS2 rotation matrix is
1955  // column-major, while Eigen::Matrix3f expects row-major.
1956  m << rotation[0], rotation[3], rotation[6],
1957  rotation[1], rotation[4], rotation[7],
1958  rotation[2], rotation[5], rotation[8];
1959  Eigen::Quaternionf q(m);
1960  return tf::Quaternion(q.x(), q.y(), q.z(), q.w());
1961 }
1962 
1963 void BaseRealSenseNode::publish_static_tf(const ros::Time& t,
1964  const float3& trans,
1965  const tf::Quaternion& q,
1966  const std::string& from,
1967  const std::string& to)
1968 {
1969  geometry_msgs::TransformStamped msg;
1970  msg.header.stamp = t;
1971  msg.header.frame_id = from;
1972  msg.child_frame_id = to;
1973  msg.transform.translation.x = trans.z;
1974  msg.transform.translation.y = -trans.x;
1975  msg.transform.translation.z = -trans.y;
1976  msg.transform.rotation.x = q.getX();
1977  msg.transform.rotation.y = q.getY();
1978  msg.transform.rotation.z = q.getZ();
1979  msg.transform.rotation.w = q.getW();
1980  _static_tf_msgs.push_back(msg);
1981 }
1982 
1983 void BaseRealSenseNode::calcAndPublishStaticTransform(const stream_index_pair& stream, const rs2::stream_profile& base_profile)
1984 {
1985  // Transform base to stream
1986  tf::Quaternion quaternion_optical;
1987  quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
1988  float3 zero_trans{0, 0, 0};
1989 
1990  ros::Time transform_ts_ = ros::Time::now();
1991 
1992  rs2_extrinsics ex;
1993  try
1994  {
1995  ex = getAProfile(stream).get_extrinsics_to(base_profile);
1996  }
1997  catch (std::exception& e)
1998  {
1999  if (!strcmp(e.what(), "Requested extrinsics are not available!"))
2000  {
2001  ROS_WARN_STREAM("(" << rs2_stream_to_string(stream.first) << ", " << stream.second << ") -> (" << rs2_stream_to_string(base_profile.stream_type()) << ", " << base_profile.stream_index() << "): " << e.what() << " : using unity as default.");
2002  ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}});
2003  }
2004  else
2005  {
2006  throw e;
2007  }
2008  }
2009 
2010  auto Q = rotationMatrixToQuaternion(ex.rotation);
2011  Q = quaternion_optical * Q * quaternion_optical.inverse();
2012 
2013  float3 trans{ex.translation[0], ex.translation[1], ex.translation[2]};
2014  publish_static_tf(transform_ts_, trans, Q, _base_frame_id, _frame_id[stream]);
2015 
2016  // Transform stream frame to stream optical frame
2017  publish_static_tf(transform_ts_, zero_trans, quaternion_optical, _frame_id[stream], _optical_frame_id[stream]);
2018 
2019  if (_align_depth && _depth_aligned_frame_id.find(stream) != _depth_aligned_frame_id.end())
2020  {
2021  publish_static_tf(transform_ts_, trans, Q, _base_frame_id, _depth_aligned_frame_id[stream]);
2022  publish_static_tf(transform_ts_, zero_trans, quaternion_optical, _depth_aligned_frame_id[stream], _optical_frame_id[stream]);
2023  }
2024 }
2025 
2026 void BaseRealSenseNode::SetBaseStream()
2027 {
2028  const std::vector<stream_index_pair> base_stream_priority = {DEPTH, POSE};
2029 
2030  std::vector<stream_index_pair>::const_iterator base_stream(base_stream_priority.begin());
2031  while( (_sensors.find(*base_stream) == _sensors.end()) && (base_stream != base_stream_priority.end()))
2032  {
2033  base_stream++;
2034  }
2035  if (base_stream == base_stream_priority.end())
2036  {
2037  throw std::runtime_error("No known base_stream found for transformations.");
2038  }
2039  ROS_INFO_STREAM("SELECTED BASE:" << base_stream->first << ", " << base_stream->second);
2040 
2041  _base_stream = *base_stream;
2042 }
2043 
2044 void BaseRealSenseNode::publishStaticTransforms()
2045 {
2046  rs2::stream_profile base_profile = getAProfile(_base_stream);
2047 
2048  // Publish static transforms
2049  if (_publish_tf)
2050  {
2051  for (std::pair<stream_index_pair, bool> ienable : _enable)
2052  {
2053  if (ienable.second)
2054  {
2055  calcAndPublishStaticTransform(ienable.first, base_profile);
2056  }
2057  }
2058  // Static transform for non-positive values
2059  if (_tf_publish_rate > 0)
2060  _tf_t = std::shared_ptr<std::thread>(new std::thread(boost::bind(&BaseRealSenseNode::publishDynamicTransforms, this)));
2061  else
2062  _static_tf_broadcaster.sendTransform(_static_tf_msgs);
2063  }
2064 
2065  // Publish Extrinsics Topics:
2066  if (_enable[DEPTH] &&
2067  _enable[FISHEYE])
2068  {
2069  static const char* frame_id = "depth_to_fisheye_extrinsics";
2070  const auto& ex = base_profile.get_extrinsics_to(getAProfile(FISHEYE));
2071 
2072  _depth_to_other_extrinsics[FISHEYE] = ex;
2073  _depth_to_other_extrinsics_publishers[FISHEYE].publish(rsExtrinsicsToMsg(ex, frame_id));
2074  }
2075 
2076  if (_enable[DEPTH] &&
2077  _enable[COLOR])
2078  {
2079  static const char* frame_id = "depth_to_color_extrinsics";
2080  const auto& ex = base_profile.get_extrinsics_to(getAProfile(COLOR));
2081  _depth_to_other_extrinsics[COLOR] = ex;
2082  _depth_to_other_extrinsics_publishers[COLOR].publish(rsExtrinsicsToMsg(ex, frame_id));
2083  }
2084 
2085  if (_enable[DEPTH] &&
2086  _enable[INFRA1])
2087  {
2088  static const char* frame_id = "depth_to_infra1_extrinsics";
2089  const auto& ex = base_profile.get_extrinsics_to(getAProfile(INFRA1));
2090  _depth_to_other_extrinsics[INFRA1] = ex;
2091  _depth_to_other_extrinsics_publishers[INFRA1].publish(rsExtrinsicsToMsg(ex, frame_id));
2092  }
2093 
2094  if (_enable[DEPTH] &&
2095  _enable[INFRA2])
2096  {
2097  static const char* frame_id = "depth_to_infra2_extrinsics";
2098  const auto& ex = base_profile.get_extrinsics_to(getAProfile(INFRA2));
2099  _depth_to_other_extrinsics[INFRA2] = ex;
2100  _depth_to_other_extrinsics_publishers[INFRA2].publish(rsExtrinsicsToMsg(ex, frame_id));
2101  }
2102 
2103 }
2104 
2105 void BaseRealSenseNode::publishDynamicTransforms()
2106 {
2107  // Publish transforms for the cameras
2108  ROS_WARN("Publishing dynamic camera transforms (/tf) at %g Hz", _tf_publish_rate);
2109 
2110  std::mutex mu;
2111  std::unique_lock<std::mutex> lock(mu);
2112  while (ros::ok() && _is_running)
2113  {
2114  _cv_tf.wait_for(lock, std::chrono::milliseconds((int)(1000.0/_tf_publish_rate)), [&]{return (!(_is_running));});
2115  {
2116  ros::Time t = ros::Time::now();
2117  for(auto& msg : _static_tf_msgs)
2118  msg.header.stamp = t;
2119  _dynamic_tf_broadcaster.sendTransform(_static_tf_msgs);
2120  }
2121  }
2122 }
2123 
2124 void BaseRealSenseNode::publishIntrinsics()
2125 {
2126  if (_enable[GYRO])
2127  {
2128  _info_publisher[GYRO] = _node_handle.advertise<IMUInfo>("gyro/imu_info", 1, true);
2129  IMUInfo info_msg = getImuInfo(GYRO);
2130  _info_publisher[GYRO].publish(info_msg);
2131  }
2132 
2133  if (_enable[ACCEL])
2134  {
2135  _info_publisher[ACCEL] = _node_handle.advertise<IMUInfo>("accel/imu_info", 1, true);
2136  IMUInfo info_msg = getImuInfo(ACCEL);
2137  _info_publisher[ACCEL].publish(info_msg);
2138  }
2139 }
2140 
2141 void reverse_memcpy(unsigned char* dst, const unsigned char* src, size_t n)
2142 {
2143  size_t i;
2144 
2145  for (i=0; i < n; ++i)
2146  dst[n-1-i] = src[i];
2147 
2148 }
2149 
2150 void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, const rs2::frameset& frameset)
2151 {
2152  if (0 == _pointcloud_publisher.getNumSubscribers())
2153  return;
2154  ROS_INFO_STREAM_ONCE("publishing " << (_ordered_pc ? "" : "un") << "ordered pointcloud.");
2155 
2156  rs2_stream texture_source_id = static_cast<rs2_stream>(_pointcloud_filter->get_option(rs2_option::RS2_OPTION_STREAM_FILTER));
2157  bool use_texture = texture_source_id != RS2_STREAM_ANY;
2158  static int warn_count(0);
2159  static const int DISPLAY_WARN_NUMBER(5);
2160  rs2::frameset::iterator texture_frame_itr = frameset.end();
2161  if (use_texture)
2162  {
2163  std::set<rs2_format> available_formats{ rs2_format::RS2_FORMAT_RGB8, rs2_format::RS2_FORMAT_Y8 };
2164 
2165  texture_frame_itr = find_if(frameset.begin(), frameset.end(), [&texture_source_id, &available_formats] (rs2::frame f)
2166  {return (rs2_stream(f.get_profile().stream_type()) == texture_source_id) &&
2167  (available_formats.find(f.get_profile().format()) != available_formats.end()); });
2168  if (texture_frame_itr == frameset.end())
2169  {
2170  warn_count++;
2171  std::string texture_source_name = _pointcloud_filter->get_option_value_description(rs2_option::RS2_OPTION_STREAM_FILTER, static_cast<float>(texture_source_id));
2172  ROS_WARN_STREAM_COND(warn_count == DISPLAY_WARN_NUMBER, "No stream match for pointcloud chosen texture " << texture_source_name);
2173  return;
2174  }
2175  warn_count = 0;
2176  }
2177 
2178  int texture_width(0), texture_height(0);
2179  int num_colors(0);
2180 
2181  const rs2::vertex* vertex = pc.get_vertices();
2182  const rs2::texture_coordinate* color_point = pc.get_texture_coordinates();
2183 
2184  rs2_intrinsics depth_intrin = pc.get_profile().as<rs2::video_stream_profile>().get_intrinsics();
2185 
2186  sensor_msgs::PointCloud2Modifier modifier(_msg_pointcloud);
2187  modifier.setPointCloud2FieldsByString(1, "xyz");
2188  modifier.resize(pc.size());
2189  if (_ordered_pc)
2190  {
2191  _msg_pointcloud.width = depth_intrin.width;
2192  _msg_pointcloud.height = depth_intrin.height;
2193  _msg_pointcloud.is_dense = false;
2194  }
2195 
2196  vertex = pc.get_vertices();
2197  size_t valid_count(0);
2198  if (use_texture)
2199  {
2200  rs2::video_frame texture_frame = (*texture_frame_itr).as<rs2::video_frame>();
2201  texture_width = texture_frame.get_width();
2202  texture_height = texture_frame.get_height();
2203  num_colors = texture_frame.get_bytes_per_pixel();
2204  uint8_t* color_data = (uint8_t*)texture_frame.get_data();
2205  std::string format_str;
2206  switch(texture_frame.get_profile().format())
2207  {
2208  case RS2_FORMAT_RGB8:
2209  format_str = "rgb";
2210  break;
2211  case RS2_FORMAT_Y8:
2212  format_str = "intensity";
2213  break;
2214  default:
2215  throw std::runtime_error("Unhandled texture format passed in pointcloud " + std::to_string(texture_frame.get_profile().format()));
2216  }
2217  _msg_pointcloud.point_step = addPointField(_msg_pointcloud, format_str.c_str(), 1, sensor_msgs::PointField::FLOAT32, _msg_pointcloud.point_step);
2218  _msg_pointcloud.row_step = _msg_pointcloud.width * _msg_pointcloud.point_step;
2219  _msg_pointcloud.data.resize(_msg_pointcloud.height * _msg_pointcloud.row_step);
2220 
2221  sensor_msgs::PointCloud2Iterator<float>iter_x(_msg_pointcloud, "x");
2222  sensor_msgs::PointCloud2Iterator<float>iter_y(_msg_pointcloud, "y");
2223  sensor_msgs::PointCloud2Iterator<float>iter_z(_msg_pointcloud, "z");
2224  sensor_msgs::PointCloud2Iterator<uint8_t>iter_color(_msg_pointcloud, format_str);
2225  color_point = pc.get_texture_coordinates();
2226 
2227  float color_pixel[2];
2228  for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++, color_point++)
2229  {
2230  float i(color_point->u);
2231  float j(color_point->v);
2232  bool valid_color_pixel(i >= 0.f && i <=1.f && j >= 0.f && j <=1.f);
2233  bool valid_pixel(vertex->z > 0 && (valid_color_pixel || _allow_no_texture_points));
2234  if (valid_pixel || _ordered_pc)
2235  {
2236  *iter_x = vertex->x;
2237  *iter_y = vertex->y;
2238  *iter_z = vertex->z;
2239 
2240  if (valid_color_pixel)
2241  {
2242  color_pixel[0] = i * texture_width;
2243  color_pixel[1] = j * texture_height;
2244  int pixx = static_cast<int>(color_pixel[0]);
2245  int pixy = static_cast<int>(color_pixel[1]);
2246  int offset = (pixy * texture_width + pixx) * num_colors;
2247  reverse_memcpy(&(*iter_color), color_data+offset, num_colors); // PointCloud2 order of rgb is bgr.
2248  }
2249  ++iter_x; ++iter_y; ++iter_z;
2250  ++iter_color;
2251  ++valid_count;
2252  }
2253  }
2254  }
2255  else
2256  {
2257  std::string format_str = "intensity";
2258  _msg_pointcloud.row_step = _msg_pointcloud.width * _msg_pointcloud.point_step;
2259  _msg_pointcloud.data.resize(_msg_pointcloud.height * _msg_pointcloud.row_step);
2260 
2261  sensor_msgs::PointCloud2Iterator<float>iter_x(_msg_pointcloud, "x");
2262  sensor_msgs::PointCloud2Iterator<float>iter_y(_msg_pointcloud, "y");
2263  sensor_msgs::PointCloud2Iterator<float>iter_z(_msg_pointcloud, "z");
2264 
2265  for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++)
2266  {
2267  bool valid_pixel(vertex->z > 0);
2268  if (valid_pixel || _ordered_pc)
2269  {
2270  *iter_x = vertex->x;
2271  *iter_y = vertex->y;
2272  *iter_z = vertex->z;
2273 
2274  ++iter_x; ++iter_y; ++iter_z;
2275  ++valid_count;
2276  }
2277  }
2278  }
2279  _msg_pointcloud.header.stamp = t;
2280  if (_align_depth) _msg_pointcloud.header.frame_id = _optical_frame_id[COLOR];
2281  else _msg_pointcloud.header.frame_id = _optical_frame_id[DEPTH];
2282  if (!_ordered_pc)
2283  {
2284  _msg_pointcloud.width = valid_count;
2285  _msg_pointcloud.height = 1;
2286  _msg_pointcloud.is_dense = true;
2287  modifier.resize(valid_count);
2288  }
2289  _pointcloud_publisher.publish(_msg_pointcloud);
2290 }
2291 
2292 
2293 Extrinsics BaseRealSenseNode::rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics, const std::string& frame_id) const
2294 {
2295  Extrinsics extrinsicsMsg;
2296  for (int i = 0; i < 9; ++i)
2297  {
2298  extrinsicsMsg.rotation[i] = extrinsics.rotation[i];
2299  if (i < 3)
2300  extrinsicsMsg.translation[i] = extrinsics.translation[i];
2301  }
2302 
2303  extrinsicsMsg.header.frame_id = frame_id;
2304  return extrinsicsMsg;
2305 }
2306 
2307 rs2::stream_profile BaseRealSenseNode::getAProfile(const stream_index_pair& stream)
2308 {
2309  const std::vector<rs2::stream_profile> profiles = _sensors[stream].get_stream_profiles();
2310  return *(std::find_if(profiles.begin(), profiles.end(),
2311  [&stream] (const rs2::stream_profile& profile) {
2312  return ((profile.stream_type() == stream.first) && (profile.stream_index() == stream.second));
2313  }));
2314 }
2315 
2316 IMUInfo BaseRealSenseNode::getImuInfo(const stream_index_pair& stream_index)
2317 {
2318  IMUInfo info{};
2319  auto sp = _enabled_profiles[stream_index].front().as<rs2::motion_stream_profile>();
2320  rs2_motion_device_intrinsic imuIntrinsics;
2321  try
2322  {
2323  imuIntrinsics = sp.get_motion_intrinsics();
2324  }
2325  catch(const std::runtime_error &ex)
2326  {
2327  ROS_DEBUG_STREAM("No Motion Intrinsics available.");
2328  imuIntrinsics = {{{1,0,0,0},{0,1,0,0},{0,0,1,0}}, {0,0,0}, {0,0,0}};
2329  }
2330 
2331  auto index = 0;
2332  info.frame_id = _optical_frame_id[stream_index];
2333  for (int i = 0; i < 3; ++i)
2334  {
2335  for (int j = 0; j < 4; ++j)
2336  {
2337  info.data[index] = imuIntrinsics.data[i][j];
2338  ++index;
2339  }
2340  info.noise_variances[i] = imuIntrinsics.noise_variances[i];
2341  info.bias_variances[i] = imuIntrinsics.bias_variances[i];
2342  }
2343  return info;
2344 }
2345 
2346 void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t,
2347  const stream_index_pair& stream,
2348  std::map<stream_index_pair, cv::Mat>& images,
2349  const std::map<stream_index_pair, ros::Publisher>& info_publishers,
2350  const std::map<stream_index_pair, ImagePublisherWithFrequencyDiagnostics>& image_publishers,
2351  std::map<stream_index_pair, int>& seq,
2352  std::map<stream_index_pair, sensor_msgs::CameraInfo>& camera_info,
2353  const std::map<rs2_stream, std::string>& encoding,
2354  bool copy_data_from_frame)
2355 {
2356  ROS_DEBUG("publishFrame(...)");
2357  unsigned int width = 0;
2358  unsigned int height = 0;
2359  auto bpp = 1;
2360  if (f.is<rs2::video_frame>())
2361  {
2362  auto image = f.as<rs2::video_frame>();
2363  width = image.get_width();
2364  height = image.get_height();
2365  bpp = image.get_bytes_per_pixel();
2366  }
2367  auto& image = images[stream];
2368 
2369  if (copy_data_from_frame)
2370  {
2371  if (images[stream].size() != cv::Size(width, height))
2372  {
2373  image.create(height, width, image.type());
2374  }
2375  image.data = (uint8_t*)f.get_data();
2376  }
2377  if (f.is<rs2::depth_frame>())
2378  {
2379  image = fix_depth_scale(image, _depth_scaled_image[stream]);
2380  }
2381 
2382  ++(seq[stream]);
2383  auto& info_publisher = info_publishers.at(stream);
2384  auto& image_publisher = image_publishers.at(stream);
2385 
2386  image_publisher.second->tick();
2387  if(0 != info_publisher.getNumSubscribers() ||
2388  0 != image_publisher.first.getNumSubscribers())
2389  {
2390  auto& cam_info = camera_info.at(stream);
2391  if (cam_info.width != width)
2392  {
2393  updateStreamCalibData(f.get_profile().as<rs2::video_stream_profile>());
2394  }
2395  cam_info.header.stamp = t;
2396  cam_info.header.seq = seq[stream];
2397  info_publisher.publish(cam_info);
2398 
2399  sensor_msgs::ImagePtr img;
2400  img = cv_bridge::CvImage(std_msgs::Header(), encoding.at(stream.first), image).toImageMsg();
2401  img->width = width;
2402  img->height = height;
2403  img->is_bigendian = false;
2404  img->step = width * bpp;
2405  img->header.frame_id = cam_info.header.frame_id;
2406  img->header.stamp = t;
2407  img->header.seq = seq[stream];
2408 
2409  image_publisher.first.publish(img);
2410  // ROS_INFO_STREAM("fid: " << cam_info.header.seq << ", time: " << std::setprecision (20) << t.toSec());
2411  ROS_DEBUG("%s stream published", rs2_stream_to_string(f.get_profile().stream_type()));
2412  }
2413 }
2414 
2415 bool BaseRealSenseNode::getEnabledProfile(const stream_index_pair& stream_index, rs2::stream_profile& profile)
2416  {
2417  // Assuming that all D400 SKUs have depth sensor
2418  auto profiles = _enabled_profiles[stream_index];
2419  auto it = std::find_if(profiles.begin(), profiles.end(),
2420  [&](const rs2::stream_profile& profile)
2421  { return (profile.stream_type() == stream_index.first); });
2422  if (it == profiles.end())
2423  return false;
2424 
2425  profile = *it;
2426  return true;
2427  }
2428 
2429 void BaseRealSenseNode::startMonitoring()
2430 {
2431  for (rs2_option option : _monitor_options)
2432  {
2433  _temperature_nodes.push_back({option, std::make_shared<TemperatureDiagnostics>(rs2_option_to_string(option), _serial_no )});
2434  }
2435 
2436  int time_interval(1000);
2437  std::function<void()> func = [this, time_interval](){
2438  std::mutex mu;
2439  std::unique_lock<std::mutex> lock(mu);
2440  while(_is_running) {
2441  _cv_monitoring.wait_for(lock, std::chrono::milliseconds(time_interval), [&]{return !_is_running;});
2442  if (_is_running)
2443  {
2444  publish_temperature();
2445  publish_frequency_update();
2446  }
2447  }
2448  };
2449  _monitoring_t = std::make_shared<std::thread>(func);
2450 }
2451 
2452 void BaseRealSenseNode::publish_temperature()
2453 {
2454  rs2::options sensor(_sensors[_base_stream]);
2455  for (OptionTemperatureDiag option_diag : _temperature_nodes)
2456  {
2457  rs2_option option(option_diag.first);
2458  if (sensor.supports(option))
2459  {
2460  try
2461  {
2462  auto option_value = sensor.get_option(option);
2463  option_diag.second->update(option_value);
2464  }
2465  catch(const std::exception& e)
2466  {
2467  ROS_DEBUG_STREAM("Failed checking for temperature." << std::endl << e.what());
2468  }
2469  }
2470  }
2471 }
2472 
2473 void BaseRealSenseNode::publish_frequency_update()
2474 {
2475  for (auto &image_publisher : _image_publishers)
2476  {
2477  image_publisher.second.second->update();
2478  }
2479 }
2480 
2481 TemperatureDiagnostics::TemperatureDiagnostics(std::string name, std::string serial_no)
2482  {
2483  _updater.add(name, this, &TemperatureDiagnostics::diagnostics);
2484  _updater.setHardwareID(serial_no);
2485  }
2486 
2487 void TemperatureDiagnostics::diagnostics(diagnostic_updater::DiagnosticStatusWrapper& status)
2488 {
2489  status.summary(0, "OK");
2490  status.add("Index", _crnt_temp);
2491 }
realsense2_camera::BaseRealSenseNode::_depth_aligned_image
std::map< stream_index_pair, cv::Mat > _depth_aligned_image
Definition: base_realsense_node.h:309
realsense2_camera::BaseRealSenseNode::FillImuData_Copy
void FillImuData_Copy(const CimuData imu_data, std::deque< sensor_msgs::Imu > &imu_msgs)
Definition: base_realsense_node.cpp:1408
realsense2_camera::BaseRealSenseNode::_seq
std::map< stream_index_pair, int > _seq
Definition: base_realsense_node.h:289
RS2_OPTION_COUNT
RS2_OPTION_COUNT
msg
msg
rs2::motion_stream_profile
RS2_LOG_SEVERITY_ERROR
RS2_LOG_SEVERITY_ERROR
rs2::stream_profile
RS2_OPTION_PROJECTOR_TEMPERATURE
RS2_OPTION_PROJECTOR_TEMPERATURE
realsense2_camera::BaseRealSenseNode::_image_format
std::map< rs2_stream, int > _image_format
Definition: base_realsense_node.h:284
rs2::video_frame
realsense2_camera::BaseRealSenseNode::BaseRealSenseNode
BaseRealSenseNode(ros::NodeHandle &nodeHandle, ros::NodeHandle &privateNodeHandle, rs2::device dev, const std::string &serial_no)
Definition: base_realsense_node.cpp:83
realsense2_camera::NamedFilter
Definition: base_realsense_node.h:73
realsense2_camera::BaseRealSenseNode::_dev
rs2::device _dev
Definition: base_realsense_node.h:250
realsense2_camera::BaseRealSenseNode::monitor_update_functions
void monitor_update_functions()
Definition: base_realsense_node.cpp:683
realsense2_camera::BaseRealSenseNode::CimuData::m_data
Eigen::Vector3d m_data
Definition: base_realsense_node.h:189
realsense2_camera::IMAGE_STREAMS
const std::vector< stream_index_pair > IMAGE_STREAMS
Definition: realsense_node_factory.h:41
b
GLboolean GLboolean GLboolean b
realsense2_camera::DEFAULT_BASE_FRAME_ID
const std::string DEFAULT_BASE_FRAME_ID
Definition: constants.h:70
realsense2_camera::BaseRealSenseNode::CimuData::is_set
bool is_set()
Definition: base_realsense_node.h:186
rs2::region_of_interest
y
GLint y
realsense2_camera::BaseRealSenseNode::clip_depth
void clip_depth(rs2::depth_frame depth_frame, float clipping_dist)
Definition: base_realsense_node.cpp:1317
realsense2_camera::BaseRealSenseNode::fix_depth_scale
cv::Mat & fix_depth_scale(const cv::Mat &from_image, cv::Mat &to_image)
Definition: base_realsense_node.cpp:1277
realsense2_camera::BaseRealSenseNode::registerAutoExposureROIOptions
void registerAutoExposureROIOptions(ros::NodeHandle &nh)
Definition: base_realsense_node.cpp:413
realsense2_camera::TemperatureDiagnostics::TemperatureDiagnostics
TemperatureDiagnostics(std::string name, std::string serial_no)
Definition: base_realsense_node.cpp:2481
sensor_msgs::PointCloud2_::data
_data_type data
realsense2_camera::BaseRealSenseNode::_static_tf_broadcaster
tf2_ros::StaticTransformBroadcaster _static_tf_broadcaster
Definition: base_realsense_node.h:276
RS2_CAMERA_INFO_PHYSICAL_PORT
RS2_CAMERA_INFO_PHYSICAL_PORT
rs2_quaternion::y
float y
ros::NodeHandle
type
U1 type
rs2::device::query_sensors
std::vector< sensor > query_sensors() const
realsense2_camera::ALIGN_DEPTH
const bool ALIGN_DEPTH
Definition: constants.h:44
sensor_msgs::image_encodings::RGB8
const std::string RGB8
rs2::stream_profile::as
T as() const
rs2_option
rs2_option
realsense2_camera::BaseRealSenseNode::rotationMatrixToQuaternion
tf::Quaternion rotationMatrixToQuaternion(const float rotation[9]) const
Definition: base_realsense_node.cpp:1951
sensor_msgs::Imu_::linear_acceleration
_linear_acceleration_type linear_acceleration
realsense2_camera::DEFAULT_ODOM_FRAME_ID
const std::string DEFAULT_ODOM_FRAME_ID
Definition: constants.h:71
rs2_vector::z
float z
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
realsense2_camera::BaseRealSenseNode::ImuMessage_AddDefaultValues
void ImuMessage_AddDefaultValues(sensor_msgs::Imu &imu_msg)
Definition: base_realsense_node.cpp:1424
rs2_pose::tracker_confidence
unsigned int tracker_confidence
s
GLdouble s
realsense2_camera::BaseRealSenseNode::_filters
std::vector< NamedFilter > _filters
Definition: base_realsense_node.h:305
realsense2_camera::DEFAULT_IMU_OPTICAL_FRAME_ID
const std::string DEFAULT_IMU_OPTICAL_FRAME_ID
Definition: constants.h:86
RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK
RS2_TIMESTAMP_DOMAIN_HARDWARE_CLOCK
realsense2_camera::SyncedImuPublisher::_publisher
ros::Publisher _publisher
Definition: base_realsense_node.h:111
info
def info(args)
realsense2_camera::SyncedImuPublisher::Pause
void Pause()
Definition: base_realsense_node.cpp:50
rs2::depth_sensor
realsense2_camera::BaseRealSenseNode::publishPointCloud
void publishPointCloud(rs2::points f, const ros::Time &t, const rs2::frameset &frameset)
Definition: base_realsense_node.cpp:2150
m
const GLfloat * m
realsense2_camera::BaseRealSenseNode::_info_publisher
std::map< stream_index_pair, ros::Publisher > _info_publisher
Definition: base_realsense_node.h:285
rs2::region_of_interest::min_x
int min_x
RS2_OPTION_SEQUENCE_SIZE
RS2_OPTION_SEQUENCE_SIZE
realsense2_camera::BaseRealSenseNode::pose_callback
void pose_callback(rs2::frame frame)
Definition: base_realsense_node.cpp:1530
rs2_extrinsics::translation
float translation[3]
realsense2_camera
Definition: base_realsense_node.h:25
realsense2_camera::BaseRealSenseNode::_dynamic_tf_broadcaster
tf2_ros::TransformBroadcaster _dynamic_tf_broadcaster
Definition: base_realsense_node.h:277
diagnostic_updater::DiagnosticStatusWrapper::summary
void summary(unsigned char lvl, const std::string s)
realsense2_camera::BaseRealSenseNode::_stream_intrinsics
std::map< stream_index_pair, rs2_intrinsics > _stream_intrinsics
Definition: base_realsense_node.h:267
rs2::fisheye_sensor
realsense2_camera::PUBLISH_ODOM_TF
const bool PUBLISH_ODOM_TF
Definition: constants.h:67
RS2_OPTION_SEQUENCE_ID
RS2_OPTION_SEQUENCE_ID
RS2_CAMERA_INFO_PRODUCT_ID
RS2_CAMERA_INFO_PRODUCT_ID
dt
double dt
RS2_OPTION_HDR_ENABLED
RS2_OPTION_HDR_ENABLED
reverse_memcpy
void reverse_memcpy(unsigned char *dst, const unsigned char *src, size_t n)
Definition: base_realsense_node.cpp:2141
rs2::stream_profile::get_extrinsics_to
rs2_extrinsics get_extrinsics_to(const stream_profile &to) const
geometry_msgs::Vector3_::y
_y_type y
realsense2_camera::BaseRealSenseNode::_cv_tf
std::condition_variable _cv_tf
Definition: base_realsense_node.h:326
geometry_msgs::Quaternion_::x
_x_type x
ros::NodeHandle::deleteParam
bool deleteParam(const std::string &key) const
realsense2_camera::BaseRealSenseNode::_ros_time_base
ros::Time _ros_time_base
Definition: base_realsense_node.h:297
ros::NodeHandle::resolveName
std::string resolveName(const std::string &name, bool remap=true) const
realsense2_camera::BaseRealSenseNode::frameSystemTimeSec
double frameSystemTimeSec(rs2::frame frame)
Definition: base_realsense_node.cpp:1798
rs2::frame::get_profile
stream_profile get_profile() const
rs2::notification::get_severity
rs2_log_severity get_severity() const
ros::service::call
bool call(const std::string &service_name, MReq &req, MRes &res)
RS2_STREAM_FISHEYE
RS2_STREAM_FISHEYE
RS2_FORMAT_Z16
RS2_FORMAT_Z16
realsense2_camera::BaseRealSenseNode::set_sensor_parameter_to_ros
void set_sensor_parameter_to_ros(const std::string &module_name, rs2::options sensor, rs2_option option)
Definition: base_realsense_node.cpp:662
rs2_pose::translation
rs2_vector translation
realsense2_camera::BaseRealSenseNode::_angular_velocity_cov
double _angular_velocity_cov
Definition: base_realsense_node.h:264
geometry_msgs::Point_::x
_x_type x
realsense2_camera::DEFAULT_UNITE_IMU_METHOD
const std::string DEFAULT_UNITE_IMU_METHOD
Definition: constants.h:93
OPTICAL_FRAME_ID
#define OPTICAL_FRAME_ID(sip)
Definition: base_realsense_node.cpp:18
realsense2_camera::SyncedImuPublisher::_waiting_list_size
std::size_t _waiting_list_size
Definition: base_realsense_node.h:114
geometry_msgs::Pose_::orientation
_orientation_type orientation
rs2::video_frame::get_bytes_per_pixel
int get_bytes_per_pixel() const
realsense2_camera::BaseRealSenseNode::registerDynamicReconfigCb
virtual void registerDynamicReconfigCb(ros::NodeHandle &nh) override
Definition: base_realsense_node.cpp:598
RS2_STREAM_POSE
RS2_STREAM_POSE
rs2::option_range::min
float min
realsense2_camera::BaseRealSenseNode::setupDevice
void setupDevice()
Definition: base_realsense_node.cpp:819
realsense2_camera::BaseRealSenseNode::_base_frame_id
std::string _base_frame_id
Definition: base_realsense_node.h:158
sensor_msgs::CameraInfo_
realsense2_camera::BaseRealSenseNode::_publish_tf
bool _publish_tf
Definition: base_realsense_node.h:274
image_transport::ImageTransport::advertise
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
dst
GLenum GLenum dst
RS2_DISTORTION_KANNALA_BRANDT4
RS2_DISTORTION_KANNALA_BRANDT4
RS2_OPTION_ASIC_TEMPERATURE
RS2_OPTION_ASIC_TEMPERATURE
realsense2_camera::BaseRealSenseNode::float3::y
float y
Definition: base_realsense_node.h:138
realsense2_camera::BaseRealSenseNode::_pointcloud_publisher
ros::Publisher _pointcloud_publisher
Definition: base_realsense_node.h:296
rs2::frame::get_data
const void * get_data() const
sensor_msgs::Imu_::angular_velocity_covariance
_angular_velocity_covariance_type angular_velocity_covariance
vertex
struct Vertex vertex[VERTEXNUM]
realsense2_camera::BaseRealSenseNode::publishTopics
virtual void publishTopics() override
Definition: base_realsense_node.cpp:240
realsense2_camera::BaseRealSenseNode::_pointcloud_filter
std::shared_ptr< rs2::filter > _pointcloud_filter
Definition: base_realsense_node.h:306
uint16_t
unsigned short uint16_t
realsense2_camera::BaseRealSenseNode::rsExtrinsicsToMsg
Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics &extrinsics, const std::string &frame_id) const
Definition: base_realsense_node.cpp:2293
image_transport
realsense2_camera::INFRA2
const stream_index_pair INFRA2
Definition: realsense_node_factory.h:32
RS2_CAMERA_INFO_NAME
RS2_CAMERA_INFO_NAME
rs2::notification::get_description
std::string get_description() const
geometry_msgs::Vector3Stamped_::vector
_vector_type vector
tf::vector3TFToMsg
static void vector3TFToMsg(const Vector3 &bt_v, geometry_msgs::Vector3 &msg_v)
realsense2_camera::BaseRealSenseNode::toggleSensors
virtual void toggleSensors(bool enabled) override
Definition: base_realsense_node.cpp:173
sensor_msgs::Imu_::orientation
_orientation_type orientation
realsense2_camera::BaseRealSenseNode::_update_functions_t
std::shared_ptr< std::thread > _update_functions_t
Definition: base_realsense_node.h:279
sensor_msgs::PointCloud2_::row_step
_row_step_type row_step
rs2_quaternion::x
float x
stream
GLuint GLuint stream
rs2::region_of_interest::max_y
int max_y
src
GLenum src
rs2::texture_coordinate::u
float u
realsense2_camera::BaseRealSenseNode::getImuInfo
IMUInfo getImuInfo(const stream_index_pair &stream_index)
Definition: base_realsense_node.cpp:2316
RS2_STREAM_GYRO
RS2_STREAM_GYRO
realsense2_camera::BaseRealSenseNode::_pointcloud_texture
stream_index_pair _pointcloud_texture
Definition: base_realsense_node.h:303
RS2_OPTION_DEPTH_UNITS
RS2_OPTION_DEPTH_UNITS
realsense2_camera::BaseRealSenseNode::_is_first_frame
std::map< rs2_stream, bool > _is_first_frame
Definition: base_realsense_node.h:319
n
GLdouble n
uint8_t
unsigned char uint8_t
rs2_stream_to_string
const char * rs2_stream_to_string(rs2_stream stream)
realsense2_camera::BaseRealSenseNode::publishFrame
void publishFrame(rs2::frame f, const ros::Time &t, const stream_index_pair &stream, std::map< stream_index_pair, cv::Mat > &images, const std::map< stream_index_pair, ros::Publisher > &info_publishers, const std::map< stream_index_pair, ImagePublisherWithFrequencyDiagnostics > &image_publishers, std::map< stream_index_pair, int > &seq, std::map< stream_index_pair, sensor_msgs::CameraInfo > &camera_info, const std::map< rs2_stream, std::string > &encoding, bool copy_data_from_frame=true)
Definition: base_realsense_node.cpp:2346
rs2_pose::angular_velocity
rs2_vector angular_velocity
realsense2_camera::BaseRealSenseNode::CimuData
Definition: base_realsense_node.h:178
realsense2_camera::BaseRealSenseNode::_image_publishers
std::map< stream_index_pair, ImagePublisherWithFrequencyDiagnostics > _image_publishers
Definition: base_realsense_node.h:281
tf::Quaternion::getW
TFSIMD_FORCE_INLINE const tfScalar & getW() const
e
e
realsense2_camera::BaseRealSenseNode::_is_initialized_time_base
std::atomic_bool _is_initialized_time_base
Definition: base_realsense_node.h:292
rs2::points::get_texture_coordinates
const texture_coordinate * get_texture_coordinates() const
rs2::region_of_interest::max_x
int max_x
realsense2_camera::SyncedImuPublisher::_pause_mode
bool _pause_mode
Definition: base_realsense_node.h:112
rs2::motion_sensor
rs2_extrinsics::rotation
float rotation[9]
ROS_WARN
#define ROS_WARN(...)
realsense2_camera::BaseRealSenseNode::registerDynamicOption
void registerDynamicOption(ros::NodeHandle &nh, rs2::options sensor, std::string &module_name)
Definition: base_realsense_node.cpp:446
realsense2_camera::BaseRealSenseNode::publishStaticTransforms
void publishStaticTransforms()
Definition: base_realsense_node.cpp:2044
image
GLenum GLenum GLsizei void * image
realsense2_camera::BaseRealSenseNode::imu_callback_sync
void imu_callback_sync(rs2::frame frame, imu_sync_method sync_method=imu_sync_method::COPY)
Definition: base_realsense_node.cpp:1437
realsense2_camera::BaseRealSenseNode::_filters_str
std::string _filters_str
Definition: base_realsense_node.h:302
realsense2_camera::SyncedImuPublisher::_pending_messages
std::queue< sensor_msgs::Imu > _pending_messages
Definition: base_realsense_node.h:113
realsense2_camera::BaseRealSenseNode::OptionTemperatureDiag
std::pair< rs2_option, std::shared_ptr< TemperatureDiagnostics > > OptionTemperatureDiag
Definition: base_realsense_node.h:322
status
status
realsense2_camera::BaseRealSenseNode::float3::z
float z
Definition: base_realsense_node.h:138
index
GLuint index
realsense2_camera::BaseRealSenseNode::_depth_aligned_camera_info
std::map< stream_index_pair, sensor_msgs::CameraInfo > _depth_aligned_camera_info
Definition: base_realsense_node.h:312
realsense2_camera::BaseRealSenseNode::~BaseRealSenseNode
virtual ~BaseRealSenseNode()
Definition: base_realsense_node.cpp:137
RS2_STREAM_COLOR
RS2_STREAM_COLOR
rs2::option_range::max
float max
rs2::option_range::step
float step
t
GLdouble t
realsense2_camera::BaseRealSenseNode::_temperature_nodes
std::vector< OptionTemperatureDiag > _temperature_nodes
Definition: base_realsense_node.h:323
a
GLboolean GLboolean GLboolean GLboolean a
geometry_msgs::Quaternion_::z
_z_type z
realsense2_camera::SyncedImuPublisher::Publish
void Publish(sensor_msgs::Imu msg)
Definition: base_realsense_node.cpp:31
geometry_msgs::Pose_::position
_position_type position
val
GLuint GLfloat * val
rs2::points::size
size_t size() const
RS2_FORMAT_RGB8
RS2_FORMAT_RGB8
realsense2_camera::BaseRealSenseNode::_sensors
std::map< stream_index_pair, rs2::sensor > _sensors
Definition: base_realsense_node.h:251
begin
not_this_one begin(...)
realsense2_camera::BaseRealSenseNode::setupStreams
void setupStreams()
Definition: base_realsense_node.cpp:1811
realsense2_camera::BaseRealSenseNode::startMonitoring
void startMonitoring()
Definition: base_realsense_node.cpp:2429
realsense2_camera::BaseRealSenseNode::_width
std::map< stream_index_pair, int > _width
Definition: base_realsense_node.h:268
is_checkbox
bool is_checkbox(rs2::options sensor, rs2_option option)
Definition: base_realsense_node.cpp:280
rs2_vector::y
float y
f
GLdouble f
sensor_msgs::PointField_::FLOAT32
FLOAT32
geometry_msgs::TransformStamped_::header
_header_type header
realsense2_camera::stream_index_pair
std::pair< rs2_stream, int > stream_index_pair
Definition: constants.h:98
rs2::frame::is
bool is() const
realsense2_camera::BaseRealSenseNode::updateStreamCalibData
void updateStreamCalibData(const rs2::video_stream_profile &video_profile)
Definition: base_realsense_node.cpp:1866
get_enum_method
std::map< std::string, int > get_enum_method(rs2::options sensor, rs2_option option)
Definition: base_realsense_node.cpp:310
realsense2_camera::SyncedImuPublisher::_mutex
std::mutex _mutex
Definition: base_realsense_node.h:110
alpha
GLfloat GLfloat GLfloat alpha
realsense2_camera::CONFIDENCE
const stream_index_pair CONFIDENCE
Definition: realsense_node_factory.h:39
realsense2_camera::PUBLISH_TF
const bool PUBLISH_TF
Definition: constants.h:50
realsense2_camera::BaseRealSenseNode::_msg_pointcloud
sensor_msgs::PointCloud2 _msg_pointcloud
Definition: base_realsense_node.h:331
STREAM_NAME
#define STREAM_NAME(sip)
Definition: base_realsense_node.cpp:16
realsense2_camera::BaseRealSenseNode::_cv_monitoring
std::condition_variable _cv_monitoring
Definition: base_realsense_node.h:326
rs2_pose::velocity
rs2_vector velocity
streams
dictionary streams
realsense2_camera::SyncedImuPublisher::SyncedImuPublisher
SyncedImuPublisher()
Definition: base_realsense_node.h:97
size
GLsizeiptr size
ddynamic_reconfigure
geometry_msgs::PoseStamped_::pose
_pose_type pose
is_enum_option
bool is_enum_option(rs2::options sensor, rs2_option option)
Definition: base_realsense_node.cpp:288
lerp
T lerp(const T &a, const T &b, const double t)
Definition: base_realsense_node.cpp:1359
stream_format
rs2::serializable_device::load_json
void load_json(const std::string &json_content) const
realsense2_camera::BaseRealSenseNode::_colorizer
std::shared_ptr< rs2::filter > _colorizer
Definition: base_realsense_node.h:306
realsense2_camera::BaseRealSenseNode::setupFilters
void setupFilters()
Definition: base_realsense_node.cpp:1179
realsense2_camera::BaseRealSenseNode::CimuData::m_time
double m_time
Definition: base_realsense_node.h:190
rs2_option_to_string
const char * rs2_option_to_string(rs2_option option)
realsense2_camera::SyncedImuPublisher::_is_enabled
bool _is_enabled
Definition: base_realsense_node.h:115
i
D const & i
realsense2_camera::BaseRealSenseNode::publishDynamicTransforms
void publishDynamicTransforms()
Definition: base_realsense_node.cpp:2105
realsense2_camera::BaseRealSenseNode::getParameters
void getParameters()
Definition: base_realsense_node.cpp:714
c
const GLubyte * c
rs2::device::is
bool is() const
realsense2_camera::BaseRealSenseNode::_depth_to_other_extrinsics
std::map< stream_index_pair, rs2_extrinsics > _depth_to_other_extrinsics
Definition: base_realsense_node.h:317
realsense2_camera::BaseRealSenseNode::_depth_scale_meters
float _depth_scale_meters
Definition: base_realsense_node.h:257
realsense2_camera::BaseRealSenseNode::COPY
Definition: base_realsense_node.h:132
rs2::options::get_option_description
const char * get_option_description(rs2_option option) const
realsense2_camera::BaseRealSenseNode::float3::x
float x
Definition: base_realsense_node.h:138
x
GLdouble x
realsense2_camera::BaseRealSenseNode::_encoding
std::map< rs2_stream, std::string > _encoding
Definition: base_realsense_node.h:287
rs2::option_range
tf::vector3MsgToTF
static void vector3MsgToTF(const geometry_msgs::Vector3 &msg_v, Vector3 &bt_v)
ros::this_node::getNamespace
ROSCPP_DECL const std::string & getNamespace()
realsense2_camera::BaseRealSenseNode::_ordered_pc
bool _ordered_pc
Definition: base_realsense_node.h:260
std_msgs::Header_::seq
_seq_type seq
rs2::frame::get_timestamp
double get_timestamp() const
sensor_msgs::PointCloud2_::point_step
_point_step_type point_step
realsense2_camera::BaseRealSenseNode::_enable
std::map< stream_index_pair, bool > _enable
Definition: base_realsense_node.h:272
realsense2_camera::GYRO
const stream_index_pair GYRO
Definition: realsense_node_factory.h:36
rs2::device::hardware_reset
void hardware_reset()
T
T
realsense2_camera::BaseRealSenseNode::_image
std::map< stream_index_pair, cv::Mat > _image
Definition: base_realsense_node.h:286
RS2_STREAM_CONFIDENCE
RS2_STREAM_CONFIDENCE
rs2::device::get_info
const char * get_info(rs2_camera_info info) const
rs2_pose
realsense2_camera::BaseRealSenseNode::frame_callback
void frame_callback(rs2::frame frame)
Definition: base_realsense_node.cpp:1622
tf::Quaternion::setRPY
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
sensor_msgs::PointCloud2_::header
_header_type header
realsense2_camera::BaseRealSenseNode::_tf_publish_rate
double _tf_publish_rate
Definition: base_realsense_node.h:275
realsense2_camera::BaseRealSenseNode::_linear_accel_cov
double _linear_accel_cov
Definition: base_realsense_node.h:263
realsense2_camera::BaseRealSenseNode::_tf_t
std::shared_ptr< std::thread > _tf_t
Definition: base_realsense_node.h:279
rs2::options::is_option_read_only
bool is_option_read_only(rs2_option option) const
height
GLint GLsizei GLsizei height
ROS_WARN_ONCE
#define ROS_WARN_ONCE(...)
realsense2_camera::BaseRealSenseNode::_allow_no_texture_points
bool _allow_no_texture_points
Definition: base_realsense_node.h:259
realsense2_camera::BaseRealSenseNode::_video_functions_stack
std::map< rs2_stream, std::vector< std::function< void()> > > _video_functions_stack
Definition: base_realsense_node.h:320
ROS_INFO
#define ROS_INFO(...)
realsense2_camera::DEPTH
const stream_index_pair DEPTH
Definition: realsense_node_factory.h:29
rs2::points::get_vertices
const vertex * get_vertices() const
geometry_msgs::TransformStamped_::child_frame_id
_child_frame_id_type child_frame_id
geometry_msgs::PoseStamped_
rs2_pose::rotation
rs2_quaternion rotation
rs2::sensor::as
T as() const
sensor_msgs::Imu_
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
realsense2_camera::BaseRealSenseNode::publish_frequency_update
void publish_frequency_update()
Definition: base_realsense_node.cpp:2473
realsense2_camera::BaseRealSenseNode::_synced_imu_publisher
std::shared_ptr< SyncedImuPublisher > _synced_imu_publisher
Definition: base_realsense_node.h:283
cv_bridge::CvImage
realsense2_camera::SyncedImuPublisher::Resume
void Resume()
Definition: base_realsense_node.cpp:57
profiles
profiles
realsense2_camera::SyncedImuPublisher::~SyncedImuPublisher
~SyncedImuPublisher()
Definition: base_realsense_node.cpp:26
rs2_format_to_string
const char * rs2_format_to_string(rs2_format format)
rs2_intrinsics::width
int width
create_graph_resource_name
std::string create_graph_resource_name(const std::string &original_name)
Definition: base_realsense_node.cpp:358
realsense2_camera::BaseRealSenseNode::_node_handle
ros::NodeHandle & _node_handle
Definition: base_realsense_node.h:163
sensor_msgs::image_encodings::MONO8
const std::string MONO8
enabled
GLenum GLenum GLsizei const GLuint GLboolean enabled
realsense2_camera::BaseRealSenseNode::_pointcloud
bool _pointcloud
Definition: base_realsense_node.h:299
realsense2_camera::BaseRealSenseNode::_is_running
bool _is_running
Definition: base_realsense_node.h:157
geometry_msgs::Transform_::rotation
_rotation_type rotation
ros::ok
ROSCPP_DECL bool ok()
j
GLint j
sensor_msgs::image_encodings::TYPE_16UC1
const std::string TYPE_16UC1
sensor_msgs::PointCloud2Iterator
RS2_OPTION_GAIN
RS2_OPTION_GAIN
rs2_format
rs2_format
get_depth_scale
float get_depth_scale(rs2::device dev)
realsense2_camera::BaseRealSenseNode::_stream_name
std::map< rs2_stream, std::string > _stream_name
Definition: base_realsense_node.h:273
ROS_WARN_STREAM_COND
#define ROS_WARN_STREAM_COND(cond, args)
RS2_CAMERA_INFO_FIRMWARE_VERSION
RS2_CAMERA_INFO_FIRMWARE_VERSION
tf::quatRotate
TFSIMD_FORCE_INLINE Vector3 quatRotate(const Quaternion &rotation, const Vector3 &v)
rs2::processing_block::start
void start(S on_frame)
realsense2_camera::BaseRealSenseNode::_align_depth
bool _align_depth
Definition: base_realsense_node.h:164
geometry_msgs::Point_::z
_z_type z
img
GLint void * img
rs2::stream_profile::stream_index
int stream_index() const
realsense2_camera::BaseRealSenseNode::imu_callback
void imu_callback(rs2::frame frame)
Definition: base_realsense_node.cpp:1485
rs2::frameset::end
iterator end() const
rs2::frameset::begin
iterator begin() const
realsense2_camera::BaseRealSenseNode::_sync_frames
bool _sync_frames
Definition: base_realsense_node.h:298
sensor
sensor
realsense2_camera::BaseRealSenseNode::_depth_aligned_image_publishers
std::map< stream_index_pair, ImagePublisherWithFrequencyDiagnostics > _depth_aligned_image_publishers
Definition: base_realsense_node.h:315
rs2_vector::x
float x
realsense2_camera::BaseRealSenseNode::_depth_scaled_image
std::map< stream_index_pair, cv::Mat > _depth_scaled_image
Definition: base_realsense_node.h:310
geometry_msgs::TransformStamped_::transform
_transform_type transform
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
realsense2_camera::BaseRealSenseNode::multiple_message_callback
void multiple_message_callback(rs2::frame frame, imu_sync_method sync_method)
Definition: base_realsense_node.cpp:1767
rs2::options::supports
bool supports(rs2_option option) const
realsense2_camera::BaseRealSenseNode::rs2_string_to_stream
rs2_stream rs2_string_to_stream(std::string str)
Definition: base_realsense_node.cpp:701
rs2::video_stream_profile::get_intrinsics
rs2_intrinsics get_intrinsics() const
RS2_STREAM_DEPTH
RS2_STREAM_DEPTH
sensor_msgs::PointCloud2Modifier
func
GLenum func
realsense2_camera::IMU_FPS
const int IMU_FPS
Definition: constants.h:57
rs2_stream
rs2_stream
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
realsense2_camera::BaseRealSenseNode::_clipping_distance
float _clipping_distance
Definition: base_realsense_node.h:258
tf2_ros::TransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
sensor_msgs::PointCloud2_< std::allocator< void > >
sensor_msgs::PointCloud2_::is_dense
_is_dense_type is_dense
realsense2_camera::BaseRealSenseNode::NONE
Definition: base_realsense_node.h:132
rs2::notification::get_category
rs2_notification_category get_category() const
realsense2_camera::ALLOW_NO_TEXTURE_POINTS
const bool ALLOW_NO_TEXTURE_POINTS
Definition: constants.h:46
RS2_STREAM_INFRARED
RS2_STREAM_INFRARED
realsense2_camera::BaseRealSenseNode::publish_temperature
void publish_temperature()
Definition: base_realsense_node.cpp:2452
realsense2_camera::BaseRealSenseNode::_imu_sync_method
imu_sync_method _imu_sync_method
Definition: base_realsense_node.h:301
tf::Quaternion::inverse
Quaternion inverse() const
realsense2_camera::BaseRealSenseNode::getEnabledProfile
bool getEnabledProfile(const stream_index_pair &stream_index, rs2::stream_profile &profile)
Definition: base_realsense_node.cpp:2415
RS2_STREAM_ACCEL
RS2_STREAM_ACCEL
rs2_extrinsics
stream_format::format
rs2_format format
realsense2_camera::BaseRealSenseNode::registerHDRoptions
void registerHDRoptions()
Definition: base_realsense_node.cpp:619
realsense2_camera::BaseRealSenseNode::CimuData::m_type
stream_index_pair m_type
Definition: base_realsense_node.h:186
realsense2_camera::POINTCLOUD
const bool POINTCLOUD
Definition: constants.h:45
rs2::stream_profile::format
rs2_format format() const
sensor_msgs::Imu_::angular_velocity
_angular_velocity_type angular_velocity
realsense2_camera::BaseRealSenseNode::_enabled_profiles
std::map< stream_index_pair, std::vector< rs2::stream_profile > > _enabled_profiles
Definition: base_realsense_node.h:294
ROS_INFO_STREAM_ONCE
#define ROS_INFO_STREAM_ONCE(args)
realsense2_camera::IMAGE_FPS
const int IMAGE_FPS
Definition: constants.h:55
rs2_quaternion::w
float w
realsense2_camera::IMAGE_WIDTH
const int IMAGE_WIDTH
Definition: constants.h:53
bpp
bpp
q
GLdouble GLdouble GLdouble q
realsense2_camera::BaseRealSenseNode::enable_devices
void enable_devices()
Definition: base_realsense_node.cpp:1052
rs2::color_sensor
realsense2_camera::BaseRealSenseNode::_namespace
const std::string _namespace
Definition: base_realsense_node.h:329
realsense2_camera::BaseRealSenseNode::_imu_publishers
std::map< stream_index_pair, ros::Publisher > _imu_publishers
Definition: base_realsense_node.h:282
realsense2_camera::BaseRealSenseNode::calcAndPublishStaticTransform
virtual void calcAndPublishStaticTransform(const stream_index_pair &stream, const rs2::stream_profile &base_profile)
Definition: base_realsense_node.cpp:1983
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
enable
GLboolean enable
realsense2_camera::FrequencyDiagnostics
Definition: base_realsense_node.h:27
realsense2_camera::TemperatureDiagnostics::diagnostics
void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &status)
Definition: base_realsense_node.cpp:2487
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
geometry_msgs::Quaternion_::w
_w_type w
RS2_STREAM_ANY
RS2_STREAM_ANY
realsense2_camera::BaseRealSenseNode::LINEAR_INTERPOLATION
Definition: base_realsense_node.h:132
realsense2_camera::BaseRealSenseNode::_depth_to_other_extrinsics_publishers
std::map< stream_index_pair, ros::Publisher > _depth_to_other_extrinsics_publishers
Definition: base_realsense_node.h:316
realsense2_camera::BaseRealSenseNode::_camera_time_base
double _camera_time_base
Definition: base_realsense_node.h:293
in
GLuint in
realsense2_camera::BaseRealSenseNode::_monitor_options
std::vector< rs2_option > _monitor_options
Definition: base_realsense_node.h:165
realsense2_camera::BaseRealSenseNode::setupPublishers
void setupPublishers()
Definition: base_realsense_node.cpp:961
realsense2_camera::BaseRealSenseNode::_update_functions_v
std::vector< std::function< void()> > _update_functions_v
Definition: base_realsense_node.h:325
sensor_msgs::PointCloud2Modifier::resize
void resize(size_t size)
sensor_msgs::PointCloud2_::width
_width_type width
realsense2_camera::BaseRealSenseNode::_frame_id
std::map< stream_index_pair, std::string > _frame_id
Definition: base_realsense_node.h:160
rs2::frame::get_frame_timestamp_domain
rs2_timestamp_domain get_frame_timestamp_domain() const
rs2::video_frame::get_height
int get_height() const
realsense2_camera::BaseRealSenseNode::_height
std::map< stream_index_pair, int > _height
Definition: base_realsense_node.h:269
realsense2_camera::ACCEL
const stream_index_pair ACCEL
Definition: realsense_node_factory.h:37
realsense2_camera::POSE
const stream_index_pair POSE
Definition: realsense_node_factory.h:38
realsense2_camera::BaseRealSenseNode::_syncer
PipelineSyncer _syncer
Definition: base_realsense_node.h:304
realsense2_camera::BaseRealSenseNode::float3
Definition: base_realsense_node.h:135
realsense2_camera::BaseRealSenseNode::set_sensor_auto_exposure_roi
void set_sensor_auto_exposure_roi(rs2::sensor sensor)
Definition: base_realsense_node.cpp:387
realsense2_camera::BaseRealSenseNode::_camera_info
std::map< stream_index_pair, sensor_msgs::CameraInfo > _camera_info
Definition: base_realsense_node.h:291
realsense2_camera::BaseRealSenseNode::SetBaseStream
void SetBaseStream()
Definition: base_realsense_node.cpp:2026
realsense_legacy_msgs::pose_
realsense2_camera::SYNC_FRAMES
const bool SYNC_FRAMES
Definition: constants.h:48
realsense2_camera::BaseRealSenseNode::_pnh
ros::NodeHandle _pnh
Definition: base_realsense_node.h:163
sensor_msgs::Imu_::orientation_covariance
_orientation_covariance_type orientation_covariance
realsense2_camera::BaseRealSenseNode::readAndSetDynamicParam
void readAndSetDynamicParam(ros::NodeHandle &nh1, std::shared_ptr< ddynamic_reconfigure::DDynamicReconfigure > ddynrec, const std::string option_name, const int min_val, const int max_val, rs2::sensor sensor, int *option_value)
Definition: base_realsense_node.cpp:400
RS2_OPTION_EXPOSURE
RS2_OPTION_EXPOSURE
realsense2_camera::BaseRealSenseNode::_unit_step_size
std::map< rs2_stream, int > _unit_step_size
Definition: base_realsense_node.h:290
std_msgs::Header_::frame_id
_frame_id_type frame_id
tf2_ros::TransformBroadcaster
realsense2_camera::TF_PUBLISH_RATE
const double TF_PUBLISH_RATE
Definition: constants.h:51
rs2::sensor::is
bool is() const
realsense2_camera::BaseRealSenseNode::_monitoring_t
std::shared_ptr< std::thread > _monitoring_t
Definition: base_realsense_node.h:324
rs2_intrinsics
realsense2_camera::BaseRealSenseNode::imu_sync_method
imu_sync_method
Definition: base_realsense_node.h:132
str
str(const bytes &b)
realsense2_camera::BaseRealSenseNode::publish_static_tf
void publish_static_tf(const ros::Time &t, const float3 &trans, const tf::Quaternion &q, const std::string &from, const std::string &to)
Definition: base_realsense_node.cpp:1963
rs2::sensor::open
void open(const stream_profile &profile) const
ros::Time::now
static Time now()
sensor_msgs::Imu_::linear_acceleration_covariance
_linear_acceleration_covariance_type linear_acceleration_covariance
rs2::roi_sensor
realsense2_camera::BaseRealSenseNode::_static_tf_msgs
std::vector< geometry_msgs::TransformStamped > _static_tf_msgs
Definition: base_realsense_node.h:278
realsense2_camera::ORDERED_POINTCLOUD
const bool ORDERED_POINTCLOUD
Definition: constants.h:47
realsense2_camera::BaseRealSenseNode::_serial_no
std::string _serial_no
Definition: base_realsense_node.h:256
rs2::notification::get_timestamp
double get_timestamp() const
realsense2_camera::BaseRealSenseNode::_depth_aligned_encoding
std::map< rs2_stream, std::string > _depth_aligned_encoding
Definition: base_realsense_node.h:311
realsense2_camera::BaseRealSenseNode::setupErrorCallback
void setupErrorCallback()
Definition: base_realsense_node.cpp:218
ros::shutdown
ROSCPP_DECL void shutdown()
realsense2_camera::HID_STREAMS
const std::vector< stream_index_pair > HID_STREAMS
Definition: realsense_node_factory.h:46
realsense2_camera::BaseRealSenseNode::_fps
std::map< stream_index_pair, int > _fps
Definition: base_realsense_node.h:270
realsense2_camera::BaseRealSenseNode::getNamespaceStr
static std::string getNamespaceStr()
Definition: base_realsense_node.cpp:76
image_transport::ImageTransport
realsense2_camera::HOLD_BACK_IMU_FOR_FRAMES
const bool HOLD_BACK_IMU_FOR_FRAMES
Definition: constants.h:66
geometry_msgs::Transform_::translation
_translation_type translation
realsense2_camera::BaseRealSenseNode::_sensors_callback
std::map< std::string, std::function< void(rs2::frame)> > _sensors_callback
Definition: base_realsense_node.h:252
rs2_motion_device_intrinsic
realsense2_camera::BaseRealSenseNode::_base_stream
stream_index_pair _base_stream
Definition: base_realsense_node.h:328
realsense2_camera::BaseRealSenseNode::_dev_sensors
std::vector< rs2::sensor > _dev_sensors
Definition: base_realsense_node.h:307
realsense2_camera::BaseRealSenseNode::runFirstFrameInitialization
void runFirstFrameInitialization(rs2_stream stream_type)
Definition: base_realsense_node.cpp:259
realsense2_camera::BaseRealSenseNode::_ddynrec
std::vector< std::shared_ptr< ddynamic_reconfigure::DDynamicReconfigure > > _ddynrec
Definition: base_realsense_node.h:253
geometry_msgs::Vector3_::x
_x_type x
realsense_legacy_msgs::extrinsics_
geometry_msgs::Vector3_::z
_z_type z
res
GLuint res
realsense2_camera::NamedFilter::_name
std::string _name
Definition: base_realsense_node.h:76
diagnostic_updater::DiagnosticStatusWrapper::add
void add(const std::string &key, const T &val)
realsense2_camera::BaseRealSenseNode::_depth_aligned_seq
std::map< stream_index_pair, int > _depth_aligned_seq
Definition: base_realsense_node.h:313
std_msgs::Header_::stamp
_stamp_type stamp
rs2::options::get_option_range
option_range get_option_range(rs2_option option) const
rs2_intrinsics::height
int height
realsense2_camera::FISHEYE
const stream_index_pair FISHEYE
Definition: realsense_node_factory.h:33
realsense2_camera::BaseRealSenseNode::_update_functions_cv
std::condition_variable _update_functions_cv
Definition: base_realsense_node.h:326
rs2::sensor::get_info
const char * get_info(rs2_camera_info info) const
geometry_msgs::TransformStamped_
realsense2_camera::COLOR
const stream_index_pair COLOR
Definition: realsense_node_factory.h:28
realsense2_camera::SyncedImuPublisher::PublishPendingMessages
void PublishPendingMessages()
Definition: base_realsense_node.cpp:64
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
realsense2_camera::BaseRealSenseNode::_auto_exposure_roi
std::map< std::string, rs2::region_of_interest > _auto_exposure_roi
Definition: base_realsense_node.h:318
isValidCharInName
bool isValidCharInName(char c)
Definition: base_realsense_node.cpp:349
realsense2_camera::BaseRealSenseNode::CreateUnitedMessage
sensor_msgs::Imu CreateUnitedMessage(const CimuData accel_data, const CimuData gyro_data)
Definition: base_realsense_node.cpp:1342
realsense2_camera::INFRA1
const stream_index_pair INFRA1
Definition: realsense_node_factory.h:31
rs2::texture_coordinate
profile::format
rs2_format format
tf2_ros::StaticTransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
realsense2_camera::BaseRealSenseNode::setBaseTime
bool setBaseTime(double frame_time, rs2_timestamp_domain time_domain)
Definition: base_realsense_node.cpp:1785
realsense2_camera::BaseRealSenseNode::getAProfile
rs2::stream_profile getAProfile(const stream_index_pair &stream)
Definition: base_realsense_node.cpp:2307
geometry_msgs::Point_::y
_y_type y
realsense2_camera::BaseRealSenseNode::_json_file_path
std::string _json_file_path
Definition: base_realsense_node.h:255
sensor_msgs::PointCloud2Modifier::setPointCloud2FieldsByString
void setPointCloud2FieldsByString(int n_fields,...)
rs2_timestamp_domain_to_string
const char * rs2_timestamp_domain_to_string(rs2_timestamp_domain info)
rs2::frame::get_frame_number
unsigned long long get_frame_number() const
sensor_msgs::PointCloud2_::height
_height_type height
sensor_msgs::Imu
::sensor_msgs::Imu_< std::allocator< void > > Imu
rs2::video_stream_profile
rs2::pose_frame
rs2::stream_profile::stream_type
rs2_stream stream_type() const
realsense2_camera::BaseRealSenseNode::_depth_aligned_info_publisher
std::map< stream_index_pair, ros::Publisher > _depth_aligned_info_publisher
Definition: base_realsense_node.h:314
realsense2_camera::BaseRealSenseNode::_optical_frame_id
std::map< stream_index_pair, std::string > _optical_frame_id
Definition: base_realsense_node.h:161
rs2::options::set_option
void set_option(rs2_option option, float value) const
ALIGNED_DEPTH_TO_FRAME_ID
#define ALIGNED_DEPTH_TO_FRAME_ID(sip)
Definition: base_realsense_node.cpp:19
geometry_msgs::Quaternion_::y
_y_type y
rs2::pose_sensor
geometry_msgs::Vector3Stamped_
realsense2_camera::BaseRealSenseNode::_hold_back_imu_for_frames
bool _hold_back_imu_for_frames
Definition: base_realsense_node.h:265
rs2::options::get_option
float get_option(rs2_option option) const
std_msgs::Header_< std::allocator< void > >
realsense2_camera::ROS_DEPTH_SCALE
const float ROS_DEPTH_SCALE
Definition: constants.h:97
is_int_option
bool is_int_option(rs2::options sensor, rs2_option option)
Definition: base_realsense_node.cpp:304
realsense2_camera::ENABLE_IMU
const bool ENABLE_IMU
Definition: constants.h:65
v
GLdouble v
sensor_msgs::Imu_::header
_header_type header
rs2_quaternion::z
float z
rs2::serializable_device
rs2::video_frame::get_width
int get_width() const
rs2::stream_profile::fps
int fps() const
rs2::region_of_interest::min_y
int min_y
realsense2_camera::BaseRealSenseNode::publishIntrinsics
void publishIntrinsics()
Definition: base_realsense_node.cpp:2124
realsense2_camera::BaseRealSenseNode::_odom_frame_id
std::string _odom_frame_id
Definition: base_realsense_node.h:159
rs2::options::get_option_value_description
const char * get_option_value_description(rs2_option option, float val) const
rs2::texture_coordinate::v
float v
RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME
RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME
realsense2_camera::BaseRealSenseNode::set_auto_exposure_roi
void set_auto_exposure_roi(const std::string option_name, rs2::sensor sensor, int new_value)
Definition: base_realsense_node.cpp:368
realsense2_camera::IMAGE_HEIGHT
const int IMAGE_HEIGHT
Definition: constants.h:54
realsense2_camera::BaseRealSenseNode::_publish_odom_tf
bool _publish_odom_tf
Definition: base_realsense_node.h:300
rs2::sensor::start
void start(T callback) const
realsense2_camera::DEFAULT_FILTERS
const std::string DEFAULT_FILTERS
Definition: constants.h:94
realsense2_camera::BaseRealSenseNode::_depth_aligned_frame_id
std::map< stream_index_pair, std::string > _depth_aligned_frame_id
Definition: base_realsense_node.h:162
realsense2_camera::BaseRealSenseNode::FillImuData_LinearInterpolation
void FillImuData_LinearInterpolation(const CimuData imu_data, std::deque< sensor_msgs::Imu > &imu_msgs)
Definition: base_realsense_node.cpp:1363
width
GLint GLsizei width
RS2_FORMAT_Y8
RS2_FORMAT_Y8
sensor_msgs::ImagePtr
std::shared_ptr< ::sensor_msgs::Image > ImagePtr
diagnostic_updater::DiagnosticStatusWrapper
Q
Q
offset
GLintptr offset
rs2::device::as
T as() const
rs2::frame::as
T as() const
ROS_DEBUG
#define ROS_DEBUG(...)
realsense2_camera::BaseRealSenseNode::_format
std::map< rs2_stream, int > _format
Definition: base_realsense_node.h:271
std::to_string
std::string to_string(T value)
rs2::depth_frame
FRAME_ID
#define FRAME_ID(sip)
Definition: base_realsense_node.cpp:17
rs2_timestamp_domain
rs2_timestamp_domain

realsense2_camera
Author(s): Sergey Dorodnicov , Doron Hirshberg
autogenerated on Thu May 13 2021 02:33:12