Merge pull request #2223 from randaz81/yarpdataplayer_more_types

yarpdataplayer is now able to reproduce ROS data types (and more!)

doc/cmd_yarpdataplayer.dox

@@ -114,6 +114,16 @@ the player will successfully load all required data.
 The parts name will be taken from each subdirectory of the `/experiment1`
 forder.
 
+\section yarpdataplayer_ros Topic/ros compatibility
+
+Yarpdataplayer allows also to reproduce topics which can be subscribed by ROS nodes.
+For example, you can record a topic (e.g. /mytopic+@/mynode) as a standard yarp port using the Bottle format provided by yarpdatadumper. The Bottle format is needed because
+yarpdatadumper is currently not able to automatically detect the type of the incoming messages. 
+After the recording, you have to manually edit the info.log file created by yarpdatadumper and replace the type Bottle with the type of the ROS message you want to publish.
+For example, if you want to publish a sensor_msgs/LaserScan ROs Message, replace Bottle with sensor_msgs/LaserScan.
+That's all. When Yarpdataplayer plays the recorded data, it will open the /mytopic+@/mynode port and will publish LaserScan data. The corresponding topic will be visible also by ROS. 
+
+
 \note Currently, if the directory selected is (in this example) head,
 torso etc, the player will not load the files as it will not be able to
 find subdirs.

doc/release/master/featureYarpDataPlayer_ROS.md

@@ -0,0 +1,17 @@
+featureYarpDataPlayer_ROS {master}
+----------------------
+
+### Tools
+
+#### yarpdataplayer
+
+* `yarpdataplayer` is now able to reproduce several ROS types, i.e.
+ ```
+sensor_msgs/LaserScan.
+nav_msgs/Odometry
+tf/tfMessage
+tf2_msgs/tfMessage
+geometry_msgs/Pose
+geometry_msgs/Pose2D
+```
+Other types can be easily added just editing `utils.cpp` and `worker.cpp`

src/yarpdataplayer/CMakeLists.txt

@@ -19,7 +19,8 @@ if(YARP_COMPILE_yarpdataplayer)
                           src/main.cpp
                           src/mainwindow.cpp
                           src/utils.cpp
-                          src/worker.cpp)
+                          src/worker.cpp
+                          src/worker-impl.cpp)
 
 
   set(yarpdataplayer_HDRS include/aboutdlg.h
@@ -84,7 +85,8 @@ if(YARP_COMPILE_yarpdataplayer)
   target_link_libraries(yarpdataplayer PRIVATE YARP::YARP_os
                                                YARP::YARP_init
                                                YARP::YARP_sig
-                                               Qt5::Widgets)
+                                               Qt5::Widgets
+                                               YARP::YARP_rosmsg)
 
   if(YARP_HAS_OpenCV)
     target_compile_definitions(yarpdataplayer PRIVATE HAS_OPENCV)

src/yarpdataplayer/include/utils.h

@@ -37,24 +37,25 @@ class MasterThread;
 class QMainWindow;
 
 struct partsData
