Merge pull request #47 from robotology/remapper_permissive

The remapper connector will not fail if some board of joint is not available

src/lib/RobotConnectors/RobotConnectors.cpp

@@ -142,6 +142,94 @@ bool RemapperConnector::getOrGuessControlBoardsFromFile(const yarp::os::Searchab
     return true;
 }
 
+bool RemapperConnector::addJointsFromBoard(const std::string &name,
+                                           const std::string &robot,
+                                           const std::string &controlBoard,
+                                           const std::unordered_map<std::string, size_t>& desiredJointsMap)
+{
+    yarp::os::Property rcb_conf{
+        {"device", yarp::os::Value("remote_controlboard")},
+        {"local", yarp::os::Value("/" + name + "/" + controlBoard + "/remoteControlBoard")},
+        {"remote", yarp::os::Value("/" + robot + "/" + controlBoard)},
+        {"part", yarp::os::Value(controlBoard)}};
+
+    yarp::dev::PolyDriver m_driver;
+
+    if (!m_driver.open(rcb_conf))
+    {
+        return false;
+    }
+
+    yarp::dev::IAxisInfo* axisInfo{nullptr};
+    yarp::dev::IEncodersTimed* pos{nullptr};
+
+    if (!m_driver.view(pos) || !pos)
+    {
+        return false;
+    }
+
+    if (!m_driver.view(axisInfo) || !axisInfo)
+    {
+        return false;
+    }
+
+    int nAxes = 0;
+
+    if (!pos->getAxes(&nAxes))
+    {
+        return false;
+    }
+
+    for (size_t i = 0; i < nAxes; ++i)
+    {
+        std::string name;
+        if (axisInfo->getAxisName(i,name))
+        {
+            auto it = desiredJointsMap.find(name);
+            if (it != desiredJointsMap.end())
+            {
+                m_availableJoints.push_back({name, it->second});
+            }
+        }
+    }
+
+    return true;
+}
+
+void RemapperConnector::getAvailableJoints(const std::string &name,
+                                           const std::string &robot,
+                                           const std::vector<std::string>& desiredJoints)
+{
+    m_availableControlBoards.clear();
+    m_availableJoints.clear();
+
+    std::unordered_map<std::string, size_t> desiredJointsMap;
+
+    for (size_t i = 0; i < m_basicInfo->jointList.size(); ++i)
+    {
+        desiredJointsMap[m_basicInfo->jointList[i]] = i;
+    }
+
+    for (const std::string& cb : m_controlBoards)
+    {
+        if (addJointsFromBoard(name, robot, cb, desiredJointsMap))
+        {
+            m_availableControlBoards.push_back(cb);
+        }
+    }
+
+    m_availableJointsInDeg.resize(m_availableJoints.size());
+    m_availableJointsInDeg.zero();
+}
+
+void RemapperConnector::fillDesiredJointsInDeg()
+{
+    for (size_t i = 0; i < m_availableJoints.size(); ++i)
+    {
+        m_jointsInDeg[m_availableJoints[i].desiredPosition] = m_availableJointsInDeg[i];
+    }
+}
+
 bool RemapperConnector::configure(const yarp::os::Searchable &inputConf, const iDynTree::Model &fullModel, std::shared_ptr<BasicInfo> basicInfo)
 {
     std::lock_guard<std::mutex> lock(m_mutex);
@@ -176,23 +264,38 @@ bool RemapperConnector::connectToRobot()
         }
 
         std::lock_guard<std::mutex> lock(m_basicInfo->mutex);
+
+        getAvailableJoints(m_basicInfo->name, m_basicInfo->robotPrefix, m_basicInfo->jointList);
+
         //First make sure to reset the device in case it was already opened.
         m_robotDevice.close();
         m_encodersInterface = nullptr;
 
