From 415cf31a36804365ed9966deb9a154879c97048e Mon Sep 17 00:00:00 2001
From: Osenberg <junyangqu@slightech.com>
Date: Sat, 23 Mar 2019 14:23:23 +0800
Subject: [PATCH] feat(ros): optimized multiple device process for ros

---
 .../launch/mynteye_multiple.launch            |  2 -
 .../src/wrapper_nodelet.cc                    | 75 +++++++------------
 2 files changed, 28 insertions(+), 49 deletions(-)

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 @@
 <launch>
       <include file="$(find mynt_eye_ros_wrapper)/launch/sub/mynteye_1.launch" />
       <include file="$(find mynt_eye_ros_wrapper)/launch/sub/mynteye_2.launch" />
-      <!-- <include file="$(find mynt_eye_ros_wrapper)/launch/sub/mynteye_3.launch" />
-      <include file="$(find mynt_eye_ros_wrapper)/launch/sub/mynteye_4.launch" /> -->
 </launch>
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];
       }
     }