ControlBoardCouplingHandler: configure buffers

Author: Nicogene

src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler.cpp

@@ -47,7 +47,7 @@ bool ControlBoardCouplingHandler::open(Searchable& config)
         return false;
     }
 
-    this->setNrOfControlledAxes(m_axesNames.size());
+    configureBuffers();
 
     Property prop;
     prop.fromString(config.toString());
@@ -85,6 +85,28 @@ struct axisLocation
     int indexInSubDevice;
 };
 
+void ControlBoardCouplingHandler::configureBuffers() {
+    if (!iJntCoupling) {
+        yCError(CONTROLBOARDCOUPLINGHANDLER) << "IJointCoupling interface not available";
+        return;
+    }
+    size_t nrOfPhysicalJoints;
+    size_t nrOfActuatedAxes;
+    auto ok = iJntCoupling->getNrOfPhysicalJoints(nrOfPhysicalJoints);
+    ok = ok && iJntCoupling->getNrOfActuatedAxes(nrOfActuatedAxes);
+    if(!ok)
+    {
+        yCError(CONTROLBOARDCOUPLINGHANDLER) << "Error in getting the number of physical joints or actuated axes";
+        return;
+    }
+    physJointsPos.resize(nrOfPhysicalJoints);
+    physJointsVel.resize(nrOfPhysicalJoints);
+    physJointsAcc.resize(nrOfPhysicalJoints);
+    actAxesPos.resize(nrOfActuatedAxes);
+    actAxesVel.resize(nrOfActuatedAxes);
+    actAxesAcc.resize(nrOfActuatedAxes);
+    return;
+}
 
 bool ControlBoardCouplingHandler::attachAllUsingAxesNames(const PolyDriverList& polylist)
 {
@@ -198,9 +220,6 @@ bool ControlBoardCouplingHandler::detachAll()
     return true;
 }
 
-void ControlBoardCouplingHandler::configureBuffers()
-{
-}
 
 //////////////////////////////////////////////////////////////////////////////
 /// ControlBoard methods

src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler.h

@@ -40,22 +40,19 @@ class ControlBoardCouplingHandler :
     yarp::dev::IAxisInfo             *info;
     yarp::dev::IJointCoupling        *iJntCoupling;
 
-    std::vector<std::string> axesNames;
+    yarp::sig::Vector physJointsPos;
+    yarp::sig::Vector physJointsVel;
+    yarp::sig::Vector physJointsAcc;
+    yarp::sig::Vector actAxesPos;
+    yarp::sig::Vector actAxesVel;
+    yarp::sig::Vector actAxesAcc;
 
     /** Verbosity of the class */
     bool _verb{false};
 
     // to open ports and print more detailed debug messages
     std::string partName;
 
-    /**
-     * Set the number of controlled axes, resizing appropriately
-     * all the necessary buffers.
-     */
-    void setNrOfControlledAxes(const size_t nrOfControlledAxes);
-
-    // TODO I am keeping it just for compiling, but it should be removed
-
     /**
      * Configure buffers used by the device
      */