Implement attach mechanism in CCS

libraries/YarpPlugins/CartesianControlServer/CMakeLists.txt

@@ -10,6 +10,7 @@ if(NOT SKIP_CartesianControlServer)
 
     yarp_add_plugin(CartesianControlServer CartesianControlServer.hpp
                                            DeviceDriverImpl.cpp
+                                           IWrapperImpl.cpp
                                            PeriodicThreadImpl.cpp
                                            RpcResponder.cpp
                                            StreamResponder.cpp

libraries/YarpPlugins/CartesianControlServer/CartesianControlServer.hpp

@@ -12,6 +12,7 @@
 
 #include <yarp/dev/Drivers.h>
 #include <yarp/dev/PolyDriver.h>
+#include <yarp/dev/WrapperSingle.h>
 
 #include "ICartesianControl.h"
 #include "KinematicRepresentation.hpp"
@@ -35,33 +36,37 @@ class StreamResponder;
  * @brief The CartesianControlServer class implements ICartesianControl server side.
  */
 class CartesianControlServer : public yarp::dev::DeviceDriver,
+                               public yarp::dev::WrapperSingle,
                                public yarp::os::PeriodicThread
 {
 public:
     CartesianControlServer() : yarp::os::PeriodicThread(1.0)
     {}
 
     // -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp --------
-
     bool open(yarp::os::Searchable & config) override;
     bool close() override;
 
+    // -------- IWrapper declarations. Implementation in IWrapperImpl.cpp --------
+    bool attach(yarp::dev::PolyDriver * poly) override;
+    bool detach() override;
+
     // -------- PeriodicThread declarations. Implementation in PeriodicThreadImpl.cpp --------
     void run() override;
 
 protected:
+    bool configureHandle();
+
     yarp::dev::PolyDriver cartesianControlDevice;
 
     yarp::os::RpcServer rpcServer, rpcTransformServer;
     yarp::os::BufferedPort<yarp::os::Bottle> fkOutPort, commandPort;
 
-    roboticslab::ICartesianControl * iCartesianControl {nullptr};
+    ICartesianControl * iCartesianControl {nullptr};
 
     RpcResponder * rpcResponder {nullptr};
     RpcResponder * rpcTransformResponder {nullptr};
     StreamResponder * streamResponder {nullptr};
-
-    bool fkStreamEnabled;
 };
 
 /**
@@ -71,24 +76,16 @@ class CartesianControlServer : public yarp::dev::DeviceDriver,
 class RpcResponder : public yarp::dev::DeviceResponder
 {
 public:
-    RpcResponder(roboticslab::ICartesianControl * _iCartesianControl)
-        : iCartesianControl(_iCartesianControl)
+    RpcResponder()
     {
         // shadows DeviceResponder::makeUsage(), which was already called by the base constructor
         makeUsage();
     }
 
-    /**
-     * Respond to a message.
-     * @param in the message
-     * @param out the response
-     * @return true if there was no critical failure
-     */
-    bool respond(const yarp::os::Bottle & in, yarp::os::Bottle & out) override;
+    void setHandle(ICartesianControl * _iCartesianControl)
+    { iCartesianControl = _iCartesianControl; }
 
-    /**
-     * Generate command usage information.
-     */
+    bool respond(const yarp::os::Bottle & in, yarp::os::Bottle & out) override;
     void makeUsage();
 
 protected:
@@ -117,7 +114,7 @@ class RpcResponder : public yarp::dev::DeviceResponder
     bool handleParameterSetterGroup(const yarp::os::Bottle & in, yarp::os::Bottle & out);
     bool handleParameterGetterGroup(const yarp::os::Bottle & in, yarp::os::Bottle & out);
 
-    roboticslab::ICartesianControl * iCartesianControl;
+    ICartesianControl * iCartesianControl;
 };
 
 /**
@@ -127,12 +124,10 @@ class RpcResponder : public yarp::dev::DeviceResponder
 class RpcTransformResponder : public RpcResponder
 {
 public:
-    RpcTransformResponder(roboticslab::ICartesianControl * iCartesianControl,
-                          KinRepresentation::coordinate_system coord,
+    RpcTransformResponder(KinRepresentation::coordinate_system coord,
                           KinRepresentation::orientation_system orient,
                           KinRepresentation::angular_units units)
-        : RpcResponder(iCartesianControl),
-          coord(coord),
+        : coord(coord),
           orient(orient),
           units(units)
     {}
@@ -153,9 +148,8 @@ class RpcTransformResponder : public RpcResponder
 class StreamResponder : public yarp::os::TypedReaderCallback<yarp::os::Bottle>
 {
 public:
-    StreamResponder(roboticslab::ICartesianControl * _iCartesianControl)
-        : iCartesianControl(_iCartesianControl)
-    {}
+    void setHandle(ICartesianControl * _iCartesianControl)
+    { iCartesianControl = _iCartesianControl;}
 
     void onRead(yarp::os::Bottle & b) override;
 
@@ -166,7 +160,7 @@ class StreamResponder : public yarp::os::TypedReaderCallback<yarp::os::Bottle>
     void handleConsumerCmdMsg(const yarp::os::Bottle & in, ConsumerFun cmd);
     void handleBiConsumerCmdMsg(const yarp::os::Bottle & in, BiConsumerFun cmd);
 
-    roboticslab::ICartesianControl * iCartesianControl;
+    ICartesianControl * iCartesianControl {nullptr};
 };
 
 } // namespace roboticslab

libraries/YarpPlugins/CartesianControlServer/DeviceDriverImpl.cpp

@@ -40,36 +40,37 @@ bool CartesianControlServer::open(yarp::os::Searchable& config)
 
         if (!cartesianControlDevice.isValid())
         {
-            yCError(CCS) << "Cannot make" << name->toString();
+            yCError(CCS) << "Cartesian control device not valid";
+            return false;
+        }
+
+        if (!cartesianControlDevice.view(iCartesianControl))
+        {
+            yCError(CCS) << "iCartesianControl view failed";
+            return false;
         }
     }
     else
     {
-        yCError(CCS) << "Subdevice option not set in CartesianControlServer";
-        return false;
+        yCInfo(CCS) << "Subdevice option not set, will use attach() later";
     }
 
-    if (!cartesianControlDevice.isValid())
+    auto prefix = config.check("name", yarp::os::Value(DEFAULT_PREFIX), "local port prefix").asString();
+
+    if (!rpcServer.open(prefix + "/rpc:s"))
     {
-        yCError(CCS) << "Cartesian control device not valid";
+        yCError(CCS) << "Failed to open RPC port";
         return false;
     }
 
-    if (!cartesianControlDevice.view(iCartesianControl))
+    if (!commandPort.open(prefix + "/command:i"))
     {
-        yCError(CCS) << "iCartesianControl view failed";
+        yCError(CCS) << "Failed to open command port";
         return false;
     }
 
-    rpcResponder = new RpcResponder(iCartesianControl);
-    streamResponder = new StreamResponder(iCartesianControl);
-
-    auto prefix = config.check("name", yarp::os::Value(DEFAULT_PREFIX), "local port prefix").asString();
-
-    bool ok = true;
-
-    ok &= rpcServer.open(prefix + "/rpc:s");
-    ok &= commandPort.open(prefix + "/command:i");
+    rpcResponder = new RpcResponder();
+    streamResponder = new StreamResponder();
 
     rpcServer.setReader(*rpcResponder);
     commandPort.useCallback(*streamResponder);
@@ -78,15 +79,13 @@ bool CartesianControlServer::open(yarp::os::Searchable& config)
 
     if (periodInMs > 0)
     {
-        fkStreamEnabled = true;
-        ok &= fkOutPort.open(prefix + "/state:o");
-
         yarp::os::PeriodicThread::setPeriod(periodInMs * 0.001);
-        yarp::os::PeriodicThread::start();
-    }
-    else
-    {
-        fkStreamEnabled = false;
+
+        if (!fkOutPort.open(prefix + "/state:o"))
+        {
+            yCError(CCS) << "Failed to open FK stream port";
+            return false;
+        }
     }
 
     yarp::os::Value * angleRepr, * coordRepr, * angularUnits;
@@ -143,22 +142,26 @@ bool CartesianControlServer::open(yarp::os::Searchable& config)
 
     if (openTransformPort)
     {
-        rpcTransformResponder = new RpcTransformResponder(iCartesianControl, coord, orient, units);
-        ok &= rpcTransformServer.open(prefix + "/rpc_transform:s");
+        rpcTransformResponder = new RpcTransformResponder(coord, orient, units);
         rpcTransformServer.setReader(*rpcTransformResponder);
+
+        if (!rpcTransformServer.open(prefix + "/rpc_transform:s"))
+        {
+            yCError(CCS) << "Failed to open transform RPC port";
+            return false;
+        }
     }
 
-    return ok;
+    return !cartesianControlDevice.isValid() || configureHandle();
 }
 
 // -----------------------------------------------------------------------------
 
 bool CartesianControlServer::close()
 {
-    if (fkStreamEnabled)
+    if (!fkOutPort.isClosed())
     {
         yarp::os::PeriodicThread::stop();
-
         fkOutPort.interrupt();
         fkOutPort.close();
     }
@@ -181,7 +184,7 @@ bool CartesianControlServer::close()
     delete streamResponder;
     streamResponder = nullptr;
 
-    return cartesianControlDevice.close();
+    return !cartesianControlDevice.isValid() || cartesianControlDevice.close();
 }
 
 // -----------------------------------------------------------------------------

libraries/YarpPlugins/CartesianControlServer/IWrapperImpl.cpp

@@ -0,0 +1,71 @@
+// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
+
+#include "CartesianControlServer.hpp"
+
+#include <yarp/os/LogStream.h>
+
+#include "LogComponent.hpp"
+
+using namespace roboticslab;
+
+// -----------------------------------------------------------------------------
+
+bool CartesianControlServer::attach(yarp::dev::PolyDriver * poly)
+{
+    if (poly == nullptr)
+    {
+        yCError(CCS) << "attach() received nullptr";
+        return false;
+    }
+
+    if (!poly->isValid())
+    {
+        yCError(CCS) << "attach() received invalid PolyDriver";
+        return false;
+    }
+
+    if (!poly->view(iCartesianControl))
+    {
+        yCError(CCS) << "attach() failed to obtain ICartesianControl interface";
+        return false;
+    }
+
+    return configureHandle();
+}
+
+// -----------------------------------------------------------------------------
+
+bool CartesianControlServer::configureHandle()
+{
+    if (!iCartesianControl)
+    {
+        yCError(CCS) << "Invalid ICartesianControl interface";
+        return false;
+    }
+
+    rpcResponder->setHandle(iCartesianControl);
+    streamResponder->setHandle(iCartesianControl);
+
+    if (rpcTransformResponder)
+    {
+        rpcTransformResponder->setHandle(iCartesianControl);
+    }
+
+    if (!fkOutPort.isClosed())
+    {
+        return yarp::os::PeriodicThread::start();
+    }
+
+    return true;
+}
+
+// -----------------------------------------------------------------------------
+
+bool CartesianControlServer::detach()
+{
+    yarp::os::PeriodicThread::stop();
+    iCartesianControl = nullptr;
+    return true;
+}
+
+// -----------------------------------------------------------------------------

libraries/YarpPlugins/CartesianControlServer/PeriodicThreadImpl.cpp

@@ -2,37 +2,35 @@
 
 #include "CartesianControlServer.hpp"
 
-#include <vector>
-
 using namespace roboticslab;
 
 // ------------------- PeriodicThread related ------------------------------------
 
 void CartesianControlServer::run()
 {
-    std::vector<double> x;
-    int state;
-    double timestamp;
-
-    if (!iCartesianControl->stat(x, &state, &timestamp))
+    if (iCartesianControl)
     {
-        return;
+        std::vector<double> x;
+        int state;
+        double timestamp;
+
+        if (!iCartesianControl->stat(x, &state, &timestamp))
+        {
+            return;
+        }
+
+        yarp::os::Bottle & out = fkOutPort.prepare();
+        out.clear();
+        out.addVocab32(state);
+
+        for (auto i = 0; i < x.size(); i++)
+        {
+            out.addFloat64(x[i]);
+        }
+
+        out.addFloat64(timestamp);
+        fkOutPort.write();
     }
-
-    yarp::os::Bottle & out = fkOutPort.prepare();
-    out.clear();
-    out.addVocab32(state);
-
-    for (size_t i = 0; i < x.size(); i++)
-    {
-        out.addFloat64(x[i]);
-    }
-
-    out.addFloat64(timestamp);
-
-    fkOutPort.write();
-
-    return;
 }
 
 // -----------------------------------------------------------------------------

libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp

@@ -51,6 +51,13 @@ namespace
 
 bool RpcResponder::respond(const yarp::os::Bottle& in, yarp::os::Bottle& out)
 {
+    if (!iCartesianControl)
+    {
+        yCError(CCS) << "Invalid ICartesianControl interface";
+        out.addVocab32(VOCAB_CC_FAILED);
+        return false;
+    }
+
     switch (in.get(0).asVocab32())
     {
     case VOCAB_CC_STAT: