From 90b64ccb96fdeb7418f18e883c4de1f30538911d Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Tue, 3 Mar 2020 18:36:38 +0100 Subject: [PATCH 1/5] preliminary refactoring of yarpdataplayer to allow more data types --- src/yarpdataplayer/include/utils.h | 5 +-- src/yarpdataplayer/include/worker.h | 3 +- src/yarpdataplayer/src/utils.cpp | 52 ++++++++++----------- src/yarpdataplayer/src/worker.cpp | 70 ++++++++++++++++++----------- 4 files changed, 75 insertions(+), 55 deletions(-) diff --git a/src/yarpdataplayer/include/utils.h b/src/yarpdataplayer/include/utils.h index fdbe159b01e..dfc5d839035 100644 --- a/src/yarpdataplayer/include/utils.h +++ b/src/yarpdataplayer/include/utils.h @@ -49,12 +49,11 @@ struct partsData 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 bottlePort; //yarp port for sending bottles - yarp::os::BufferedPort imagePort; //yarp port for sending images + 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 - }; +}; struct RowInfo { std::string name; diff --git a/src/yarpdataplayer/include/worker.h b/src/yarpdataplayer/include/worker.h index b128cd688cf..3f05ce49bda 100644 --- a/src/yarpdataplayer/include/worker.h +++ b/src/yarpdataplayer/include/worker.h @@ -69,8 +69,9 @@ class WorkerClass : public QObject */ void setManager(Utilities *utilities); /** - * Function that sends the images + * Functions that sends data (many different types) */ + int sendBottle(int part, int id); int sendImages( int part, int id ); /** * Function that returns the frame rate diff --git a/src/yarpdataplayer/src/utils.cpp b/src/yarpdataplayer/src/utils.cpp index 713b508cf3d..02488057763 100644 --- a/src/yarpdataplayer/src/utils.cpp +++ b/src/yarpdataplayer/src/utils.cpp @@ -366,49 +366,51 @@ 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; } + } + 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; } } - else { + 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); + } + + 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; } diff --git a/src/yarpdataplayer/src/worker.cpp b/src/yarpdataplayer/src/worker.cpp index e60579b5e30..b15d5bf7113 100644 --- a/src/yarpdataplayer/src/worker.cpp +++ b/src/yarpdataplayer/src/worker.cpp @@ -24,6 +24,7 @@ #include #include +#include #include "include/worker.h" #include "include/mainwindow.h" #include "include/log.h" @@ -110,29 +111,13 @@ void WorkerClass::run() if (isActive) { - Bottle tmp; - if (utilities->withExtraColumn){ - tmp = utilities->partDetails[part].bot.get(frame).asList()->tail().tail().tail(); - } else { - tmp = utilities->partDetails[part].bot.get(frame).asList()->tail().tail(); + if (strcmp (utilities->partDetails[part].type.c_str(),"Bottle") == 0) + { + sendBottle(part, frame); } - - if (strcmp (utilities->partDetails[part].type.c_str(),"Bottle") == 0){ - - Bottle& outBot = utilities->partDetails[part].bottlePort.prepare(); - outBot = tmp; - - //propagate timestamp - Stamp ts(frame,utilities->partDetails[part].timestamp[frame]); - utilities->partDetails[part].bottlePort.setEnvelope(ts); - - if (utilities->sendStrict){ - utilities->partDetails[part].bottlePort.writeStrict(); - } else { - utilities->partDetails[part].bottlePort.write(); - } - } - if (strcmp (utilities->partDetails[part].type.c_str(),"Image:ppm") == 0 || strcmp (utilities->partDetails[part].type.c_str(),"Image") == 0){ + if (strcmp (utilities->partDetails[part].type.c_str(),"Image:ppm") == 0 || + strcmp (utilities->partDetails[part].type.c_str(),"Image") == 0) + { sendImages(part, frame); } } @@ -153,6 +138,36 @@ double WorkerClass::getTimeTaken() return yarp::os::Time::now()-startTime; } +/**********************************************************/ +int WorkerClass::sendBottle(int part, int frame) +{ + Bottle tmp; + if (utilities->withExtraColumn) { + tmp = utilities->partDetails[part].bot.get(frame).asList()->tail().tail().tail(); + } + else { + tmp = utilities->partDetails[part].bot.get(frame).asList()->tail().tail(); + } + + yarp::os::BufferedPort* the_port = dynamic_cast*> (utilities->partDetails[part].outputPort); + if (the_port == nullptr) { yFatal() << "dynamic_cast failed"; } + + Bottle& outBot = the_port->prepare(); + outBot = tmp; + + //propagate timestamp + Stamp ts(frame, utilities->partDetails[part].timestamp[frame]); + the_port->setEnvelope(ts); + + if (utilities->sendStrict) { + the_port->writeStrict(); + } + else { + the_port->write(); + } + return 0; +} + /**********************************************************/ int WorkerClass::sendImages(int part, int frame) { @@ -244,15 +259,18 @@ int WorkerClass::sendImages(int part, int frame) } else { - utilities->partDetails[part].imagePort.prepare()=*img_yarp; + yarp::os::BufferedPort* the_port = dynamic_cast*> (utilities->partDetails[part].outputPort); + if (the_port == nullptr) { yFatal() << "dynamic_cast failed"; } + + the_port->prepare()=*img_yarp; Stamp ts(frame,utilities->partDetails[part].timestamp[frame]); - utilities->partDetails[part].imagePort.setEnvelope(ts); + the_port->setEnvelope(ts); if (utilities->sendStrict) { - utilities->partDetails[part].imagePort.writeStrict(); + the_port->writeStrict(); } else { - utilities->partDetails[part].imagePort.write(); + the_port->write(); } } From b781907684cd093d94529ab34ebed7f4b1c14fbc Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Wed, 4 Mar 2020 12:33:28 +0100 Subject: [PATCH 2/5] `yarpdataplayer` is now able to reproduce several ROS types. Other types can be easily added just editing `utils.cpp` and `worker.cpp` --- .../master/featureYarpDataPlayer_ROS.md | 17 +++++++ src/yarpdataplayer/CMakeLists.txt | 3 +- src/yarpdataplayer/include/worker.h | 36 ++++++++++++- src/yarpdataplayer/src/utils.cpp | 31 +++++++++-- src/yarpdataplayer/src/worker.cpp | 51 +++++++++++++++---- 5 files changed, 121 insertions(+), 17 deletions(-) create mode 100644 doc/release/master/featureYarpDataPlayer_ROS.md diff --git a/doc/release/master/featureYarpDataPlayer_ROS.md b/doc/release/master/featureYarpDataPlayer_ROS.md new file mode 100644 index 00000000000..cbca00f7b14 --- /dev/null +++ b/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` \ No newline at end of file diff --git a/src/yarpdataplayer/CMakeLists.txt b/src/yarpdataplayer/CMakeLists.txt index 9037c24cf06..11a78005c2e 100644 --- a/src/yarpdataplayer/CMakeLists.txt +++ b/src/yarpdataplayer/CMakeLists.txt @@ -84,7 +84,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) diff --git a/src/yarpdataplayer/include/worker.h b/src/yarpdataplayer/include/worker.h index 3f05ce49bda..2723588c490 100644 --- a/src/yarpdataplayer/include/worker.h +++ b/src/yarpdataplayer/include/worker.h @@ -20,11 +20,13 @@ #define WORKER_H #include +#include "include/log.h" #include #include #include #include +#include #include #include "include/utils.h" #include @@ -39,7 +41,6 @@ #include #endif - class Utilities; //class MainWindow; @@ -72,7 +73,38 @@ class WorkerClass : public QObject * Functions that sends data (many different types) */ int sendBottle(int part, int id); - int sendImages( int part, int id ); + int sendImages( int part, int id); + + template + int 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* the_port = dynamic_cast*> (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; + } + /** * Function that returns the frame rate */ diff --git a/src/yarpdataplayer/src/utils.cpp b/src/yarpdataplayer/src/utils.cpp index 02488057763..1611603ce41 100644 --- a/src/yarpdataplayer/src/utils.cpp +++ b/src/yarpdataplayer/src/utils.cpp @@ -44,6 +44,14 @@ #include "include/mainwindow.h" #include "include/log.h" + //ROS messages +#include +#include +#include +#include +#include +#include + using namespace yarp::os; using namespace yarp::sig; using namespace std; @@ -366,14 +374,27 @@ bool Utilities::configurePorts(partsData &part) tmp_port_name="/"+moduleName+tmp_port_name; } - if (strcmp (part.type.c_str(),"Bottle") == 0) - { + if (strcmp (part.type.c_str(),"Bottle") == 0) { if (part.outputPort == nullptr) { part.outputPort = new BufferedPort; } } - else if (strcmp (part.type.c_str(),"Image:ppm") == 0 || strcmp (part.type.c_str(),"Image") == 0) - { + 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; } } + else if (strcmp(part.type.c_str(), "LaserScan") == 0 ) { + if (part.outputPort == nullptr) { part.outputPort = new BufferedPort; } + } + else if (strcmp(part.type.c_str(), "Odometry") == 0) { + if (part.outputPort == nullptr) { part.outputPort = new BufferedPort; } + } + else if (strcmp(part.type.c_str(), "tf") == 0) { + if (part.outputPort == nullptr) { part.outputPort = new BufferedPort; } + } + else if (strcmp(part.type.c_str(), "Pose") == 0) { + if (part.outputPort == nullptr) { part.outputPort = new BufferedPort; } + } + else if (strcmp(part.type.c_str(), "Pose2D") == 0) { + if (part.outputPort == nullptr) { part.outputPort = new BufferedPort; } + } else { LOG("Something is wrong with the data...%s\nIt seems its missing a type \n",part.name.c_str()); @@ -389,7 +410,7 @@ bool Utilities::configurePorts(partsData &part) } else { - LOG("port %s already exists, skipping\n", tmp_port_name); + LOG("port %s already exists, skipping\n", tmp_port_name.c_str()); } diff --git a/src/yarpdataplayer/src/worker.cpp b/src/yarpdataplayer/src/worker.cpp index b15d5bf7113..ad907fa1e17 100644 --- a/src/yarpdataplayer/src/worker.cpp +++ b/src/yarpdataplayer/src/worker.cpp @@ -23,7 +23,6 @@ #endif #include -#include #include #include "include/worker.h" #include "include/mainwindow.h" @@ -32,6 +31,15 @@ #include #endif +//ROS messages +#include +#include +#include +#include +#include +#include + + using namespace yarp::sig; using namespace yarp::sig::file; using namespace yarp::os; @@ -111,14 +119,38 @@ void WorkerClass::run() if (isActive) { - if (strcmp (utilities->partDetails[part].type.c_str(),"Bottle") == 0) - { - sendBottle(part, frame); - } + int ret=-1; if (strcmp (utilities->partDetails[part].type.c_str(),"Image:ppm") == 0 || - strcmp (utilities->partDetails[part].type.c_str(),"Image") == 0) - { - sendImages(part, frame); + strcmp (utilities->partDetails[part].type.c_str(),"Image") == 0) { + ret = sendImages(part, frame); + } + else if (strcmp(utilities->partDetails[part].type.c_str(), "Bottle") == 0) { + ret = sendBottle(part, frame); + // the above line can be safely replaced with sendGenericData. + // I kept it for no particular reason, thinking that maybe it could be convenient (later) + // to process Bottles in a different way. + } + else if (strcmp(utilities->partDetails[part].type.c_str(), "LaserScan") == 0) { + ret = sendGenericData(part, frame); + } + else if (strcmp(utilities->partDetails[part].type.c_str(), "Odometry") == 0) { + ret = sendGenericData(part, frame); + } + else if (strcmp(utilities->partDetails[part].type.c_str(), "tf") == 0) { + ret = sendGenericData(part, frame); + } + else if (strcmp(utilities->partDetails[part].type.c_str(), "Pose") == 0) { + ret = sendGenericData(part, frame); + } + else if (strcmp(utilities->partDetails[part].type.c_str(), "Pose2D") == 0) { + ret = sendGenericData(part, frame); + } + else { + LOG("Unknown data type: %s", utilities->partDetails[part].type.c_str()); + } + + if (ret==-1) { + LOG("Failed to send data: %s", utilities->partDetails[part].type.c_str()); } } utilities->partDetails[part].sent++; @@ -150,7 +182,8 @@ int WorkerClass::sendBottle(int part, int frame) } yarp::os::BufferedPort* the_port = dynamic_cast*> (utilities->partDetails[part].outputPort); - if (the_port == nullptr) { yFatal() << "dynamic_cast failed"; } + if (the_port == nullptr) + { LOG_ERROR("dynamic_cast failed"); return -1;} Bottle& outBot = the_port->prepare(); outBot = tmp; From 1321dc25cc5e3340f97710837683d9943bc169d7 Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Wed, 4 Mar 2020 14:12:28 +0100 Subject: [PATCH 3/5] * added documentation in cmd_yarpdataplayer.doc * cleanup --- doc/cmd_yarpdataplayer.dox | 10 ++++++++++ src/yarpdataplayer/src/utils.cpp | 10 +++++----- src/yarpdataplayer/src/worker.cpp | 10 +++++----- 3 files changed, 20 insertions(+), 10 deletions(-) diff --git a/doc/cmd_yarpdataplayer.dox b/doc/cmd_yarpdataplayer.dox index e70fa11eb3f..548689f391c 100644 --- a/doc/cmd_yarpdataplayer.dox +++ b/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. diff --git a/src/yarpdataplayer/src/utils.cpp b/src/yarpdataplayer/src/utils.cpp index 1611603ce41..e6a82ae7540 100644 --- a/src/yarpdataplayer/src/utils.cpp +++ b/src/yarpdataplayer/src/utils.cpp @@ -380,19 +380,19 @@ bool Utilities::configurePorts(partsData &part) 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; } } - else if (strcmp(part.type.c_str(), "LaserScan") == 0 ) { + else if (strcmp(part.type.c_str(), "sensor_msgs/LaserScan") == 0 ) { if (part.outputPort == nullptr) { part.outputPort = new BufferedPort; } } - else if (strcmp(part.type.c_str(), "Odometry") == 0) { + else if (strcmp(part.type.c_str(), "nav_msgs/Odometry") == 0) { if (part.outputPort == nullptr) { part.outputPort = new BufferedPort; } } - else if (strcmp(part.type.c_str(), "tf") == 0) { + else if (strcmp(part.type.c_str(), "tf2_msgs/tf") == 0) { if (part.outputPort == nullptr) { part.outputPort = new BufferedPort; } } - else if (strcmp(part.type.c_str(), "Pose") == 0) { + else if (strcmp(part.type.c_str(), "geometry_msgs/Pose") == 0) { if (part.outputPort == nullptr) { part.outputPort = new BufferedPort; } } - else if (strcmp(part.type.c_str(), "Pose2D") == 0) { + else if (strcmp(part.type.c_str(), "geometry_msgs/Pose2D") == 0) { if (part.outputPort == nullptr) { part.outputPort = new BufferedPort; } } else diff --git a/src/yarpdataplayer/src/worker.cpp b/src/yarpdataplayer/src/worker.cpp index ad907fa1e17..b11c5856d08 100644 --- a/src/yarpdataplayer/src/worker.cpp +++ b/src/yarpdataplayer/src/worker.cpp @@ -130,19 +130,19 @@ void WorkerClass::run() // I kept it for no particular reason, thinking that maybe it could be convenient (later) // to process Bottles in a different way. } - else if (strcmp(utilities->partDetails[part].type.c_str(), "LaserScan") == 0) { + else if (strcmp(utilities->partDetails[part].type.c_str(), "sensor_msgs/LaserScan") == 0) { ret = sendGenericData(part, frame); } - else if (strcmp(utilities->partDetails[part].type.c_str(), "Odometry") == 0) { + else if (strcmp(utilities->partDetails[part].type.c_str(), "nav_msgs/Odometry") == 0) { ret = sendGenericData(part, frame); } - else if (strcmp(utilities->partDetails[part].type.c_str(), "tf") == 0) { + else if (strcmp(utilities->partDetails[part].type.c_str(), "tf2_msgs/tf") == 0) { ret = sendGenericData(part, frame); } - else if (strcmp(utilities->partDetails[part].type.c_str(), "Pose") == 0) { + else if (strcmp(utilities->partDetails[part].type.c_str(), "geometry_msgs/Pose") == 0) { ret = sendGenericData(part, frame); } - else if (strcmp(utilities->partDetails[part].type.c_str(), "Pose2D") == 0) { + else if (strcmp(utilities->partDetails[part].type.c_str(), "geometry_msgs/Pose2D") == 0) { ret = sendGenericData(part, frame); } else { From e9960342f171148a502e6ada513047c37f586c37 Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Wed, 4 Mar 2020 16:54:50 +0100 Subject: [PATCH 4/5] cleanup --- src/yarpdataplayer/include/utils.h | 34 +++++++++++++++-------------- src/yarpdataplayer/include/worker.h | 4 ++-- src/yarpdataplayer/src/utils.cpp | 4 ++-- src/yarpdataplayer/src/worker.cpp | 6 ++--- 4 files changed, 25 insertions(+), 23 deletions(-) diff --git a/src/yarpdataplayer/include/utils.h b/src/yarpdataplayer/include/utils.h index dfc5d839035..3d248c22a41 100644 --- a/src/yarpdataplayer/include/utils.h +++ b/src/yarpdataplayer/include/utils.h @@ -37,22 +37,24 @@ 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::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 +{ + 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 { diff --git a/src/yarpdataplayer/include/worker.h b/src/yarpdataplayer/include/worker.h index 2723588c490..0a9383e2ce1 100644 --- a/src/yarpdataplayer/include/worker.h +++ b/src/yarpdataplayer/include/worker.h @@ -74,7 +74,7 @@ class WorkerClass : public QObject */ int sendBottle(int part, int id); int sendImages( int part, int id); - + template int sendGenericData(int part, int id) { @@ -104,7 +104,7 @@ class WorkerClass : public QObject } return 0; } - + /** * Function that returns the frame rate */ diff --git a/src/yarpdataplayer/src/utils.cpp b/src/yarpdataplayer/src/utils.cpp index e6a82ae7540..dc2c68419cb 100644 --- a/src/yarpdataplayer/src/utils.cpp +++ b/src/yarpdataplayer/src/utils.cpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include @@ -376,7 +376,7 @@ bool Utilities::configurePorts(partsData &part) if (strcmp (part.type.c_str(),"Bottle") == 0) { if (part.outputPort == nullptr) { part.outputPort = new BufferedPort; } - } + } 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; } } diff --git a/src/yarpdataplayer/src/worker.cpp b/src/yarpdataplayer/src/worker.cpp index b11c5856d08..b809ac59941 100644 --- a/src/yarpdataplayer/src/worker.cpp +++ b/src/yarpdataplayer/src/worker.cpp @@ -35,7 +35,7 @@ #include #include #include -#include +#include #include #include @@ -125,8 +125,8 @@ void WorkerClass::run() ret = sendImages(part, frame); } else if (strcmp(utilities->partDetails[part].type.c_str(), "Bottle") == 0) { - ret = sendBottle(part, frame); - // the above line can be safely replaced with sendGenericData. + ret = sendBottle(part, frame); + // the above line can be safely replaced with sendGenericData. // I kept it for no particular reason, thinking that maybe it could be convenient (later) // to process Bottles in a different way. } From 30cd2820173efd54208b99206ba88c40fdd80a1d Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Mon, 9 Mar 2020 16:16:24 +0100 Subject: [PATCH 5/5] fixed gcc warning --- src/yarpdataplayer/CMakeLists.txt | 3 +- src/yarpdataplayer/include/worker.h | 29 +---------- src/yarpdataplayer/src/worker-impl.cpp | 69 ++++++++++++++++++++++++++ 3 files changed, 72 insertions(+), 29 deletions(-) create mode 100644 src/yarpdataplayer/src/worker-impl.cpp diff --git a/src/yarpdataplayer/CMakeLists.txt b/src/yarpdataplayer/CMakeLists.txt index 11a78005c2e..2b8ff5a38a1 100644 --- a/src/yarpdataplayer/CMakeLists.txt +++ b/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 diff --git a/src/yarpdataplayer/include/worker.h b/src/yarpdataplayer/include/worker.h index 0a9383e2ce1..8bb61fa8b69 100644 --- a/src/yarpdataplayer/include/worker.h +++ b/src/yarpdataplayer/include/worker.h @@ -76,34 +76,7 @@ class WorkerClass : public QObject int sendImages( int part, int id); template - int 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* the_port = dynamic_cast*> (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; - } + int sendGenericData(int part, int id); /** * Function that returns the frame rate diff --git a/src/yarpdataplayer/src/worker-impl.cpp b/src/yarpdataplayer/src/worker-impl.cpp new file mode 100644 index 00000000000..5c220de2920 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +template +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* the_port = dynamic_cast*> (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(int,int); +template int WorkerClass::sendGenericData(int, int); +template int WorkerClass::sendGenericData(int, int); +template int WorkerClass::sendGenericData(int, int); +template int WorkerClass::sendGenericData(int, int); +template int WorkerClass::sendGenericData(int, int);