Merge pull request #3326 from randaz81/get_available

getAvailable: added new interface methods

Author: randaz81

bindings/yarp.i

@@ -1242,20 +1242,20 @@ MAKE_COMMS  (Map2DPath, yarp::dev::Nav2D::Map2DPath)
         return buffer;
     }
 
-    bool getControlModes(std::vector<int>& data) {
-        return self->getControlModes(&data[0]);
+    bool getControlModes(std::vector<yarp::dev::ControlModeEnum>& data) {
+        return self->getControlModes(data);
     }
 
-    bool getControlModes(int n_joint, std::vector<int>& joints, std::vector<int>& data) {
-        return self->getControlModes(n_joint, &joints[0], &data[0]);
+    bool getControlModes(std::vector<int>& joints, std::vector<yarp::dev::ControlModeEnum>& data) {
+        return self->getControlModes(joints, data);
     }
 
-    bool setControlModes(std::vector<int>& data) {
-        return self->setControlModes(&data[0]);
+    bool setControlModes(std::vector<yarp::dev::SelectableControlModeEnum>& data) {
+        return self->setControlModes(data);
     }
 
-    bool setControlModes(int n_joint, std::vector<int>& joints, std::vector<int>& data) {
-        return self->setControlModes(n_joint, &joints[0], &data[0]);
+    bool setControlModes(std::vector<int>& joints, std::vector<yarp::dev::SelectableControlModeEnum>& data) {
+        return self->setControlModes(joints, data);
     }
 }
 

src/devices/controlBoardRemapper/ControlBoardRemapper.cpp

@@ -478,25 +478,36 @@ void ControlBoardRemapper::configureBuffers()
 }
 
 
-bool ControlBoardRemapper::setControlModeAllAxes(const int cm)
-{
-    std::lock_guard<std::mutex> lock(buffers.mutex);
-
-    for(int j=0; j < controlledJoints; j++)
-    {
-        buffers.controlBoardModes[j] = cm;
-    }
-
-    return this->setControlModes(buffers.controlBoardModes.data());
-}
-
 //////////////////////////////////////////////////////////////////////////////
 /// ControlBoard methods
 //////////////////////////////////////////////////////////////////////////////
 
 //
 //  IPid Interface
 //
+
+ReturnValue ControlBoardRemapper::getAvailablePids(int j, std::vector<yarp::dev::PidControlTypeEnum>& avail)
+{
+    int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
+    size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
+
+    RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
+    if (!p) {
+        return ReturnValue::return_code::return_value_error_generic;
+    }
+
+    ReturnValue ok = ReturnValue_ok;
+
+    if (p->pid) {
+        ok = p->pid->getAvailablePids(off, avail);
+    }
+    else {
+        ok = ReturnValue::return_code::return_value_error_generic;
+    }
+
+    return ok;
+}
+
 ReturnValue ControlBoardRemapper::setPid(const PidControlTypeEnum& pidtype, int j, const Pid &p)
 {
     int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
@@ -2737,7 +2748,7 @@ ReturnValue ControlBoardRemapper::enableAmp(int j)
 
 ReturnValue ControlBoardRemapper::disableAmp(int j)
 {
-    return this->setControlMode(j, VOCAB_CM_IDLE);
+    return this->setControlMode(j, yarp::dev::SelectableControlModeEnum::VOCAB_CM_IDLE);
 }
 
 ReturnValue ControlBoardRemapper::getAmpStatus(int *st)
@@ -3739,7 +3750,29 @@ ReturnValue ControlBoardRemapper::getCurrentImpedanceLimit(int j, double *min_st
     return ReturnValue::return_code::return_value_error_generic;
 }
 
-ReturnValue ControlBoardRemapper::getControlMode(int j, int *mode)
+ReturnValue ControlBoardRemapper::getAvailableControlModes(int j, std::vector<yarp::dev::SelectableControlModeEnum>& avail)
+{
+    int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
+    size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
+
+    RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(subIndex);
+    if (!p) {
+        return ReturnValue::return_code::return_value_error_generic;
+    }
+
+    ReturnValue ok = ReturnValue_ok;
+
+    if (p->iMode) {
+        ok = p->iMode->getAvailableControlModes(off, avail);
+    }
+    else {
+        ok = ReturnValue::return_code::return_value_error_generic;
+    }
+
+    return ok;
+}
+
+ReturnValue ControlBoardRemapper::getControlMode(int j, yarp::dev::ControlModeEnum& mode)
 {
     int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
     size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex;
@@ -3762,7 +3795,7 @@ ReturnValue ControlBoardRemapper::getControlMode(int j, int *mode)
 
 }
 
-ReturnValue ControlBoardRemapper::getControlModes(int *modes)
+ReturnValue ControlBoardRemapper::getControlModes(std::vector<yarp::dev::ControlModeEnum>& modes)
 {
     ReturnValue ret=ReturnValue_ok;
     std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
@@ -3775,9 +3808,8 @@ ReturnValue ControlBoardRemapper::getControlModes(int *modes)
 
         if( p->iMode )
         {
-            ok = p->iMode->getControlModes(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
-                                            allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
-                                            allJointsBuffers.m_bufferForSubControlBoardControlModes[ctrlBrd].data());
+            ok = p->iMode->getControlModes( allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
+                                            allJointsBuffers.m_bufferForSubControlBoardControlModesEnum[ctrlBrd]);
         }
         else
         {
@@ -3787,19 +3819,19 @@ ReturnValue ControlBoardRemapper::getControlModes(int *modes)
         ret = ret && ok;
     }
 
-    allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(modes,remappedControlBoards);
+    allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(modes.data(),remappedControlBoards);
 
     return ret;
 }
 
 // IControlMode interface
-ReturnValue ControlBoardRemapper::getControlModes(const int n_joints, const int *joints, int *modes)
+ReturnValue ControlBoardRemapper::getControlModes(std::vector<int> joints, std::vector<yarp::dev::ControlModeEnum>& modes)
 {
     ReturnValue ret=ReturnValue_ok;
     std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
 
     // Resize the input buffers
-    selectedJointsBuffers.resizeSubControlBoardBuffers(n_joints,joints,remappedControlBoards);
+    selectedJointsBuffers.resizeSubControlBoardBuffers(joints.size(),joints.data(),remappedControlBoards);
 
     for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
     {
@@ -3809,9 +3841,8 @@ ReturnValue ControlBoardRemapper::getControlModes(const int n_joints, const int
 
         if( p->iMode )
         {
-            ok = p->iMode->getControlModes(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
-                                            selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
-                                            selectedJointsBuffers.m_bufferForSubControlBoardControlModes[ctrlBrd].data());
+            ok = p->iMode->getControlModes( selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
+                                            selectedJointsBuffers.m_bufferForSubControlBoardControlModesEnum[ctrlBrd]);
         }
         else
         {
@@ -3821,12 +3852,12 @@ ReturnValue ControlBoardRemapper::getControlModes(const int n_joints, const int
         ret = ret && ok;
     }
 
-    selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(modes,n_joints,joints,remappedControlBoards);
+    selectedJointsBuffers.fillArbitraryJointVectorFromSubControlBoardBuffers(modes.data(),joints.size(),joints.data(),remappedControlBoards);
 
     return ret;
 }
 
-ReturnValue ControlBoardRemapper::setControlMode(const int j, const int mode)
+ReturnValue ControlBoardRemapper::setControlMode(int j, yarp::dev::SelectableControlModeEnum mode)
 {
     ReturnValue ret = ReturnValue_ok;
 
@@ -3852,22 +3883,21 @@ ReturnValue ControlBoardRemapper::setControlMode(const int j, const int mode)
     return ret;
 }
 
-ReturnValue ControlBoardRemapper::setControlModes(const int n_joints, const int *joints, int *modes)
+ReturnValue ControlBoardRemapper::setControlModes(std::vector<int> joints, std::vector<yarp::dev::SelectableControlModeEnum> modes)
 {
     ReturnValue ret=ReturnValue_ok;
     std::lock_guard<std::mutex> lock(selectedJointsBuffers.mutex);
 
-    selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(modes,n_joints,joints,remappedControlBoards);
+    selectedJointsBuffers.fillSubControlBoardBuffersFromArbitraryJointVector(modes.data(),joints.size(),joints.data(),remappedControlBoards);
 
     for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
     {
         RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
 
         ReturnValue ok = ReturnValue_ok;
         if (p->iMode) {
-            ok = p->iMode->setControlModes(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
-                                             selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
-                                             selectedJointsBuffers.m_bufferForSubControlBoardControlModes[ctrlBrd].data());
+            ok = p->iMode->setControlModes(  selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
+                                             selectedJointsBuffers.m_bufferForSubControlBoardSelectableControlModesEnum[ctrlBrd]);
         }
         else {
             ok = ReturnValue::return_code::return_value_error_generic;
@@ -3878,22 +3908,21 @@ ReturnValue ControlBoardRemapper::setControlModes(const int n_joints, const int
     return ret;
 }
 
-ReturnValue ControlBoardRemapper::setControlModes(int *modes)
+ReturnValue ControlBoardRemapper::setControlModes(std::vector<yarp::dev::SelectableControlModeEnum> modes)
 {
     ReturnValue ret=ReturnValue_ok;
     std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);
 
-    allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(modes,remappedControlBoards);
+    allJointsBuffers.fillSubControlBoardBuffersFromCompleteJointVector(modes.data(),remappedControlBoards);
 
     for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
     {
         RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);
 
         ReturnValue ok = ReturnValue_ok;
         if (p->iMode) {
-            ok = p->iMode->setControlModes(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
-                                             allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
-                                             allJointsBuffers.m_bufferForSubControlBoardControlModes[ctrlBrd].data());
+            ok = p->iMode->setControlModes(allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd],
+                                           allJointsBuffers.m_bufferForSubControlBoardSelectableControlModesEnum[ctrlBrd]);
         }
         else {
             ok = ReturnValue::return_code::return_value_error_generic;

src/devices/controlBoardRemapper/ControlBoardRemapper.h

@@ -167,13 +167,6 @@ class ControlBoardRemapper :
      */
     bool attachAllUsingAxesNames(const yarp::dev::PolyDriverList &l);
 
-    /**
-     * Helper for setting the same control mode in all the axes
-     * of the controlboard.
-     */
-    bool setControlModeAllAxes(const int cm);
-
-
 public:
     ControlBoardRemapper() = default;
     ControlBoardRemapper(const ControlBoardRemapper&) = delete;
@@ -207,6 +200,7 @@ class ControlBoardRemapper :
     bool attachAll(const yarp::dev::PolyDriverList &l) override;
 
     /* IPidControl */
+    yarp::dev::ReturnValue getAvailablePids(int j, std::vector<yarp::dev::PidControlTypeEnum>& avail) override;
     yarp::dev::ReturnValue setPid(const yarp::dev::PidControlTypeEnum& pidtype,int j, const yarp::dev::Pid &p) override;
     yarp::dev::ReturnValue setPids(const yarp::dev::PidControlTypeEnum& pidtype,const yarp::dev::Pid *ps) override;
     yarp::dev::ReturnValue setPidReference(const yarp::dev::PidControlTypeEnum& pidtype,int j, double ref) override;
@@ -378,14 +372,17 @@ class ControlBoardRemapper :
     yarp::dev::ReturnValue getImpedance(int j, double *stiff, double *damp) override;
     yarp::dev::ReturnValue getImpedanceOffset(int j, double *offset) override;
     yarp::dev::ReturnValue getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override;
-    yarp::dev::ReturnValue getControlMode(int j, int *mode) override;
-    yarp::dev::ReturnValue getControlModes(int *modes) override;
 
     // IControlMode interface
-    yarp::dev::ReturnValue getControlModes(const int n_joint, const int *joints, int *modes) override;
-    yarp::dev::ReturnValue setControlMode(const int j, const int mode) override;
-    yarp::dev::ReturnValue setControlModes(const int n_joints, const int *joints, int *modes) override;
-    yarp::dev::ReturnValue setControlModes(int *modes) override;
+    yarp::dev::ReturnValue getAvailableControlModes(int j, std::vector<yarp::dev::SelectableControlModeEnum>& avail) override;
+    yarp::dev::ReturnValue getControlMode(int j, yarp::dev::ControlModeEnum& mode) override;
+    yarp::dev::ReturnValue getControlModes(std::vector<int> joints, std::vector<yarp::dev::ControlModeEnum>& modes) override;
+    yarp::dev::ReturnValue getControlModes(std::vector<yarp::dev::ControlModeEnum>& mode) override;
+    yarp::dev::ReturnValue setControlMode(int j, yarp::dev::SelectableControlModeEnum mode) override;
+    yarp::dev::ReturnValue setControlModes(std::vector<int> joints, std::vector<yarp::dev::SelectableControlModeEnum> modes) override;
+    yarp::dev::ReturnValue setControlModes(std::vector<yarp::dev::SelectableControlModeEnum> mode) override;
+
+    // IPositionDirect interface
     yarp::dev::ReturnValue setPosition(int j, double ref) override;
     yarp::dev::ReturnValue setPositions(const int n_joints, const int *joints, const double *dpos) override;
     yarp::dev::ReturnValue setPositions(const double *refs) override;