Added implementation for ControlBoardRemapper device

robotology · Nov 18, 2021 · 2584562 · 2584562
1 parent 09f8953
commit 2584562
Show file tree

Hide file tree

Showing 6 changed files with 44 additions and 2 deletions.
diff --git a/doc/release/master/yarpmotogui_hwfault.md b/doc/release/master/yarpmotogui_hwfault.md
@@ -16,6 +16,10 @@ yarpmotorgui_hwfault {#master}
 
 * ControlBoard_nws_yarp now implements IJointFault
 
+#### ControlBoardRemapper
+
+* ControlBoardRemapper now implements IJointFault
+
 #### fakeMotionControl
 
 * fakeMotionControl now simulates an hardware fault when it receives a TorqueCommand > 1Nm

diff --git a/src/devices/ControlBoardRemapper/ControlBoardRemapper.cpp b/src/devices/ControlBoardRemapper/ControlBoardRemapper.cpp
@@ -1623,6 +1623,26 @@ bool ControlBoardRemapper::stop(const int n_joints, const int *joints)
     return ret;
 }
 
+/* IJointFaultControl */
+bool ControlBoardRemapper::getLastJointFault(int j, int& fault, std::string& message)
+{
+    int off = (int)remappedControlBoards.lut[j].axisIndexInSubControlBoard;
+    size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex;
+
+    RemappedSubControlBoard* p = remappedControlBoards.getSubControlBoard(subIndex);
+
+    if (!p)
+    {
+        return false;
+    }
+
+    if (p->iFault)
+    {
+        return p->iFault->getLastJointFault(off, fault, message);
+    }
+
+    return false;
+}
 
 /* IVelocityControl */
 

diff --git a/src/devices/ControlBoardRemapper/ControlBoardRemapper.h b/src/devices/ControlBoardRemapper/ControlBoardRemapper.h
@@ -88,7 +88,8 @@ class ControlBoardRemapper :
         public yarp::dev::IAxisInfo,
         public yarp::dev::IPreciselyTimed,
         public yarp::dev::IInteractionMode,
-        public yarp::dev::IRemoteVariables {
+        public yarp::dev::IRemoteVariables,
+        public yarp::dev::IJointFault {
 private:
     std::vector<std::string> axesNames;
     RemappedControlBoards remappedControlBoards;
@@ -302,6 +303,10 @@ class ControlBoardRemapper :
 
     bool stop(const int n_joints, const int *joints) override;
 
+    /* IJointFault */
+
+    bool getLastJointFault(int j, int& fault, std::string& message) override;
+
     /* IVelocityControl */
     bool velocityMove(int j, double v) override;
 

diff --git a/src/devices/ControlBoardRemapper/ControlBoardRemapperHelpers.cpp b/src/devices/ControlBoardRemapper/ControlBoardRemapperHelpers.cpp
@@ -112,6 +112,7 @@ bool RemappedSubControlBoard::attach(yarp::dev::PolyDriver *d, const std::string
         subdevice->view(iVar);
         subdevice->view(iPwm);
         subdevice->view(iCurr);
+        subdevice->view(iFault);
     }
     else
     {
@@ -164,8 +165,13 @@ bool RemappedSubControlBoard::attach(yarp::dev::PolyDriver *d, const std::string
         yCWarning(CONTROLBOARDREMAPPER) << "ICurrentControl not valid interface";
     }
 
+    if ((iFault == nullptr) && (_subDevVerbose))
+    {
+        yCWarning(CONTROLBOARDREMAPPER) << "IJointFault not valid interface";
+    }
+
 
-    // checking minimum set of intefaces required
+    // checking minimum set of interfaces required
     if( !(pos) )
     {
         yCError(CONTROLBOARDREMAPPER, "IPositionControl interface was not found in subdevice. Quitting");

diff --git a/src/devices/ControlBoardRemapper/ControlBoardRemapperHelpers.h b/src/devices/ControlBoardRemapper/ControlBoardRemapperHelpers.h
@@ -64,6 +64,7 @@ class RemappedSubControlBoard
     yarp::dev::IRemoteVariables      *iVar;
     yarp::dev::IPWMControl           *iPwm;
     yarp::dev::ICurrentControl       *iCurr;
+    yarp::dev::IJointFault           *iFault;
 
     RemappedSubControlBoard();
 

diff --git a/src/yarpmotorgui/jointitem.cpp b/src/yarpmotorgui/jointitem.cpp
@@ -428,6 +428,7 @@ void JointItem::setUnits(yarp::dev::JointTypeEnum t)
         ui->labelPWMposUnits->setText(pos_metric_revolute);
         ui->labelCurrentposUnits->setText(pos_metric_revolute);
         ui->labelVelocityposUnits->setText(pos_metric_revolute);
+        ui->labelFaultposUnits->setText(pos_metric_revolute);
 
         ui->labelIdletrqUnits->setText(trq_metric_revolute);
         ui->labelPositiontrqUnits->setText(trq_metric_revolute);
@@ -466,6 +467,7 @@ void JointItem::setUnits(yarp::dev::JointTypeEnum t)
         ui->labelPWMposUnits->setText(pos_metric_prism);
         ui->labelCurrentposUnits->setText(pos_metric_prism);
         ui->labelVelocityposUnits->setText(pos_metric_prism);
+        ui->labelFaultposUnits->setText(pos_metric_prism);
 
         ui->labelIdletrqUnits->setText(trq_metric_prism);
         ui->labelPositiontrqUnits->setText(trq_metric_prism);
@@ -1487,6 +1489,10 @@ void JointItem::setPosition(double val)
         ui->editIdleJointPos->setText(sVal);
     }
 
+    if (ui->stackedWidget->currentIndex() == HW_FAULT) {
+        ui->editFaultJointPos->setText(sVal);
+    }
+
     if(ui->stackedWidget->currentIndex() == POSITION){
         ui->editPositionJointPos->setText(sVal);
         updateSliderPosition(ui->sliderTrajectoryPosition, val);