diff --git a/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp index acd6825ce..5559ee865 100644 --- 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 index 38a6b4512..76b354f57 100644 --- a/libraries/YarpPlugins/TechnosoftIpos/IWrapperImpl.cpp +++ b/libraries/YarpPlugins/TechnosoftIpos/IWrapperImpl.cpp @@ -2,16 +2,27 @@ #include "TechnosoftIposBase.hpp" -using namespace roboticslab; +#include // std::find_if +#include #include #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 index ae68cb888..bc9d8daa8 100644 --- a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIposBase.hpp +++ b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIposBase.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include @@ -592,6 +593,8 @@ class TechnosoftIposBase : public yarp::dev::DeviceDriver, yarp::os::Timer * monitorThread {nullptr}; roboticslab::ICanSenderDelegate * sender {nullptr}; + + std::vector disabledEncoderIds; }; } // namespace roboticslab