Remove support for reading joint positions from ROS 1's `/joint_state…

…s` topic (#52)

* Remove support for reading joint positions from ROS 1's /joint_states topic

* Add back idl_tools YARP component

.github/workflows/conda-ci.yml

@@ -32,8 +32,7 @@ jobs:
       run: |
         # Workaround for https://github.com/conda-incubator/setup-miniconda/issues/186
         conda config --remove channels defaults
-        # We pin yarp to 3.9.* until https://github.com/robotology/idyntree-yarp-tools/issues/50 is fixed
-        conda install -c conda-forge -c robotology cmake compilers make ninja pkg-config qt-main yarp==3.9.* icub-main idyntree
+        conda install -c conda-forge -c robotology cmake compilers make ninja pkg-config qt-main yarp icub-main idyntree
 
     # Additional dependencies useful only on Linux
     - name: Dependencies [Conda/Linux]

CMakeLists.txt

@@ -61,7 +61,7 @@ set(CMAKE_CXX_EXTENSIONS OFF)
 
 
 find_package(YCM REQUIRED)
-find_package(YARP 3.3 REQUIRED COMPONENTS os dev math rosmsg idl_tools)
+find_package(YARP 3.3 REQUIRED COMPONENTS os dev math idl_tools)
 find_package(iDynTree 5.0.0 REQUIRED)
 find_package(Eigen3 REQUIRED)
 

README.md

@@ -12,7 +12,7 @@ Module to visualize a robot using the iDynTree Visualizer library, loading the r
 ### `yarprobotstatepublisher`
 
 Drop-in replacement of the [ROS `robot_state_publisher`](http://wiki.ros.org/robot_state_publisher) that uses iDynTree for loading the URDF
-and computing the forward kinematics, and YARP to subscribe to the joint topic and publish the computed transforms.
+and computing the forward kinematics, and YARP to read the joint positions and publish the computed transforms.
 
 ### `urdf2dh`
 

src/lib/RobotConnectors/CMakeLists.txt

@@ -5,7 +5,6 @@ set (SRC RobotConnectors.cpp)
 
 add_library(${LIBRARY_NAME} ${HDR} ${SRC})
 target_link_libraries(${LIBRARY_NAME} PRIVATE YARP::YARP_os
-                                              YARP::YARP_rosmsg
                                               YARP::YARP_dev
                                               ReadOnlyRemoteControlBoardLib)
 

src/lib/RobotConnectors/RobotConnectors.cpp

@@ -98,11 +98,6 @@ ConnectionType BasicConnector::RequestedType(const yarp::os::Searchable &inputCo
         return ConnectionType::REMAPPER;
     }
 
-    if (inputConf.check("connectToJointState"))
-    {
-        return ConnectionType::JOINT_STATE;
-    }
-
     return defaultType;
 }
 
@@ -770,163 +765,3 @@ StateExtConnector::EncodersInterface::~EncodersInterface()
 }
 
 /************************************************************/
-JointStateConnector::JointStateSubscriber::JointStateSubscriber()
-{
-
-}
-
-JointStateConnector::JointStateSubscriber::~JointStateSubscriber()
-{
-    std::lock_guard<std::mutex> lock(m_mutex);
-}
-
-void JointStateConnector::JointStateSubscriber::attach(JointStateConnector *connector)
-{
-    std::lock_guard<std::mutex> lock(m_mutex);
-    m_connector = connector;
-}
-
-void JointStateConnector::JointStateSubscriber::onRead(yarp::rosmsg::sensor_msgs::JointState& v)
-{
-    yarp::os::Subscriber<yarp::rosmsg::sensor_msgs::JointState>::onRead(v);
-
-    std::lock_guard<std::mutex> lock(m_mutex);
-
-    if (m_connector)
-        m_connector->onRead(v);
-}
-
-void JointStateConnector::onRead(yarp::rosmsg::sensor_msgs::JointState &v)
-{
-    {
-        std::lock_guard<std::mutex> lock(m_mutex);
-
-        if (!m_connected)
-            return;
-
-        for (size_t i=0; i < v.name.size(); i++)
-        {
-
-            auto jointIndex_it = m_nameToIndexMap.find(v.name[i]);
-            if (jointIndex_it != m_nameToIndexMap.end())
-            {
-                m_jointsInRad(jointIndex_it->second) = v.position[i];
-            }
-        }
-    }
-
-    {
-        std::lock_guard<std::mutex> lock(m_callbackMutex);
-
-        if (!m_connected)
-            return;
-
-        //Update the joint values
-        if (m_callback)
-        {
-            m_callback();
-        }
-    }
-}
-
-void JointStateConnector::setCallback(std::function<void ()> callback)
-{
-    std::lock_guard<std::mutex> lock(m_callbackMutex);
-
-    m_callback = callback;
-}
-
-bool JointStateConnector::configure(const yarp::os::Searchable &inputConf, const iDynTree::Model &fullModel, std::shared_ptr<BasicInfo> basicInfo)
-{
-    std::lock_guard<std::mutex> lock(m_mutex);
-
-    m_basicInfo = basicInfo;
-
-    if (!m_basicInfo)
-    {
-        yError() << "Basic info pointer not set.";
-        return false;
-    }
-
-    m_namePrefix = inputConf.check("name-prefix", yarp::os::Value("")).asString();
-    m_jointStatesTopicName = inputConf.check("jointstates-topic", yarp::os::Value("/joint_states")).asString();
-
-    if (!getJointNamesFromModel(inputConf, fullModel))
-    {
-        yError() << "Failed to get the list of joints.";
-        return false;
-    }
-
-    {
-        std::lock_guard<std::mutex> lock(m_basicInfo->mutex);
-        for (size_t i = 0; i < m_basicInfo->jointList.size(); ++i)
-        {
-            m_nameToIndexMap[m_basicInfo->jointList[i]] = i;
-        }
-    }
-
-    return true;
-}
-
-bool JointStateConnector::connectToRobot()
-{
-    this->close();
-    {
-        std::lock_guard<std::mutex> lock(m_mutex);
-
-        std::string name;
-        {
-            std::lock_guard<std::mutex> lock(m_basicInfo->mutex);
-            name = m_basicInfo->name;
-        }
-
-        if (!m_namePrefix.empty()) {
-            m_rosNode = std::make_unique<yarp::os::Node>("/"+m_namePrefix+"/" +name);
-        }
-        else {
-            m_rosNode = std::make_unique<yarp::os::Node>("/" + name);
-        }
-        // Setup the topic and configureisValid the onRead callback
-        m_subscriber = std::make_unique<JointStateSubscriber>();
-        m_subscriber->attach(this);
-
-        if (!m_subscriber->topic(m_jointStatesTopicName))
-        {
-            return false;
-        }
-        m_subscriber->useCallback();
-    }
-
-    m_connected = true;
-    return true;
-}
-
-bool JointStateConnector::getJointValues(iDynTree::VectorDynSize &jointValuesInRad)
-{
-    if (!m_connected)
-        return false;
-
-    std::lock_guard<std::mutex> lock(m_mutex);
-    jointValuesInRad = m_jointsInRad;
-
-    return true;
-}
-
-void JointStateConnector::close()
-{
-    m_connected = false;
-    {
-        std::lock_guard<std::mutex> lockCallback(m_callbackMutex);
-        std::lock_guard<std::mutex> lock(m_mutex);
-        if (m_subscriber)
-        {
-            m_subscriber->interrupt();
-            m_subscriber->disableCallback();
-            m_subscriber->close();
-        }
-        m_rosNode = nullptr;
-        m_subscriber = nullptr;
-    }
-}
-
-/************************************************************/

src/lib/RobotConnectors/RobotConnectors.h

@@ -10,7 +10,6 @@
 
 #include <yarp/dev/PolyDriver.h>
 #include <yarp/dev/IEncodersTimed.h>
-#include <yarp/rosmsg/sensor_msgs/JointState.h>
 #include <yarp/os/Node.h>
 #include <yarp/os/Subscriber.h>
 #include <iDynTree/Core/VectorDynSize.h>
@@ -32,8 +31,7 @@ namespace idyntree_yarp_tools {
 enum class ConnectionType
 {
     REMAPPER,
-    STATE_EXT,
-    JOINT_STATE
+    STATE_EXT
 };
 
 struct BasicInfo
@@ -162,63 +160,6 @@ class StateExtConnector : public BasicConnector
 
 /************************************************************/
 
-class JointStateConnector: public BasicConnector
-{
-private:
-
-    class JointStateSubscriber: public yarp::os::Subscriber<yarp::rosmsg::sensor_msgs::JointState>
-    {
-    private:
-        // Mutex protecting the method across the different threads
-        std::mutex m_mutex;
-
-        JointStateConnector* m_connector{nullptr};
-
-    public:
-        JointStateSubscriber();
-
-        ~JointStateSubscriber();
-
-        JointStateSubscriber(const JointStateSubscriber&) = delete;
-        JointStateSubscriber(JointStateSubscriber&&) = delete;
-        JointStateSubscriber& operator=(const JointStateSubscriber&) = delete;
-        JointStateSubscriber& operator=(JointStateSubscriber&&) = delete;
-
-        void attach(JointStateConnector* connector);
-
-        virtual void onRead(yarp::rosmsg::sensor_msgs::JointState &v) override;
-    };
-
-    // Mutex protecting the method across the different threads
-    std::mutex m_mutex;
-    std::mutex m_callbackMutex;
-
-    std::function<void()> m_callback;
-
-    // /JointState topic scruscriber
-    std::unique_ptr<yarp::os::Node> m_rosNode{nullptr};
-    std::unique_ptr<JointStateSubscriber> m_subscriber{nullptr};
-    std::string m_jointStatesTopicName;
-    std::string m_namePrefix;
-    std::unordered_map<std::string, size_t> m_nameToIndexMap;
-    std::atomic<bool> m_connected{false};
-
-    void onRead(yarp::rosmsg::sensor_msgs::JointState &v);
-
-public:
-    void setCallback(std::function<void()> callback);
-
-    bool configure(const yarp::os::Searchable &inputConf, const iDynTree::Model& fullModel, std::shared_ptr<BasicInfo> basicInfo);
-
-    virtual bool connectToRobot() override;
-
-    virtual bool getJointValues(iDynTree::VectorDynSize& jointValuesInRad) override;
-
-    virtual void close() override;
-};
-
-/************************************************************/
-
 }
 
 #endif //  IDYNTREE_YARP_ROBOTCONNECTORS_H

src/modules/idyntree-yarp-visualizer/Visualizer.cpp

@@ -418,20 +418,6 @@ bool idyntree_yarp_tools::Visualizer::configure(const yarp::os::ResourceFinder &
         break;
     }
 
-    case ConnectionType::JOINT_STATE:
-    {
-        std::shared_ptr<JointStateConnector> jointStateConnector = std::make_shared<JointStateConnector>();
-
-        if (!jointStateConnector->configure(rf, m_modelLoader.model(), m_basicInfo))
-        {
-            yError() << "Failed to configure the module to connect to the robot via JointState.";
-            return false;
-        }
-
-        m_robotConnector = jointStateConnector;
-        break;
-    }
-
     default:
         yError() << "The specified connector is not available for this module.";
         return false;
@@ -704,8 +690,6 @@ bool idyntree_yarp_tools::Visualizer::neededHelp(const yarp::os::ResourceFinder
                   << "                                                   In case --robot is \"icub\" or \"icubSim\" it is possible to use the following simplified syntax to connect to all the supported joints:" << std::endl
                   << "                                                   --connectToStateExt default" << std::endl
                   << "                                                   When using connectToStateExt, the --controlboards and --joints options are ignored;" << std::endl << std::endl
-                  << "--connectToJointState                              Specify to connect to jointState topic. The --controlboards option is ignored." << std::endl << std::endl
-                  << "--jointstates-topic                                Specify the topic name to connect when using --connectToJointState. Default: /joint_states" << std::endl << std::endl
                   << "--noNetExternalWrenches                            Avoid connecting to WholeBodyDynamics to retrieve the net external wrenches applied on the robot link;" << std::endl << std::endl
                   << "--netExternalWrenchesPortName <name>               The name of the WholeBodyDynamics port to retrieve the net external wrenches." << std::endl
                   << "                                                   The port is supposed to send a bottle of n pairs, where n is the number of links." << std::endl

src/modules/yarprobotstatepublisher/src/main.cpp

@@ -41,10 +41,6 @@ int main(int argc, char *argv[])
         cout<<"\t--reduced-model                        : use the option to stream only the link TFs\n"
               "                                           \t By default TFs of all the frames in the model are streamed"<<endl;
         cout<<"\t--base-frame             <frame-name>  : specify the base frame of the published tf tree"<<endl;
-        cout<<"\t--jointstates-topic      <topic-name>  : source ROS topic that streams the joint state (default /joint_states)\n"
-              "                                           \t The position values of the model joints are initilized to Zero\n"
-              "                                           \t In runtime, the joint values from the ROS topic are used to set\n"
-              "                                           \t the position values of some of the model joints."<<endl;
         cout<<"\t--tree-type              <tree-type>   : the type of tree tou want to represent the transformations in.\n"
               "                                           \t the values can be DEEP or SHALLOW. If omitted, the default\n"
               "                                           \t value SHALLOW will be used"<<endl;

src/modules/yarprobotstatepublisher/src/robotstatepublisher.cpp

@@ -103,7 +103,7 @@ bool YARPRobotStatePublisherModule::configure(yarp::os::ResourceFinder &rf)
     iDynTree::ModelLoader modelLoader;
     bool ok = modelLoader.loadModelFromFile(pathToModel);
 
-    ConnectionType connection = BasicConnector::RequestedType(rf, ConnectionType::JOINT_STATE);
+    ConnectionType connection = BasicConnector::RequestedType(rf, ConnectionType::STATE_EXT);
 
     switch (connection)
     {
@@ -135,23 +135,6 @@ bool YARPRobotStatePublisherModule::configure(yarp::os::ResourceFinder &rf)
         break;
     }
 
-    case ConnectionType::JOINT_STATE:
-    {
-        std::shared_ptr<JointStateConnector> jointStateConnector = std::make_shared<JointStateConnector>();
-
-        if (!jointStateConnector->configure(rf, modelLoader.model(), basicInfo))
-        {
-            yError() << "Failed to configure the module to connect to the robot via JointState.";
-            return false;
-        }
-
-        jointStateConnector->setCallback([this](){this->onReadCallback();});
-        m_useCallback = true;
-
-        m_connector = jointStateConnector;
-        break;
-    }
-
     default:
         yError() << "The specified connector is not available for this module.";
         return false;
@@ -318,7 +301,7 @@ bool YARPRobotStatePublisherModule::close()
 {
     std::lock_guard<std::mutex> guard(m_mutex);
 
-    // Disconnect the topic subscriber
+    // Close the connector
     if (m_connector)
     {
         m_connector->close();