+        if (m_availableControlBoards.size() == 0)
+        {
+            yError() << "No available boards.";
+            return false;
+        }
+
+        if (m_availableJoints.size() == 0)
+        {
+            yError() << "No available joints.";
+            return false;
+        }
+
         yarp::os::Property remapperOptions;
         remapperOptions.put("device", "remotecontrolboardremapper");
         yarp::os::Bottle axesNames;
         yarp::os::Bottle & axesList = axesNames.addList();
-        for (auto& joint : m_basicInfo->jointList)
+        for (auto& joint : m_availableJoints)
         {
-            axesList.addString(joint);
+            axesList.addString(joint.name);
         }
         remapperOptions.put("axesNames",axesNames.get(0));
 
         yarp::os::Bottle remoteControlBoards;
         yarp::os::Bottle & remoteControlBoardsList = remoteControlBoards.addList();
-        for (auto& cb : m_controlBoards)
+        for (auto& cb : m_availableControlBoards)
         {
             remoteControlBoardsList.addString("/" + m_basicInfo->robotPrefix + "/" + cb);
         }
@@ -204,12 +307,33 @@ bool RemapperConnector::connectToRobot()
             yError() << "Failed to open remote control board remapper.";
             return false;
         }
+
         if (!m_robotDevice.view(m_encodersInterface) || !m_encodersInterface)
         {
             yError() << "Failed to view encoder interface.";
 
             return false;
         }
+
+        std::stringstream boardsInfo;
+
+        boardsInfo << "Connected to the following boards:" <<std::endl;
+        for (const std::string& cb : m_availableControlBoards)
+        {
+            boardsInfo << "- " << cb << std::endl;
+        }
+
+        yInfo() << boardsInfo.str();
+
+        std::stringstream jointsInfo;
+        jointsInfo << "Connected to the following joints:" <<std::endl;
+        for (const Joint& joint : m_availableJoints)
+        {
+            jointsInfo << "- " << joint.name << std::endl;
+        }
+
+        yInfo() << jointsInfo.str();
+
     }
 
     m_connected = true;
@@ -222,8 +346,9 @@ bool RemapperConnector::getJointValues(iDynTree::VectorDynSize &jointValuesInRad
     {
         std::lock_guard<std::mutex> lock(m_mutex);
 
-        if (m_encodersInterface->getEncoders(m_jointsInDeg.data()))
+        if (m_encodersInterface->getEncoders(m_availableJointsInDeg.data()))
         {
+            fillDesiredJointsInDeg();
             fillJointValuesInRad();
             jointValuesInRad = m_jointsInRad;
         }

src/lib/RobotConnectors/RobotConnectors.h

@@ -23,6 +23,7 @@
 #include <memory>
 #include <functional>
 #include <unordered_map>
+#include <map>
 
 
 
@@ -76,13 +77,29 @@ class BasicConnector
 
 class RemapperConnector : public BasicConnector
 {
+    struct Joint {
+        std::string name;
+        size_t desiredPosition;
+    };
 
     std::vector<std::string> m_controlBoards;
+    std::vector<std::string> m_availableControlBoards;
+    std::vector<Joint> m_availableJoints;
+    iDynTree::VectorDynSize m_availableJointsInDeg;
     yarp::dev::PolyDriver m_robotDevice;
     yarp::dev::IEncodersTimed *m_encodersInterface{nullptr};
 
     bool getOrGuessControlBoardsFromFile(const yarp::os::Searchable &inputConf);
 
+    bool addJointsFromBoard(const std::string& name,
+                            const std::string& robot,
+                            const std::string& controlBoard,
+                            const std::unordered_map<std::string, size_t> &desiredJointsMap);
+
+    void getAvailableJoints(const std::string& name, const std::string& robot, const std::vector<std::string> &desiredJoints);
+
+    void fillDesiredJointsInDeg();
+
 public:
 
     bool configure(const yarp::os::Searchable &inputConf, const iDynTree::Model& fullModel, std::shared_ptr<BasicInfo> basicInfo);