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 index 0782cf5..cf6cbad 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye_multiple.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye_multiple.launch @@ -2,6 +2,4 @@ - 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 40fbd53..30b4ac4 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 @@ -1098,6 +1098,18 @@ class ROSWrapperNodelet : public nodelet::Nodelet { 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); + auto &&serial_number = device->GetInfo(Info::SERIAL_NUMBER); + NODELET_INFO_STREAM(" index: " << i << ", name: " << + name << ", serial number: " << serial_number); + } + bool is_multiple = false; private_nh_.getParam("is_multiple", is_multiple); if (is_multiple) { @@ -1106,62 +1118,31 @@ class ROSWrapperNodelet : public nodelet::Nodelet { 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 + NODELET_FATAL_COND(i == (n - 1), "No corresponding device was found," + " check the serial_number configuration. "); } - } - 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; + 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 devices[i]; } }