Cleaned up the loading of input devices

adf_loader/adf_loader_base.h

@@ -122,13 +122,13 @@ class ADFLoaderBase{
     virtual bool loadVehicleAttribs(YAML::Node* a_node, afVehicleAttributes* attribs){}
 
     // Load Input Device Attributes
-    virtual bool loadInputDeviceAttributes(YAML::Node* a_node, afInputDeviceAttributes *attribs){}
+    virtual bool loadInputDeviceAttribs(YAML::Node* a_node, afInputDeviceAttributes *attribs){}
 
     // Load Simulated Device Attributes
-    virtual bool loadSimulatedDeviceAttributes(YAML::Node* a_node, afSimulatedDeviceAttribs *attribs){}
+    virtual bool loadSimulatedDeviceAttribs(YAML::Node* a_node, afSimulatedDeviceAttribs *attribs){}
 
     // Load all the TU device attributes
-    virtual bool loadAllTeleRoboticUnitsAttribs(string a_filepath, afAllTeleRoboticUnitsAttributes *attribs){}
+    virtual bool loadTeleRoboticUnitsAttribs(string a_filepath, vector<afTeleRoboticUnitAttributes>* attribs, vector<int> dev_indexes){}
 
     // Load model from ADF file
     virtual bool loadModelAttribs(string a_filepath, afModelAttributes* attribs){}

adf_loader/adf_loader_interface.cpp

@@ -205,13 +205,13 @@ bool ADFLoaderInterface::loadModelAttribs(string a_filepath, afModelAttributes *
     }
 }
 