-    {
-        WorkerClass             *worker;                            //personal rate thread
-        std::mutex         mutex;                              //mutex
-        std::string             name;                               //string containing the name of the part
-        std::string             infoFile;                           //string containing the path of the infoFile
-        std::string             logFile;                            //string containing the path of the logFile
-        std::string             path;                               //string containing the path of the part
-        std::string             type;                               //string containing the type of the data
-        int                     currFrame;                          //integer containing the current frame
-        int                     maxFrame;                           //integer containing the maxFrame
-        yarp::os::Bottle        bot;                                //yarp Bottle containing all the data
-        yarp::sig::Vector       timestamp;                          //yarp Vector containing all the timestamps
-        yarp::os::BufferedPort<yarp::os::Bottle>        bottlePort; //yarp port for sending bottles
-        yarp::os::BufferedPort<yarp::sig::Image>        imagePort;  //yarp port for sending images
-        std::string             portName;                           //the name of the port
-        int                     sent;                               //integer used for step from command
-        bool                    hasNotified;                        //boolean used for individual part notification that it has reached eof
-    };
+{
+    WorkerClass             *worker;                            //personal rate thread
+    std::mutex              mutex;                              //mutex
+    std::string             name;                               //string containing the name of the part
+    std::string             infoFile;                           //string containing the path of the infoFile
+    std::string             logFile;                            //string containing the path of the logFile
+    std::string             path;                               //string containing the path of the part
+    std::string             type;                               //string containing the type of the data
+    int                     currFrame;                          //integer containing the current frame
+    int                     maxFrame;                           //integer containing the maxFrame
+    yarp::os::Bottle        bot;                                //yarp Bottle containing all the data
+    yarp::sig::Vector       timestamp;                          //yarp Vector containing all the timestamps
+    yarp::os::Contactable*  outputPort;                         //yarp port for sending out data
+    std::string             portName;                           //the name of the port
+    int                     sent;                               //integer used for step from command
+    bool                    hasNotified;                        //boolean used for individual part notification that it has reached eof
+
+    partsData() { outputPort = nullptr; worker = nullptr;}
+};
 
 struct RowInfo {
     std::string name;

src/yarpdataplayer/include/worker.h

@@ -20,11 +20,13 @@
 #define WORKER_H
 
 #include <QObject>
+#include "include/log.h"
 
 #include <yarp/sig/Image.h>
 #include <yarp/sig/Vector.h>
 #include <yarp/os/PeriodicThread.h>
 #include <yarp/os/Semaphore.h>
+#include <yarp/os/Stamp.h>
 #include <yarp/sig/ImageFile.h>
 #include "include/utils.h"
 #include <yarp/os/Event.h>
@@ -39,7 +41,6 @@
 #include <yarp/cv/Cv.h>
 #endif
 
-
 class Utilities;
 //class MainWindow;
 
@@ -69,9 +70,14 @@ class WorkerClass : public QObject
     */
     void setManager(Utilities *utilities);
     /**
-    * Function that sends the images
+    * Functions that sends data (many different types)
     */
-    int sendImages( int part, int id );
+    int sendBottle(int part, int id);
+    int sendImages( int part, int id);
+
+    template <class T>
+    int sendGenericData(int part, int id);
+
     /**
     * Function that returns the frame rate
     */

src/yarpdataplayer/src/utils.cpp

@@ -44,6 +44,14 @@
 #include "include/mainwindow.h"
 #include "include/log.h"
 
+ //ROS messages
+#include <yarp/rosmsg/sensor_msgs/LaserScan.h>
+#include <yarp/rosmsg/nav_msgs/Odometry.h>
+#include <yarp/rosmsg/tf/tfMessage.h>
+#include <yarp/rosmsg/tf2_msgs/TFMessage.h>
+#include <yarp/rosmsg/geometry_msgs/Pose.h>
+#include <yarp/rosmsg/geometry_msgs/Pose2D.h>
+
 using namespace yarp::os;
 using namespace yarp::sig;
 using namespace std;
@@ -366,49 +374,64 @@ bool Utilities::configurePorts(partsData &part)
         tmp_port_name="/"+moduleName+tmp_port_name;
     }
 
