Don't attach disabled external encoders

roboticslab-uc3m · Jan 22, 2024 · 32ec086 · 32ec086
1 parent 9b8e3ec
commit 32ec086
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 2 deletions.
diff --git a/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp
@@ -130,6 +130,16 @@ bool TechnosoftIposBase::open(yarp::os::Searchable & config)
         }
     }
 
+    if (yarp::os::Value * val; iposGroup.check("disabledEncoders", val, "disabled encoder IDs") && val->isList())
+    {
+        const auto * ids = val->asList();
+
+        for (int i = 0; i < ids->size(); i++)
+        {
+            disabledEncoderIds.push_back(ids->get(i).asInt32());
+        }
+    }
+
     return true;
 }
 

diff --git a/libraries/YarpPlugins/TechnosoftIpos/IWrapperImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IWrapperImpl.cpp
@@ -2,16 +2,27 @@
 
 #include "TechnosoftIposBase.hpp"
 
-using namespace roboticslab;
+#include <algorithm> // std::find_if
+#include <string>
 
 #include <yarp/os/LogStream.h>
 
 #include "LogComponent.hpp"
 
+using namespace roboticslab;
+
 // -----------------------------------------------------------------------------
 
 bool TechnosoftIposBase::attach(yarp::dev::PolyDriver * driver)
 {
+    if (auto it = std::find_if(disabledEncoderIds.begin(), disabledEncoderIds.end(),
+                               [driver](int id) { return "ID" + std::to_string(id) == driver->id(); });
+        it != disabledEncoderIds.end())
+    {
+        yCIInfo(IPOS, id()) << "Skipping attach of disabled external encoder" << driver->id();
+        return true;
+    }
+
     if (!driver->view(iEncodersTimedRawExternal))
     {
         yCIError(IPOS, id()) << "Unable to view IEncodersTimedRaw in" << driver->id();
@@ -24,7 +35,7 @@ bool TechnosoftIposBase::attach(yarp::dev::PolyDriver * driver)
         return false;
     }
 
-    return false;
+    return true;
 }
 
 // -----------------------------------------------------------------------------

diff --git a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIposBase.hpp b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIposBase.hpp
@@ -9,6 +9,7 @@
 #include <bitset>
 #include <memory>
 #include <string>
+#include <vector>
 
 #include <yarp/conf/numeric.h>
 
@@ -592,6 +593,8 @@ class TechnosoftIposBase : public yarp::dev::DeviceDriver,
 
     yarp::os::Timer * monitorThread {nullptr};
     roboticslab::ICanSenderDelegate * sender {nullptr};
+
+    std::vector<int> disabledEncoderIds;
 };
 
 } // namespace roboticslab