-bool ADFLoaderInterface::loadAllTeleRoboticUnitsAttribs(string a_filepath, afAllTeleRoboticUnitsAttributes *attribs)
+bool ADFLoaderInterface::loadTeleRoboticUnitsAttribs(std::string a_filepath, vector<afTeleRoboticUnitAttributes> *attribs, vector<int> dev_indexes)
 {
     if (!setLoaderVersionForFile(a_filepath)){
         return false;
     }
     else{
-        bool result = m_loader->loadAllTeleRoboticUnitsAttribs(a_filepath, attribs);
+        bool result = m_loader->loadTeleRoboticUnitsAttribs(a_filepath, attribs, dev_indexes);
         return result;
     }
 

adf_loader/adf_loader_interface.h

@@ -68,7 +68,7 @@ class ADFLoaderInterface{
     bool loadModelAttribs(std::string a_filepath, afModelAttributes* attribs);
 
     // Load TU Attribs
-    bool loadAllTeleRoboticUnitsAttribs(std::string a_filepath, afAllTeleRoboticUnitsAttributes* attribs);
+    bool loadTeleRoboticUnitsAttribs(std::string a_filepath, vector<afTeleRoboticUnitAttributes> *attribs, vector<int> dev_indexes);
 
     // Load the Launch file Attribs
     bool loadLaunchFileAttribs(std::string a_filepath, afLaunchAttributes* attribs);

adf_loader/version_1_0/adf_loader_1_0.cpp

@@ -1016,7 +1016,7 @@ bool ADFLoader_1_0::loadObjectAttribs(YAML::Node *a_node, string a_objName, afOb
     case afObjectType::CAMERA:
         return loadCameraAttribs(&node, (afCameraAttributes*)attribs);
     case afObjectType::INPUT_DEVICE:
-        return loadInputDeviceAttributes(&node, (afInputDeviceAttributes*)attribs);
+        return loadInputDeviceAttribs(&node, (afInputDeviceAttributes*)attribs);
     default:
         return false;
     }
@@ -1903,7 +1903,7 @@ bool ADFLoader_1_0::loadVehicleAttribs(YAML::Node* a_node, afVehicleAttributes *
 }
 
 
-bool ADFLoader_1_0::loadInputDeviceAttributes(YAML::Node* a_node, afInputDeviceAttributes *attribs)
+bool ADFLoader_1_0::loadInputDeviceAttribs(YAML::Node* a_node, afInputDeviceAttributes *attribs)
 {
     YAML::Node& node = *a_node;
     if (node.IsNull()){
@@ -2005,7 +2005,7 @@ bool ADFLoader_1_0::loadInputDeviceAttributes(YAML::Node* a_node, afInputDeviceA
     return true;
 }
 
-bool ADFLoader_1_0::loadSimulatedDeviceAttributes(YAML::Node *a_node, afSimulatedDeviceAttribs *attribs)
+bool ADFLoader_1_0::loadSimulatedDeviceAttribs(YAML::Node *a_node, afSimulatedDeviceAttribs *attribs)
 {
     YAML::Node& node = *a_node;
     if (node.IsNull()){
@@ -2081,54 +2081,68 @@ bool ADFLoader_1_0::loadSimulatedDeviceAttributes(YAML::Node *a_node, afSimulate
 }
 
 
-bool ADFLoader_1_0::loadAllTeleRoboticUnitsAttribs(string a_filepath, afAllTeleRoboticUnitsAttributes *attribs){
-    attribs->m_filePath = afPath(a_filepath);
+bool ADFLoader_1_0::loadTeleRoboticUnitsAttribs(string a_filepath, vector<afTeleRoboticUnitAttributes>* attribs, vector<int> dev_indexes){
     YAML::Node node = YAML::LoadFile(a_filepath);
-    return loadAllTeleRoboticUnitsAttribs(&node, attribs);
+    return loadTeleRoboticUnitsAttribs(&node, a_filepath, attribs, dev_indexes);
 }
 
 
-bool ADFLoader_1_0::loadAllTeleRoboticUnitsAttribs(YAML::Node *a_node, afAllTeleRoboticUnitsAttributes *attribs)
+bool ADFLoader_1_0::loadTeleRoboticUnitsAttribs(YAML::Node *a_node, string a_filepath, vector<afTeleRoboticUnitAttributes>* attribs, vector<int> dev_indexes)
 {
     YAML::Node& node = *a_node;
     if (node.IsNull()){
         cerr << "ERROR: ALL INPUT DEVICES'S YAML CONFIG DATA IS NULL\n";
         return 0;
     }
 
+    afPath filePath(a_filepath);
+
     YAML::Node inputDevicesNode = node["input devices"];
 
-    for (int i = 0 ; i < inputDevicesNode.size() ; i++){
-        afTeleRoboticUnitAttributes tuAttribs;
-        std::string devName = inputDevicesNode[i].as<std::string>();
-        YAML::Node tuNode = node[devName];
-        bool results[2] = {false, false};
-        if (loadInputDeviceAttributes(&tuNode, &tuAttribs.m_iidAttribs)){
-            results[0] = true;
-        }
-        tuAttribs.m_sdeAttribs.m_filePath = attribs->m_filePath;
-        if (loadSimulatedDeviceAttributes(&tuNode, &tuAttribs.m_sdeAttribs)){
-            results[1] = true;
-        }
+    int valid_dev_idxs = min(dev_indexes.size(), inputDevicesNode.size());
 
-        YAML::Node pairCamerasNode = tuNode["pair cameras"];
+    bool load_status = true;
 
-        if(pairCamerasNode.IsDefined() && results[0] == true && results[1] == true){
-            for(int i = 0 ; i < pairCamerasNode.size() ; i++){
-                string camName = pairCamerasNode[i].as<string>();
-                tuAttribs.m_pairedCamerasNames.push_back(camName);
-            }
-        }
+    if (valid_dev_idxs > 0){
+        for (int i = 0 ; i < valid_dev_idxs ; i++){
+            int devIdx = dev_indexes[i];
+            if (devIdx >=0 && devIdx < inputDevicesNode.size()){
+                afTeleRoboticUnitAttributes tuAttribs;
+                std::string devName = inputDevicesNode[devIdx].as<std::string>();
+                YAML::Node tuNode = node[devName];
+                bool results[2] = {false, false};
+                if (loadInputDeviceAttribs(&tuNode, &tuAttribs.m_iidAttribs)){
+                    results[0] = true;
+                }
+                tuAttribs.m_sdeAttribs.m_filePath = filePath;
+                if (loadSimulatedDeviceAttribs(&tuNode, &tuAttribs.m_sdeAttribs)){
+                    results[1] = true;
+                }
 
-        if (results[0] == true && results[1] == true){
-            attribs->m_teleRoboticUnitsAttribs.push_back(tuAttribs);
-        }
-        else{
-            // THROW SOME WARNING
+                YAML::Node pairCamerasNode = tuNode["pair cameras"];
+
+                if(pairCamerasNode.IsDefined() && results[0] == true && results[1] == true){
+                    for(int j = 0 ; j < pairCamerasNode.size() ; j++){
+                        string camName = pairCamerasNode[j].as<string>();
+                        tuAttribs.m_pairedCamerasNames.push_back(camName);
+                    }
+                }
+
+                if (results[0] == true && results[1] == true){
+                    attribs->push_back(tuAttribs);
+                }
+                else{
+                    // THROW SOME WARNING
+                }
+            }
+            else{
+                std::cerr << "ERROR: DEVICE INDEX : \"" << devIdx << "\" > \"" << inputDevicesNode.size() << "\" NO. OF DEVICE SPECIFIED IN \"" << filePath.c_str() << "\"\n";
+                load_status = false;
+            }
         }
     }
 
-    return true;
+    return load_status;
 }
 
 

adf_loader/version_1_0/adf_loader_1_0.h

@@ -154,13 +154,13 @@ class ADFLoader_1_0: public ADFLoaderBase{
     virtual bool loadVehicleAttribs(YAML::Node* a_node, afVehicleAttributes* attribs);
 
     // Load Input Device Attributes
-    virtual bool loadInputDeviceAttributes(YAML::Node* a_node, afInputDeviceAttributes *attribs);
+    virtual bool loadInputDeviceAttribs(YAML::Node* a_node, afInputDeviceAttributes *attribs);
 
     // Load Simulated Device Attributes
-    virtual bool loadSimulatedDeviceAttributes(YAML::Node* a_node, afSimulatedDeviceAttribs *attribs);
+    virtual bool loadSimulatedDeviceAttribs(YAML::Node* a_node, afSimulatedDeviceAttribs *attribs);
 
     // Load all the input device attributes
-    virtual bool loadAllTeleRoboticUnitsAttribs(string, afAllTeleRoboticUnitsAttributes *attribs);
+    virtual bool loadTeleRoboticUnitsAttribs(string, vector<afTeleRoboticUnitAttributes> *attribs, vector<int> dev_indexes);
 
     // Load model from ADF file
     virtual bool loadModelAttribs(string, afModelAttributes* attribs);
@@ -172,7 +172,7 @@ class ADFLoader_1_0: public ADFLoaderBase{
     virtual bool loadLaunchFileAttribs(string, afLaunchAttributes* attribs);
 
     // Load all the input device attributes
-    bool loadAllTeleRoboticUnitsAttribs(YAML::Node* a_node, afAllTeleRoboticUnitsAttributes *attribs);
+    bool loadTeleRoboticUnitsAttribs(YAML::Node* a_node, string a_filepath, vector<afTeleRoboticUnitAttributes>* attribs, vector<int> dev_indexes);
 
     // Load model from ADF file
     bool loadModelAttribs(YAML::Node* a_node, afModelAttributes* attribs);

ambf_framework/afAttributes.h

@@ -1028,23 +1028,6 @@ struct afTeleRoboticUnitAttributes{
 };
 
 
-// Struct for multiple input devices
-struct afAllTeleRoboticUnitsAttributes: public afFileObjectAttributes{
-public:
-    afAllTeleRoboticUnitsAttributes(){}
-    std::vector <afTeleRoboticUnitAttributes> m_teleRoboticUnitsAttribs;
-
-    virtual bool resolveRelativePathAttribs(){
-        for (int i = 0 ; i < m_teleRoboticUnitsAttribs.size() ; i++){
-            m_teleRoboticUnitsAttribs[i].m_sdeAttribs.m_filePath = m_filePath;
-        }
-
-        return true;
-    }
-};
-
-
-
 ///
 /// \brief The afWorldAttributes struct
 ///

ambf_framework/afInputDevices.cpp

@@ -812,64 +812,43 @@ bool afCollateralControlManager::checkClaimedDeviceIdx(int a_devIdx){
 }
 
 
-bool afCollateralControlManager::createFromAttribs(afAllTeleRoboticUnitsAttributes* a_attribs, int a_max_load_devs){
-    std::vector<int> devIdxes;
-    for (int i = 0 ; i < a_max_load_devs ; i++){
-        devIdxes.push_back(i);
-    }
-    return createFromAttribs(a_attribs, devIdxes);
-}
-
-
 ///
 /// \brief afCollateralControlManager::createFromAttribs
 /// \param a_attribs
 /// \return
 ///
-bool afCollateralControlManager::createFromAttribs(afAllTeleRoboticUnitsAttributes* a_attribs, std::vector<int> a_device_indices){
-
-    afAllTeleRoboticUnitsAttributes& attribs = *a_attribs;
+bool afCollateralControlManager::createFromAttribs(vector<afTeleRoboticUnitAttributes> *a_attribsVec){
 
     bool load_status = false;
 
-    int valid_dev_idxs = cMin(a_device_indices.size(), attribs.m_teleRoboticUnitsAttribs.size());
-    if (valid_dev_idxs > 0){
+    if (a_attribsVec->size() > 0){
         m_deviceHandler.reset(new cHapticDeviceHandler());
-        for (int i = 0; i < valid_dev_idxs; i++){
-            int devIdx = a_device_indices[i];
-            string devName = attribs.m_teleRoboticUnitsAttribs[devIdx].m_iidAttribs.m_hardwareName;
-            if (devIdx >=0 && devIdx < attribs.m_teleRoboticUnitsAttribs.size()){
-                afPhysicalDevice* pD = new afPhysicalDevice(this);
-                afSimulatedDevice* sD = new afSimulatedDevice(m_afWorld, pD);
-
-                if (pD->createFromAttribs(&attribs.m_teleRoboticUnitsAttribs[devIdx].m_iidAttribs)){
-                    if (sD->createFromAttribs(&attribs.m_teleRoboticUnitsAttribs[devIdx].m_sdeAttribs)){
-                        afCollateralControlUnit ccu;
-                        ccu.m_physicalDevicePtr = pD;
-                        ccu.m_simulatedDevicePtr = sD;
-                        ccu.m_name = devName;
-                        ccu.pairCameras(m_afWorld, attribs.m_teleRoboticUnitsAttribs[devIdx].m_pairedCamerasNames);
-                        m_collateralControlUnits.push_back(ccu);
-                        load_status = true;
-                    }
-                }
-                else
-                {
-                    std::cerr << "WARNING: FAILED TO LOAD DEVICE: \"" << devName << "\"\n";
-                    load_status = false;
-                    delete pD;
-                    delete sD;
-                }
-            }
-            else{
-                std::cerr << "ERROR: DEVICE INDEX : \"" << devIdx << "\" > \"" << attribs.m_teleRoboticUnitsAttribs.size() << "\" NO. OF DEVICE SPECIFIED IN \"" << attribs.m_filePath.c_str() << "\"\n";
-                load_status = false;
+    }
+
+    for (int i = 0; i < a_attribsVec->size(); i++){
+        afTeleRoboticUnitAttributes tuAttrib = (*a_attribsVec)[i];
+        string devName = tuAttrib.m_iidAttribs.m_hardwareName;
+        afPhysicalDevice* pD = new afPhysicalDevice(this);
+        afSimulatedDevice* sD = new afSimulatedDevice(m_afWorld, pD);
+
+        if (pD->createFromAttribs(&tuAttrib.m_iidAttribs)){
+            if (sD->createFromAttribs(&tuAttrib.m_sdeAttribs)){
+                afCollateralControlUnit ccu;
+                ccu.m_physicalDevicePtr = pD;
+                ccu.m_simulatedDevicePtr = sD;
+                ccu.m_name = devName;
+                ccu.pairCameras(m_afWorld, tuAttrib.m_pairedCamerasNames);
+                m_collateralControlUnits.push_back(ccu);
+                load_status = true;
             }
         }
-    }
-    else{
-        std::cerr << "ERROR: SIZE OF DEVICE INDEXES : \"" << a_device_indices.size() << "\" > NO. OF DEVICE SPECIFIED IN \"" << attribs.m_filePath.c_str() << "\"\n";
-        load_status = false;
+        else
+        {
+            std::cerr << "WARNING: FAILED TO LOAD DEVICE: \"" << devName << "\"\n";
+            load_status = false;
+            delete pD;
+            delete sD;
+        }
     }
 
     m_numDevices = m_collateralControlUnits.size();

ambf_framework/afInputDevices.h

@@ -346,9 +346,7 @@ class afCollateralControlManager{
     // Get an instance of AFWorld from Input Deivces class
     const afWorldPtr getAFWorld(){return m_afWorld;}
 
-    bool createFromAttribs(afAllTeleRoboticUnitsAttributes* a_attribs, int a_num_devs_to_load=MAX_DEVICES);
-
-    bool createFromAttribs(afAllTeleRoboticUnitsAttributes* a_attribs, std::vector<int> a_device_indices);
+    bool createFromAttribs(vector<afTeleRoboticUnitAttributes> *a_attribs);
 
     boost::filesystem::path getBasePath(){return m_basePath;}
 

ambf_simulator/src/ambf_simulator.cpp

@@ -93,7 +93,7 @@ struct CommandLineOptions{
     // Enable Force Feedback
     bool enableForceFeedback = true;
     // Number of Devices to Load
-    int numDevicesToLoad;
+    int numDevicesToLoad = MAX_DEVICES;
     // A string of device indexes to load
     std::string devicesToLoad = "";
     // A string list of multibody indexes to load
@@ -281,7 +281,6 @@ int main(int argc, char* argv[])
     p_opt::store(p_opt::command_line_parser(argc, argv).options(cmd_opts).run(), var_map);
     p_opt::notify(var_map);
 
-    g_cmdOpts.numDevicesToLoad = MAX_DEVICES;
     if(var_map.count("help")){ std::cout<< cmd_opts << std::endl; return 0;}
 
     if(var_map.count("ndevs")){ g_cmdOpts.numDevicesToLoad = var_map["ndevs"].as<int>();}
@@ -380,7 +379,7 @@ int main(int argc, char* argv[])
 
     afLaunchAttributes launchAttribs;
     afWorldAttributes worldAttribs;
-    afAllTeleRoboticUnitsAttributes allTUAttribs;
+    std::vector<afTeleRoboticUnitAttributes> tuAttribs;
     std::vector<afModelAttributes> modelsAttribs;
     if (g_adfLoader->loadLaunchFileAttribs(g_cmdOpts.launchFilePath, &launchAttribs) == false){
         // Safely Return the program
@@ -390,7 +389,26 @@ int main(int argc, char* argv[])
     launchAttribs.resolveRelativePathAttribs();
 
     g_adfLoader->loadWorldAttribs(launchAttribs.m_worldFilepath.c_str(), &worldAttribs);
-    g_adfLoader->loadAllTeleRoboticUnitsAttribs(launchAttribs.m_inputDevicesFilepath.c_str(), &allTUAttribs);
+
+    std::vector<int> devIndexes;
+    if (!g_cmdOpts.devicesToLoad.empty()){
+        std::string loadDevIndices = g_cmdOpts.devicesToLoad;
+        loadDevIndices.erase(std::remove(loadDevIndices.begin(), loadDevIndices.end(), ' '), loadDevIndices.end());
+        std::stringstream ss(loadDevIndices);
+        while(ss.good() )
+        {
+            string devIndex;
+            getline( ss, devIndex, ',' );
+            devIndexes.push_back(std::stoi(devIndex));
+        }
+    }
+    else{
+        for (int i = 0 ; i < g_cmdOpts.numDevicesToLoad ; i++){
+            devIndexes.push_back(i);
+        }
+    }
+
+    g_adfLoader->loadTeleRoboticUnitsAttribs(launchAttribs.m_inputDevicesFilepath.c_str(), &tuAttribs, devIndexes);
 
     // create a dynamic world.
     g_afWorld = new afWorld(g_cmdOpts.prepend_namespace);
@@ -535,22 +553,7 @@ int main(int argc, char* argv[])
     // START: INITIALIZE THREADS FOR ALL REQUIRED HAPTIC DEVICES AND PHYSICS THREAD
     //-----------------------------------------------------------------------------------------------------------
     g_inputDevices = std::make_shared<afCollateralControlManager>(g_afWorld);
-    if (!g_cmdOpts.devicesToLoad.empty()){
-        std::vector<int> devIndices;
-        std::string loadDevIndices = g_cmdOpts.devicesToLoad;
-        loadDevIndices.erase(std::remove(loadDevIndices.begin(), loadDevIndices.end(), ' '), loadDevIndices.end());
-        std::stringstream ss(loadDevIndices);
-        while(ss.good() )
-        {
-            string devIndex;
-            getline( ss, devIndex, ',' );
-            devIndices.push_back(std::stoi(devIndex));
-        }
-        g_inputDevices->createFromAttribs(&allTUAttribs, devIndices);
-    }
-    else{
-        g_inputDevices->createFromAttribs(&allTUAttribs, g_cmdOpts.numDevicesToLoad);
-    }
+    g_inputDevices->createFromAttribs(&tuAttribs);
 
     //-----------------------------------------------------------------------------------------------------------
     // END: SEARCH FOR CONTROLLING DEVICES FOR CAMERAS IN AMBF AND ADD THEM TO RELEVANT WINDOW-CAMERA PAIR