realsense_node_factory.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2017 Intel Corporation. All Rights Reserved
3 
4 #include "../include/realsense_node_factory.h"
5 #include "../include/base_realsense_node.h"
6 #include "../include/t265_realsense_node.h"
7 #include <iostream>
8 #include <map>
9 #include <mutex>
10 #include <condition_variable>
11 #include <signal.h>
12 #include <thread>
13 #include <regex>
14 
15 using namespace realsense2_camera;
16 
17 #define REALSENSE_ROS_EMBEDDED_VERSION_STR (VAR_ARG_STRING(VERSION: REALSENSE_ROS_MAJOR_VERSION.REALSENSE_ROS_MINOR_VERSION.REALSENSE_ROS_PATCH_VERSION))
19 
21 
22 std::string api_version_to_string(int version)
23 {
24  std::ostringstream ss;
25  if (version / 10000 == 0)
26  ss << version;
27  else
28  ss << (version / 10000) << "." << (version % 10000) / 100 << "." << (version % 100);
29  return ss.str();
30 }
31 
33  _is_alive(true)
34 {
35  rs2_error* e = nullptr;
36  std::string running_librealsense_version(api_version_to_string(rs2_get_api_version(&e)));
37  ROS_INFO("RealSense ROS v%s", REALSENSE_ROS_VERSION_STR);
38  ROS_INFO("Built with LibRealSense v%s", RS2_API_VERSION_STR);
39  ROS_INFO_STREAM("Running with LibRealSense v" << running_librealsense_version);
40  if (RS2_API_VERSION_STR != running_librealsense_version)
41  {
42  ROS_WARN("***************************************************");
43  ROS_WARN("** running with a different librealsense version **");
44  ROS_WARN("** than the one the wrapper was compiled with! **");
45  ROS_WARN("***************************************************");
46  }
47 
48  auto severity = rs2_log_severity::RS2_LOG_SEVERITY_WARN;
50  if (rs2_log_severity::RS2_LOG_SEVERITY_DEBUG == severity)
52 
54 }
55 
57 {
58  _is_alive = false;
59  if (_query_thread.joinable())
60  {
61  _query_thread.join();
62  }
63 }
64 
65 std::string RealSenseNodeFactory::parse_usb_port(std::string line)
66 {
67  std::string port_id;
68  std::regex self_regex("(?:[^ ]+/usb[0-9]+[0-9./-]*/){0,1}([0-9.-]+)(:){0,1}[^ ]*", std::regex_constants::ECMAScript);
69  std::smatch base_match;
70  bool found = std::regex_match(line, base_match, self_regex);
71  if (found)
72  {
73  port_id = base_match[1].str();
74  if (base_match[2].str().size() == 0) //This is libuvc string. Remove counter is exists.
75  {
76  std::regex end_regex = std::regex(".+(-[0-9]+$)", std::regex_constants::ECMAScript);
77  bool found_end = std::regex_match(port_id, base_match, end_regex);
78  if (found_end)
79  {
80  port_id = port_id.substr(0, port_id.size() - base_match[1].str().size());
81  }
82  }
83  }
84  return port_id;
85 }
86 
88 {
89  if (!_device)
90  {
91  if (0 == list.size())
92  {
93  ROS_WARN("No RealSense devices were found!");
94  }
95  else
96  {
97  bool found = false;
98  ROS_INFO_STREAM(" ");
99  for (size_t count = 0; count < list.size(); count++)
100  {
102  try
103  {
104  dev = list[count];
105  }
106  catch(const std::exception& ex)
107  {
108  ROS_WARN_STREAM("Device " << count+1 << "/" << list.size() << " failed with exception: " << ex.what());
109  continue;
110  }
112  ROS_INFO_STREAM("Device with serial number " << sn << " was found."<<std::endl);
113  std::string pn = dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT);
114  std::string name = dev.get_info(RS2_CAMERA_INFO_NAME);
115  ROS_INFO_STREAM("Device with physical ID " << pn << " was found.");
116  std::vector<std::string> results;
117  ROS_INFO_STREAM("Device with name " << name << " was found.");
118  std::string port_id = parse_usb_port(pn);
119  if (port_id.empty())
120  {
121  std::stringstream msg;
122  msg << "Error extracting usb port from device with physical ID: " << pn << std::endl << "Please report on github issue at https://github.com/IntelRealSense/realsense-ros";
123  if (_usb_port_id.empty())
124  {
125  ROS_WARN_STREAM(msg.str());
126  }
127  else
128  {
129  ROS_ERROR_STREAM(msg.str());
130  ROS_ERROR_STREAM("Please use serial number instead of usb port.");
131  }
132  }
133  else
134  {
135  ROS_INFO_STREAM("Device with port number " << port_id << " was found.");
136  }
137  bool found_device_type(true);
138  if (!_device_type.empty())
139  {
140  std::smatch match_results;
141  std::regex device_type_regex(_device_type.c_str(), std::regex::icase);
142  found_device_type = std::regex_search(name, match_results, device_type_regex);
143  }
144 
145  if ((_serial_no.empty() || sn == _serial_no) && (_usb_port_id.empty() || port_id == _usb_port_id) && found_device_type)
146  {
147  _device = dev;
148  _serial_no = sn;
149  found = true;
150  break;
151  }
152  }
153  if (!found)
154  {
155  // T265 could be caught by another node.
156  std::string msg ("The requested device with ");
157  bool add_and(false);
158  if (!_serial_no.empty())
159  {
160  msg += "serial number " + _serial_no;
161  add_and = true;
162  }
163  if (!_usb_port_id.empty())
164  {
165  if (add_and)
166  {
167  msg += " and ";
168  }
169  msg += "usb port id " + _usb_port_id;
170  add_and = true;
171  }
172  if (!_device_type.empty())
173  {
174  if (add_and)
175  {
176  msg += " and ";
177  }
178  msg += "device name containing " + _device_type;
179  }
180  msg += " is NOT found. Will Try again.";
181  ROS_ERROR_STREAM(msg);
182  }
183  else
184  {
186  {
187  std::string usb_type = _device.get_info(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR);
188  ROS_INFO_STREAM("Device USB type: " << usb_type);
189  if (usb_type.find("2.") != std::string::npos)
190  {
191  ROS_WARN_STREAM("Device " << _serial_no << " is connected using a " << usb_type << " port. Reduced performance is expected.");
192  }
193  }
194  }
195  }
196  }
197 
198  bool remove_tm2_handle(_device && RS_T265_PID != std::stoi(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID), 0, 16));
199  if (remove_tm2_handle)
200  {
202  }
203 
204  if (_device && _initial_reset)
205  {
206  _initial_reset = false;
207  try
208  {
209  ROS_INFO("Resetting device...");
211  _device = rs2::device();
212  }
213  catch(const std::exception& ex)
214  {
215  ROS_WARN_STREAM("An exception has been thrown: " << ex.what());
216  }
217  }
218 }
219 
221 {
222  if (info.was_removed(_device))
223  {
224  ROS_ERROR("The device has been disconnected!");
225  reset();
226  }
227 }
228 
229 bool RealSenseNodeFactory::toggle_sensor_callback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
230 {
231  if (req.data)
232  ROS_INFO_STREAM("toggling sensor : ON");
233  else
234  ROS_INFO_STREAM("toggling sensor : OFF");
235  _realSenseNode->toggleSensors(req.data);
236  res.success=true;
237  return true;
238 }
239 
241 {
242  auto nh = getNodeHandle();
243  _init_timer = nh.createWallTimer(ros::WallDuration(0.01), &RealSenseNodeFactory::initialize, this, true);
244 }
245 
247 {
248  _device = rs2::device();
249  try
250  {
251 #ifdef BPDEBUG
252  std::cout << "Attach to Process: " << getpid() << std::endl;
253  std::cout << "Press <ENTER> key to continue." << std::endl;
254  std::cin.get();
255 #endif
257  auto privateNh = getPrivateNodeHandle();
258  privateNh.param("serial_no", _serial_no, std::string(""));
259  privateNh.param("usb_port_id", _usb_port_id, std::string(""));
260  privateNh.param("device_type", _device_type, std::string(""));
261 
262  if (!toggle_sensor_srv)
263  {
265  }
266  std::string rosbag_filename("");
267  privateNh.param("rosbag_filename", rosbag_filename, std::string(""));
268 
269  if (!rosbag_filename.empty())
270  {
271  {
272  ROS_INFO_STREAM("publish topics from rosbag file: " << rosbag_filename.c_str());
273  auto pipe = std::make_shared<rs2::pipeline>();
275  cfg.enable_device_from_file(rosbag_filename.c_str(), false);
276  cfg.enable_all_streams();
277  pipe->start(cfg); //File will be opened in read mode at this point
278  _device = pipe->get_active_profile().get_device();
280  }
281  if (_device)
282  {
283  StartDevice();
284  }
285  }
286  else
287  {
288  privateNh.param("initial_reset", _initial_reset, false);
289 
290  _is_alive = true;
291  _query_thread = std::thread([=]()
292  {
293  std::chrono::milliseconds timespan(6000);
294  while (_is_alive && !_device)
295  {
297  if (_device)
298  {
299  std::function<void(rs2::event_information&)> change_device_callback_function = [this](rs2::event_information& info){change_device_callback(info);};
300  _ctx.set_devices_changed_callback(change_device_callback_function);
301  StartDevice();
302  }
303  else
304  {
305  std::this_thread::sleep_for(timespan);
306  }
307  }
308  });
309  if (!_reset_srv)
310  {
311  _reset_srv = privateNh.advertiseService("reset", &RealSenseNodeFactory::handleReset, this);
312  }
313  }
314  }
315  catch(const std::exception& ex)
316  {
317  ROS_ERROR_STREAM("An exception has been thrown: " << ex.what());
318  exit(1);
319  }
320  catch(...)
321  {
322  ROS_ERROR_STREAM("Unknown exception has occured!");
323  exit(1);
324  }
325 }
326 
328 {
329  if (_realSenseNode) _realSenseNode.reset();
330  try
331  {
334  // TODO
335  std::string pid_str(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID));
336  uint16_t pid = std::stoi(pid_str, 0, 16);
337  switch(pid)
338  {
339  case SR300_PID:
340  case SR300v2_PID:
341  case RS400_PID:
342  case RS405_PID:
343  case RS410_PID:
344  case RS460_PID:
345  case RS415_PID:
346  case RS420_PID:
347  case RS420_MM_PID:
348  case RS430_PID:
349  case RS430_MM_PID:
350  case RS430_MM_RGB_PID:
351  case RS435_RGB_PID:
352  case RS435i_RGB_PID:
353  case RS455_PID:
354  case RS465_PID:
355  case RS_USB2_PID:
356  case RS_L515_PID_PRE_PRQ:
357  case RS_L515_PID:
358  _realSenseNode = std::shared_ptr<BaseRealSenseNode>(new BaseRealSenseNode(nh, privateNh, _device, _serial_no));
359  break;
360  case RS_T265_PID:
361  _realSenseNode = std::shared_ptr<T265RealsenseNode>(new T265RealsenseNode(nh, privateNh, _device, _serial_no));
362  break;
363  default:
364  ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str);
365  ros::shutdown();
366  exit(1);
367  }
369  _realSenseNode->publishTopics();
370  }
371  catch (const rs2::error& e)
372  {
373  ROS_ERROR_STREAM("Exception: " << e.what());
374  }
375 }
376 
378 {
379  _is_alive = false;
380  if (_query_thread.joinable())
381  {
382  _query_thread.join();
383  }
384 
385  try
386  {
387  _realSenseNode.reset();
388  if (_device)
389  {
391  _device = rs2::device();
392  }
393  }
394  catch (const rs2::error& e)
395  {
396  ROS_ERROR_STREAM("Exception: " << e.what());
397  }
398 
400 }
401 
402 bool RealSenseNodeFactory::handleReset(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
403 {
404  reset();
405  return true;
406 }
407 
409 {
410  static const char* severity_var_name = "LRS_LOG_LEVEL";
411  auto content = getenv(severity_var_name);
412 
413  if (content)
414  {
415  std::string content_str(content);
416  std::transform(content_str.begin(), content_str.end(), content_str.begin(), ::toupper);
417 
418  for (uint32_t i = 0; i < RS2_LOG_SEVERITY_COUNT; i++)
419  {
420  auto current = std::string(rs2_log_severity_to_string((rs2_log_severity)i));
421  std::transform(current.begin(), current.end(), current.begin(), ::toupper);
422  if (content_str == current)
423  {
424  severity = (rs2_log_severity)i;
425  break;
426  }
427  }
428  }
429 }
msg
pipe
const char * rs2_log_severity_to_string(rs2_log_severity severity)
bool handleReset(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
GLenum GLuint GLenum severity
RS2_CAMERA_INFO_PHYSICAL_PORT
dev
const uint16_t RS430_MM_RGB_PID
Definition: constants.h:31
def info(args)
RS2_CAMERA_INFO_SERIAL_NUMBER
const uint16_t RS_L515_PID_PRE_PRQ
Definition: constants.h:40
bool was_removed(const rs2::device &dev) const
RS2_CAMERA_INFO_PRODUCT_ID
device_list query_devices() const
std::string api_version_to_string(int version)
const uint16_t RS_T265_PID
Definition: constants.h:39
uint32_t size() const
bool toggle_sensor_callback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
unsigned short uint16_t
const uint16_t RS415_PID
Definition: constants.h:23
const uint16_t RS_USB2_PID
Definition: constants.h:26
RS2_CAMERA_INFO_NAME
void unload_tracking_module()
const uint16_t RS430_PID
Definition: constants.h:24
e
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
const uint16_t RS460_PID
Definition: constants.h:32
cfg
#define ROS_WARN(...)
const uint16_t RS465_PID
Definition: constants.h:35
void initialize(const ros::WallTimerEvent &ignored)
ros::NodeHandle & getPrivateNodeHandle() const
static std::string parse_usb_port(std::string line)
const uint16_t RS420_MM_PID
Definition: constants.h:28
GLsizeiptr size
D const & i
const uint16_t RS420_PID
Definition: constants.h:27
unsigned int uint32_t
const uint16_t SR300v2_PID
Definition: constants.h:20
RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR
void hardware_reset()
const uint16_t RS430_MM_PID
Definition: constants.h:25
#define ROS_FATAL_STREAM(args)
const char * get_info(rs2_camera_info info) const
#define ROS_INFO(...)
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
std::shared_ptr< InterfaceRealSenseNode > _realSenseNode
void tryGetLogSeverity(rs2_log_severity &severity) const
constexpr auto realsense_ros_camera_version
ros::NodeHandle & getNodeHandle() const
const uint16_t RS435_RGB_PID
Definition: constants.h:33
#define ROS_WARN_STREAM(args)
bool supports(rs2_camera_info info) const
const uint16_t RS_L515_PID
Definition: constants.h:41
int rs2_get_api_version(rs2_error **error) BEGIN_API_CALL
#define REALSENSE_ROS_EMBEDDED_VERSION_STR
RS2_LOG_SEVERITY_COUNT
const uint16_t RS455_PID
Definition: constants.h:38
void log_to_console(rs2_log_severity min_severity)
#define ROS_INFO_STREAM(args)
void change_device_callback(rs2::event_information &info)
GLint GLsizei count
#define RS2_API_VERSION_STR
const uint16_t RS405_PID
Definition: constants.h:37
#define REALSENSE_ROS_VERSION_STR
Definition: constants.h:15
str(const bytes &b)
void set_devices_changed_callback(T callback)
ROSCPP_DECL void shutdown()
int stoi(const std::string &value)
GLuint res
ROSCONSOLE_DECL bool set_logger_level(const std::string &name, levels::Level level)
#define ROS_ERROR_STREAM(args)
const uint16_t RS435i_RGB_PID
Definition: constants.h:34
const uint16_t RS400_PID
Definition: constants.h:21
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
const uint16_t RS410_PID
Definition: constants.h:22
rs2_log_severity
#define ROS_ERROR(...)
#define ROSCONSOLE_DEFAULT_NAME
const uint16_t SR300_PID
Definition: constants.h:19


realsense2_camera
Author(s): Sergey Dorodnicov , Doron Hirshberg
autogenerated on Wed Apr 7 2021 02:33:32