Implement attach logic in TechnosoftIpos

roboticslab-uc3m · Jan 14, 2024 · 5ad0b3b · 5ad0b3b
1 parent 479b218
commit 5ad0b3b
Show file tree

Hide file tree

Showing 6 changed files with 60 additions and 56 deletions.
diff --git a/libraries/YarpPlugins/TechnosoftIpos/CMakeLists.txt b/libraries/YarpPlugins/TechnosoftIpos/CMakeLists.txt
@@ -20,6 +20,7 @@ if(NOT SKIP_TechnosoftIpos)
                                           EncoderRead.hpp
                                           EncoderRead.cpp
                                           DeviceDriverImpl.cpp
+                                          IWrapperImpl.cpp
                                           IAxisInfoRawImpl.cpp
                                           ICanBusSharerImpl.cpp
                                           IControlLimitsRawImpl.cpp

diff --git a/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp
@@ -67,41 +67,6 @@ bool TechnosoftIposBase::open(yarp::os::Searchable & config)
         return false;
     }
 
-    if (iposGroup.check("externalEncoder", "external encoder"))
-    {
-        std::string externalEncoder = iposGroup.find("externalEncoder").asString();
-        yarp::os::Bottle & externalEncoderGroup = robotConfig->findGroup(externalEncoder);
-
-        if (externalEncoderGroup.isNull())
-        {
-            yCIError(IPOS, id()) << "Missing external encoder device group" << externalEncoder;
-            return false;
-        }
-
-        yarp::os::Property externalEncoderOptions;
-        externalEncoderOptions.fromString(externalEncoderGroup.toString());
-        externalEncoderOptions.put("robotConfig", config.find("robotConfig"));
-        externalEncoderOptions.setMonitor(config.getMonitor(), externalEncoder.c_str());
-
-        if (!externalEncoderDevice.open(externalEncoderOptions))
-        {
-            yCIError(IPOS, id()) << "Unable to open external encoder device" << externalEncoder;
-            return false;
-        }
-
-        if (!externalEncoderDevice.view(iEncodersTimedRawExternal))
-        {
-            yCIError(IPOS, id()) << "Unable to view IEncodersTimedRaw in" << externalEncoder;
-            return false;
-        }
-
-        if (!externalEncoderDevice.view(iExternalEncoderCanBusSharer))
-        {
-            yCIError(IPOS, id()) << "Unable to view ICanBusSharer in" << externalEncoder;
-            return false;
-        }
-    }
-
     double sdoTimeout = iposGroup.check("sdoTimeout", yarp::os::Value(DEFAULT_SDO_TIMEOUT),
             "CAN SDO timeout (seconds)").asFloat64();
     double driveStateTimeout = iposGroup.check("driveStateTimeout", yarp::os::Value(DEFAULT_DRIVE_STATE_TIMEOUT),
@@ -186,11 +151,6 @@ bool TechnosoftIposBase::close()
     delete can;
     can = nullptr;
 
-    if (externalEncoderDevice.isValid())
-    {
-        return externalEncoderDevice.close();
-    }
-
     return true;
 }
 

diff --git a/libraries/YarpPlugins/TechnosoftIpos/ICanBusSharerImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/ICanBusSharerImpl.cpp
@@ -94,9 +94,7 @@ bool TechnosoftIposBase::initialize()
                 0x1018, 0x04);
     }
 