-    if (strcmp (part.type.c_str(),"Bottle") == 0){
-        if ( !yarp::os::Network::exists(tmp_port_name) ){
-            LOG("need to create new port %s for %s\n",part.type.c_str(), part.name.c_str());
-            part.bottlePort.close();
-            LOG("creating and opening %s port for part %s\n",part.type.c_str(), part.name.c_str());
-            part.bottlePort.open(tmp_port_name);
-        }
-    } else if (strcmp (part.type.c_str(),"Image:ppm") == 0 || strcmp (part.type.c_str(),"Image") == 0){
-        if ( !yarp::os::Network::exists(tmp_port_name) ){
-            LOG("need to create new port %s for %s\n",part.type.c_str(), part.name.c_str());
-            part.imagePort.close();
-            LOG("creating and opening image port for part %s\n",part.name.c_str());
-            part.imagePort.open(tmp_port_name);
-        }
+    if (strcmp (part.type.c_str(),"Bottle") == 0)   {
+        if (part.outputPort == nullptr) { part.outputPort = new BufferedPort<yarp::os::Bottle>; }
+    }
+    else if (strcmp (part.type.c_str(),"Image:ppm") == 0 || strcmp (part.type.c_str(),"Image") == 0)  {
+        if (part.outputPort == nullptr) { part.outputPort = new BufferedPort<yarp::sig::Image>; }
+    }
+    else if (strcmp(part.type.c_str(), "sensor_msgs/LaserScan") == 0 ) {
+        if (part.outputPort == nullptr) { part.outputPort = new BufferedPort<yarp::rosmsg::sensor_msgs::LaserScan>; }
     }
-    else {
+    else if (strcmp(part.type.c_str(), "nav_msgs/Odometry") == 0) {
+        if (part.outputPort == nullptr) { part.outputPort = new BufferedPort<yarp::rosmsg::nav_msgs::Odometry>; }
+    }
+    else if (strcmp(part.type.c_str(), "tf2_msgs/tf") == 0) {
+        if (part.outputPort == nullptr) { part.outputPort = new BufferedPort<yarp::rosmsg::tf2_msgs::TFMessage>; }
+    }
+    else if (strcmp(part.type.c_str(), "geometry_msgs/Pose") == 0) {
+        if (part.outputPort == nullptr) { part.outputPort = new BufferedPort<yarp::rosmsg::geometry_msgs::Pose>; }
+    }
+    else if (strcmp(part.type.c_str(), "geometry_msgs/Pose2D") == 0) {
+        if (part.outputPort == nullptr) { part.outputPort = new BufferedPort<yarp::rosmsg::geometry_msgs::Pose2D>; }
+    }
+    else
+    {
         LOG("Something is wrong with the data...%s\nIt seems its missing a type \n",part.name.c_str());
         return false;
     }
+
+    if (!yarp::os::Network::exists(tmp_port_name))
+    {
+        LOG("need to create new port %s for %s\n", part.type.c_str(), part.name.c_str());
+        part.outputPort->close();
+        LOG("creating and opening %s port for part %s\n", part.type.c_str(), part.name.c_str());
+        part.outputPort->open(tmp_port_name);
+    }
+    else
+    {
+        LOG("port %s already exists, skipping\n", tmp_port_name.c_str());
+    }
+
+
     return true;
 }
 /**********************************************************/
 bool Utilities::interruptPorts(partsData &part)
 {
-    if (strcmp (part.type.c_str(),"Bottle") == 0){
-        part.bottlePort.interrupt();
-    } else if (strcmp (part.type.c_str(),"Image:ppm") == 0 || strcmp (part.type.c_str(),"Image") == 0){
-        part.imagePort.interrupt();
-    } else {
+    if (part.outputPort == nullptr)  {
         return false;
     }
+    part.outputPort->interrupt();
     return true;
 }
 /**********************************************************/
 bool Utilities::closePorts(partsData &part)
 {
-    if (strcmp (part.type.c_str(),"Bottle") == 0){
-        part.bottlePort.close();
-    } else if (strcmp (part.type.c_str(),"Image:ppm") == 0 || strcmp (part.type.c_str(),"Image") == 0) {
-        part.imagePort.close();
-    } else {
+    if (part.outputPort == nullptr)  {
         return false;
     }
+    part.outputPort->close();
     return true;
 }
 

src/yarpdataplayer/src/worker-impl.cpp

@@ -0,0 +1,69 @@
+/*
+ * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ */
+
+#if defined(_WIN32)
+    #pragma warning (disable : 4099)
+    #pragma warning (disable : 4250)
+    #pragma warning (disable : 4520)
+#endif
+
+#include "include/utils.h"
+#include <yarp/rosmsg/sensor_msgs/LaserScan.h>
+#include <yarp/rosmsg/nav_msgs/Odometry.h>
+#include <yarp/rosmsg/tf/tfMessage.h>
+#include <yarp/rosmsg/tf2_msgs/TFMessage.h>
+#include <yarp/rosmsg/geometry_msgs/Pose.h>
+#include <yarp/rosmsg/geometry_msgs/Pose2D.h>
+
+template <class T>
+int WorkerClass::sendGenericData(int part, int id)
+{
+    yarp::os::Bottle tmp;
+    if (utilities->withExtraColumn) {
+        tmp = utilities->partDetails[part].bot.get(id).asList()->tail().tail().tail();
+    }
+    else {
+        tmp = utilities->partDetails[part].bot.get(id).asList()->tail().tail();
+    }
+
+    yarp::os::BufferedPort<T>* the_port = dynamic_cast<yarp::os::BufferedPort<T>*> (utilities->partDetails[part].outputPort);
+    if (the_port == nullptr) { LOG_ERROR("dynamic_cast failed"); return -1; }
+
+    auto& dat = the_port->prepare();
+    yarp::os::Portable::copyPortable(tmp, dat);
+
+    //propagate timestamp
+    yarp::os::Stamp ts(id, utilities->partDetails[part].timestamp[id]);
+    the_port->setEnvelope(ts);
+
+    if (utilities->sendStrict) {
+        the_port->writeStrict();
+    }
+    else {
+        the_port->write();
+    }
+    return 0;
+}
+
+
+template int WorkerClass::sendGenericData<yarp::os::Bottle>(int,int);
+template int WorkerClass::sendGenericData<yarp::rosmsg::sensor_msgs::LaserScan>(int, int);
+template int WorkerClass::sendGenericData<yarp::rosmsg::nav_msgs::Odometry>(int, int);
+template int WorkerClass::sendGenericData<yarp::rosmsg::tf2_msgs::TFMessage>(int, int);
+template int WorkerClass::sendGenericData<yarp::rosmsg::geometry_msgs::Pose>(int, int);
+template int WorkerClass::sendGenericData<yarp::rosmsg::geometry_msgs::Pose2D>(int, int);