From 195da375f19528fade2d79a98ffe79ed2959f46b Mon Sep 17 00:00:00 2001 From: Osenberg Date: Fri, 22 Mar 2019 20:12:06 +0800 Subject: [PATCH] feat(ros): added multiple device launch for ros --- .../launch/mynteye.launch | 10 + .../launch/mynteye_multiple.launch | 7 + .../launch/sub/mynteye_1.launch | 377 ++++++++++++++++++ .../launch/sub/mynteye_2.launch | 377 ++++++++++++++++++ .../src/wrapper_nodelet.cc | 110 +++-- 5 files changed, 849 insertions(+), 32 deletions(-) create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye_multiple.launch create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_1.launch create mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_2.launch diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch index b82dc5d..8cf722d 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch @@ -2,6 +2,13 @@ + + + + + + + @@ -227,6 +234,9 @@ + + + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye_multiple.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye_multiple.launch new file mode 100644 index 0000000..0782cf5 --- /dev/null +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye_multiple.launch @@ -0,0 +1,7 @@ + + + + + + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_1.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_1.launch new file mode 100644 index 0000000..3885271 --- /dev/null +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_1.launch @@ -0,0 +1,377 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_2.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_2.launch new file mode 100644 index 0000000..66215a0 --- /dev/null +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_2.launch @@ -0,0 +1,377 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + + + diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc index 0ff0e2d..5c09bab 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc @@ -1034,39 +1034,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet { private: void initDevice() { - NODELET_INFO_STREAM("Detecting MYNT EYE devices"); - - Context context; - auto &&devices = context.devices(); - - size_t n = devices.size(); - NODELET_FATAL_COND(n <= 0, "No MYNT EYE devices :("); - - NODELET_INFO_STREAM("MYNT EYE devices:"); - for (size_t i = 0; i < n; i++) { - auto &&device = devices[i]; - auto &&name = device->GetInfo(Info::DEVICE_NAME); - NODELET_INFO_STREAM(" index: " << i << ", name: " << name); - } - std::shared_ptr device = nullptr; - if (n <= 1) { - device = devices[0]; - NODELET_INFO_STREAM("Only one MYNT EYE device, select index: 0"); - } else { - while (true) { - size_t i; - NODELET_INFO_STREAM( - "There are " << n << " MYNT EYE devices, select index: "); - std::cin >> i; - if (i >= n) { - NODELET_WARN_STREAM("Index out of range :("); - continue; - } - device = devices[i]; - break; - } - } + + device = selectDevice(); api_ = API::Create(device); auto &&requests = device->GetStreamRequests(); @@ -1115,6 +1085,82 @@ class ROSWrapperNodelet : public nodelet::Nodelet { computeRectTransforms(); } + std::shared_ptr selectDevice() { + NODELET_INFO_STREAM("Detecting MYNT EYE devices"); + + Context context; + auto &&devices = context.devices(); + + bool is_multiple = false; + private_nh_.getParam("is_multiple", is_multiple); + if (is_multiple) { + std::string sn; + private_nh_.getParam("serial_number", sn); + NODELET_FATAL_COND(sn.empty(), "Must set serial_number " + "in mynteye_1.launch and mynteye_2.launch."); + + size_t n = devices.size(); + NODELET_FATAL_COND(n <= 0, "No MYNT EYE devices :("); + + NODELET_INFO_STREAM("MYNT EYE devices:"); + for (size_t i = 0; i < n; i++) { + auto &&device = devices[i]; + auto &&name = device->GetInfo(Info::DEVICE_NAME); + auto &&serial_number = device->GetInfo(Info::SERIAL_NUMBER); + NODELET_INFO_STREAM(" index: " << i << ", name: " << + name << ", serial number: " << serial_number); + } + for (size_t i = 0; i < n; i++) { + auto &&device = devices[i]; + auto &&name = device->GetInfo(Info::DEVICE_NAME); + auto &&serial_number = device->GetInfo(Info::SERIAL_NUMBER); + NODELET_INFO_STREAM(" index: " << i << ", name: " << + name << ", serial number: " << serial_number); + if (sn == serial_number) + return device; +#if 0 + if (i == (n - 1)) { + /* + NODELET_FATAL_COND(i == (n - 1), "No corresponding device was found," + " check the serial_number configuration. "); + */ + NODELET_FATAL_COND(i == (n - 1), "No corresponding device was found," + " check the serial_number configuration. "); + return nullptr; + } +#endif + } + } + size_t n = devices.size(); + NODELET_FATAL_COND(n <= 0, "No MYNT EYE devices :("); + + NODELET_INFO_STREAM("MYNT EYE devices:"); + for (size_t i = 0; i < n; i++) { + auto &&device = devices[i]; + auto &&name = device->GetInfo(Info::DEVICE_NAME); + NODELET_INFO_STREAM(" index: " << i << ", name: " << name); + } + + if (n <= 1) { + NODELET_INFO_STREAM("Only one MYNT EYE device, select index: 0"); + return devices[0]; + } else { + while (true) { + size_t i; + NODELET_INFO_STREAM( + "There are " << n << " MYNT EYE devices, select index: "); + std::cin >> i; + if (i >= n) { + NODELET_WARN_STREAM("Index out of range :("); + continue; + } + return devices[i]; + } + } + + return nullptr; + } + std::shared_ptr getDefaultIntrinsics() { auto res = std::make_shared(); res->width = 640;