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

realsense2_camera
Author(s): Sergey Dorodnicov , Doron Hirshberg
autogenerated on Fri Mar 25 2022 02:15:35