-    double extEnc;
-
-    if (!configuredOnce
+    if (double extEnc; !configuredOnce
         || (iExternalEncoderCanBusSharer && !iExternalEncoderCanBusSharer->initialize())
         || !setLimitsRaw(0, min, max)
         || !setRefSpeedRaw(0, refSpeed)

diff --git a/libraries/YarpPlugins/TechnosoftIpos/IWrapperImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IWrapperImpl.cpp
@@ -0,0 +1,37 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "TechnosoftIposBase.hpp"
+
+using namespace roboticslab;
+
+#include <yarp/os/LogStream.h>
+
+#include "LogComponent.hpp"
+
+// -----------------------------------------------------------------------------
+
+bool TechnosoftIposBase::attach(yarp::dev::PolyDriver * driver)
+{
+    if (!driver->view(iEncodersTimedRawExternal))
+    {
+        yCIError(IPOS, id()) << "Unable to view IEncodersTimedRaw in" << driver->id();
+        return false;
+    }
+
+    if (!driver->view(iExternalEncoderCanBusSharer))
+    {
+        yCIError(IPOS, id()) << "Unable to view ICanBusSharer in" << driver->id();
+        return false;
+    }
+
+    return false;
+}
+
+// -----------------------------------------------------------------------------
+
+bool TechnosoftIposBase::detach()
+{
+    return true;
+}
+
+// -----------------------------------------------------------------------------
diff --git a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
@@ -19,6 +19,7 @@ namespace roboticslab
  * @brief Implementation for the Technosoft iPOS as a single CAN bus joint (control board raw interfaces).
  */
 class TechnosoftIpos : public yarp::dev::DeviceDriver,
+                       public yarp::dev::IWrapper,
                        public yarp::dev::IAxisInfoRaw,
                        public yarp::dev::IControlLimitsRaw,
                        public yarp::dev::IControlModeRaw,
@@ -50,6 +51,14 @@ class TechnosoftIpos : public yarp::dev::DeviceDriver,
     std::string id() const override
     { return impl->id(); }
 
+    //  --------- IWrapper declarations. Implementation in IWrapperImpl.cpp ---------
+
+    bool attach(yarp::dev::PolyDriver * driver) override
+    { return impl->attach(driver); }
+
+    bool detach() override
+    { return impl->detach(); }
+
     //  --------- ICanBusSharer declarations. Implementation in ICanBusSharerImpl.cpp ---------
 
     unsigned int getId() override

diff --git a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIposBase.hpp b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIposBase.hpp
@@ -31,6 +31,7 @@
 #include <yarp/dev/IRemoteVariables.h>
 #include <yarp/dev/ITorqueControl.h>
 #include <yarp/dev/IVelocityControl.h>
+#include <yarp/dev/IWrapper.h>
 #include <yarp/dev/PolyDriver.h>
 
 #include "CanOpenNode.hpp"
@@ -52,6 +53,7 @@ namespace roboticslab
  * @brief Base class for all proxied TechnosoftIpos implementations.
  */
 class TechnosoftIposBase : public yarp::dev::DeviceDriver,
+                           public yarp::dev::IWrapper,
                            public yarp::dev::IAxisInfoRaw,
                            public yarp::dev::IControlLimitsRaw,
                            public yarp::dev::IControlModeRaw,
@@ -72,18 +74,16 @@ class TechnosoftIposBase : public yarp::dev::DeviceDriver,
 {
 public:
 
-    TechnosoftIposBase()
-        : can(nullptr),
-          iEncodersTimedRawExternal(nullptr),
-          iExternalEncoderCanBusSharer(nullptr),
-          monitorThread(nullptr)
-    {}
-
     //  --------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp ---------
 
     bool open(yarp::os::Searchable & config) override;
     bool close() override;
 
+    //  --------- IWrapper declarations. Implementation in IWrapperImpl.cpp ---------
+
+    bool attach(yarp::dev::PolyDriver * driver) override;
+    bool detach() override;
+
     //  --------- ICanBusSharer declarations. Implementation in ICanBusSharerImpl.cpp ---------
 
     unsigned int getId() override;
@@ -499,7 +499,7 @@ class TechnosoftIposBase : public yarp::dev::DeviceDriver,
     //! Convert torque (Nm) to current (amperes).
     double torqueToCurrent(double torque) const;
 
-    CanOpenNode * can;
+    CanOpenNode * can {nullptr};
     CommandBuffer commandBuffer;
 
     std::unique_ptr<StateObserver> controlModeObserverPtr {new StateObserver(1.0)}; // arbitrary 1 second wait
@@ -587,12 +587,11 @@ class TechnosoftIposBase : public yarp::dev::DeviceDriver,
     bool setLimitRaw(double limit, bool isMin);
     bool getLimitRaw(double * limit, bool isMin);
 
-    yarp::dev::PolyDriver externalEncoderDevice;
-    yarp::dev::IEncodersTimedRaw * iEncodersTimedRawExternal;
-    roboticslab::ICanBusSharer * iExternalEncoderCanBusSharer;
+    yarp::dev::IEncodersTimedRaw * iEncodersTimedRawExternal {nullptr};
+    roboticslab::ICanBusSharer * iExternalEncoderCanBusSharer {nullptr};
 
-    yarp::os::Timer * monitorThread;
-    roboticslab::ICanSenderDelegate * sender;
+    yarp::os::Timer * monitorThread {nullptr};
+    roboticslab::ICanSenderDelegate * sender {nullptr};
 };
 
 } // namespace roboticslab