added YARP_COMPILE_yarpActionsPlayer

cmake/YarpFindDependencies.cmake

@@ -524,6 +524,10 @@ yarp_dependent_option(
   YARP_COMPILE_yarpdatadumper "Do you want to compile yarpdatadumper?" ON
   "YARP_COMPILE_EXECUTABLES" OFF
 )
+yarp_dependent_option(
+  YARP_COMPILE_yarpActionsPlayer "Do you want to compile yarpActionsPlayer?" ON
+  "YARP_COMPILE_EXECUTABLES" OFF
+)
 yarp_dependent_option(
   YARP_COMPILE_yarpview "Do you want to compile yarpview?" ON
   "YARP_COMPILE_EXECUTABLES;YARP_COMPILE_GUIS;YARP_HAS_Qt5" OFF
@@ -682,6 +686,7 @@ yarp_print_feature(YARP_COMPILE_EXECUTABLES 0 "Compile executables")
 yarp_print_feature(YARP_COMPILE_yarprobotinterface 1 "Compile yarprobotinterface${YARP_COMPILE_yarprobotinterface_disable_reason}")
 yarp_print_feature(YARP_COMPILE_yarpmanager-console 1 "Compile YARP Module Manager (console)${YARP_COMPILE_yarpmanager-console_disable_reason}")
 yarp_print_feature(YARP_COMPILE_yarpdatadumper 1 "Compile yarpdatadumper${YARP_COMPILE_yarpdatadumper_disable_reason}")
+yarp_print_feature(YARP_COMPILE_yarpActionsPlayer 1 "Compile yarpActionsPlayer${YARP_COMPILE_yarpActionsPlayer_disable_reason}")
 yarp_print_feature("YARP_COMPILE_yarpdatadumper AND YARP_HAS_OpenCV" 2 "yarpdatadumper video support")
 yarp_print_feature(YARP_COMPILE_GUIS 1 "Compile GUIs${YARP_COMPILE_GUIS_disable_reason}")
 yarp_print_feature(YARP_COMPILE_yarpview 2 "Compile yarpview${YARP_COMPILE_yarpview_disable_reason}")

src/yarpActionsPlayer/CMakeLists.txt

@@ -1,23 +1,25 @@
 # SPDX-FileCopyrightText: 2024-2024 Istituto Italiano di Tecnologia (IIT)
 # SPDX-License-Identifier: BSD-3-Clause
 
-project(yarpActionsPlayer)
+if(YARP_COMPILE_yarpActionsPlayer)
+  project(yarpActionsPlayer)
 
-file(GLOB folder_source *.cpp)
-file(GLOB folder_header *.h)
-source_group("Source Files" FILES ${folder_source})
-source_group("Header Files" FILES ${folder_header})
+  file(GLOB folder_source *.cpp)
+  file(GLOB folder_header *.h)
+  source_group("Source Files" FILES ${folder_source})
+  source_group("Header Files" FILES ${folder_header})
 
-add_executable(yarpActionsPlayer ${folder_source} ${folder_header})
+  add_executable(yarpActionsPlayer ${folder_source} ${folder_header})
 
-target_link_libraries(yarpActionsPlayer
-  PRIVATE
-    YARP::YARP_init
-    YARP::YARP_os
-    YARP::YARP_sig
-    YARP::YARP_dev
-)
+  target_link_libraries(yarpActionsPlayer
+    PRIVATE
+      YARP::YARP_init
+      YARP::YARP_os
+      YARP::YARP_sig
+      YARP::YARP_dev
+  )
 
-install(TARGETS yarpActionsPlayer COMPONENT utilities DESTINATION ${CMAKE_INSTALL_BINDIR})
+  install(TARGETS yarpActionsPlayer COMPONENT utilities DESTINATION ${CMAKE_INSTALL_BINDIR})
 
-set_property(TARGET yarpActionsPlayer PROPERTY FOLDER "Command Line Tools")
+  set_property(TARGET yarpActionsPlayer PROPERTY FOLDER "Command Line Tools")
+endif()

src/yarpActionsPlayer/broadcastingThread.cpp

@@ -0,0 +1,139 @@
+/*
+ * SPDX-FileCopyrightText: 2024 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <yarp/os/Time.h>
+#include <yarp/os/Log.h>
+#include <yarp/os/LogStream.h>
+#include <yarp/os/BufferedPort.h>
+#include <yarp/dev/IPidControl.h>
+
+#include "robotDriver.h"
+#include "robotAction.h"
+#include "broadcastingThread.h"
+
+
+BroadcastingThread::BroadcastingThread(std::string name, robotDriver *p, action_class *a, double period): PeriodicThread(period)
+{
+    yAssert(p != nullptr);
+    yAssert(a != nullptr);
+
+    module_name = name;
+    driver = p;
+    actions = a;
+}
+
+BroadcastingThread::~BroadcastingThread()
+{
+    port_data_out.interrupt();
+    port_data_out.close();
+}
+
+bool BroadcastingThread::threadInit()
+{
+    if (!port_data_out.open(std::string("/") + module_name + "/all_joints_data_out:o"));
+    {
+        return false;
+    }
+
+    if (!driver)
+    {
+        return false;
+    }
+
+    if (!actions)
+    {
+        return false;
+    }
+
+    njoints = driver->getNJoints();
+    encs.resize(njoints);
+    outs.resize(njoints);
+    errs.resize(njoints);
+    mots.resize(njoints);
+
+    return true;
+}
+
+void BroadcastingThread::run()
+{
+    //reads the current position
+    if (driver && driver->ienc_ll)
+    {
+        driver->ienc_ll->getEncoders(encs.data());
+    }
+    else
+    {
+        //invalid driver
+    }
+
+    //reads the pid output
+    if (driver && driver->ipid_ll)
+    {
+        driver->ipid_ll->getPidOutputs(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_POSITION,outs.data());
+    }
+    else
+    {
+        //invalid driver
+    }
+
+    //reads the pid error
+    if (driver && driver->ipid_ll)
+    {
+        driver->ipid_ll->getPidErrors(yarp::dev::PidControlTypeEnum::VOCAB_PIDTYPE_POSITION,errs.data());
+    }
+    else
+    {
+        //invalid driver
+    }
+
+    //reads the motor encoders
+    if (driver && driver->imotenc_ll)
+    {
+        driver->imotenc_ll->getMotorEncoders(mots.data());
+    }
+    else
+    {
+        //invalid driver
+    }
+
+    size_t j = actions->current_frame;
+
+    yarp::os::Bottle& bot2 = this->port_data_out.prepare();
+    bot2.clear();
+    bot2.addInt32((int)actions->action_frames_vector[j].counter);
+    bot2.addFloat64(actions->action_frames_vector[j].time);
+
+    size_t size = this->actions->action_frames_vector[j].q_joints.size();
+    double *ll = actions->action_frames_vector[j].q_joints.data();
+
+    bot2.addString("commands:");
+    for (int ix=0;ix<size;ix++)
+    {
+        bot2.addFloat64(ll[ix]);
+    }
+    bot2.addString("joint encoders:");
+    for (int ix=0;ix<size;ix++)
+    {
+        bot2.addFloat64(encs[ix]);
+    }
+    bot2.addString("outputs:");
+    for (int ix=0;ix<size;ix++)
+    {
+        bot2.addFloat64(outs[ix]);
+    }
+    bot2.addString("motor encoders:");
+    for (int ix=0;ix<size;ix++)
+    {
+        bot2.addFloat64(mots[ix]);
+    }
+    bot2.addString("errors:");
+    for (int ix=0;ix<size;ix++)
+    {
+        bot2.addFloat64(errs[ix]);
+    }
+    bot2.addString("timestamp:");
+    bot2.addFloat64(yarp::os::Time::now());
+    this->port_data_out.write();
+}

src/yarpActionsPlayer/broadcastingThread.h

@@ -0,0 +1,49 @@
+/*
+ * SPDX-FileCopyrightText: 2024 Istituto Italiano di Tecnologia (IIT)
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <yarp/os/Log.h>
+#include <yarp/os/LogStream.h>
+#include <yarp/os/BufferedPort.h>
+#include <yarp/os/PeriodicThread.h>
+#include <yarp/sig/Vector.h>
+
+#include <iostream>
+#include <iomanip>
+#include <string>
+#include <vector>
+#include <cmath>
+#include <mutex>
+
+#include "robotDriver.h"
+#include "robotAction.h"
+
+#ifndef BROADCASTING_THREAD
+#define BROADCASTING_THREAD
+
+// ******************** THE THREAD
+class BroadcastingThread: public yarp::os::PeriodicThread
+{
+    size_t njoints=0;
+    std::vector<double> encs;
+    std::vector<double> outs;
+    std::vector<double> errs;
+    std::vector<double> mots;
+
+private:
+    std::string           module_name;
+    action_class          *actions=nullptr;
+    robotDriver           *driver=nullptr;
+    yarp::os::BufferedPort<yarp::os::Bottle>  port_data_out;
+
+public:
+    BroadcastingThread(std::string module_name, robotDriver *p, action_class *a, double period = 0.001);
+    ~BroadcastingThread();
+    void attachRobotDriver(robotDriver *p);
+    void attachActions(action_class *a);
+    bool threadInit() override;
+    void run() override;
+};
+
+#endif

src/yarpActionsPlayer/controlThread.h

@@ -8,7 +8,6 @@
 #include <yarp/os/BufferedPort.h>
 #include <yarp/os/PeriodicThread.h>
 #include <yarp/sig/Vector.h>
-#include <yarp/math/Math.h>
 
 #include <string>
 #include <cmath>

src/yarpActionsPlayer/robotAction.cpp

@@ -5,6 +5,7 @@
 
 #include <yarp/os/Log.h>
 #include <yarp/os/LogStream.h>
+#include <algorithm>
 
 #include <fstream>