From df326e5afde1812ea154c45b3f5deb23eaf0aff1 Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Thu, 13 Feb 2025 18:10:07 +0100 Subject: [PATCH] added YARP_COMPILE_yarpActionsPlayer --- cmake/YarpFindDependencies.cmake | 5 + src/yarpActionsPlayer/CMakeLists.txt | 32 +++-- src/yarpActionsPlayer/broadcastingThread.cpp | 139 +++++++++++++++++++ src/yarpActionsPlayer/broadcastingThread.h | 49 +++++++ src/yarpActionsPlayer/controlThread.h | 1 - src/yarpActionsPlayer/robotAction.cpp | 1 + 6 files changed, 211 insertions(+), 16 deletions(-) create mode 100644 src/yarpActionsPlayer/broadcastingThread.cpp create mode 100644 src/yarpActionsPlayer/broadcastingThread.h diff --git a/cmake/YarpFindDependencies.cmake b/cmake/YarpFindDependencies.cmake index 46c42aabdac..70c03caa819 100644 --- a/cmake/YarpFindDependencies.cmake +++ b/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}") diff --git a/src/yarpActionsPlayer/CMakeLists.txt b/src/yarpActionsPlayer/CMakeLists.txt index 95f9561d095..421c5e25195 100644 --- a/src/yarpActionsPlayer/CMakeLists.txt +++ b/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() diff --git a/src/yarpActionsPlayer/broadcastingThread.cpp b/src/yarpActionsPlayer/broadcastingThread.cpp new file mode 100644 index 00000000000..346f5be7ccf --- /dev/null +++ b/src/yarpActionsPlayer/broadcastingThread.cpp @@ -0,0 +1,139 @@ +/* + * SPDX-FileCopyrightText: 2024 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include +#include +#include + +#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;ixport_data_out.write(); +} diff --git a/src/yarpActionsPlayer/broadcastingThread.h b/src/yarpActionsPlayer/broadcastingThread.h new file mode 100644 index 00000000000..c47beac12d0 --- /dev/null +++ b/src/yarpActionsPlayer/broadcastingThread.h @@ -0,0 +1,49 @@ +/* + * SPDX-FileCopyrightText: 2024 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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 encs; + std::vector outs; + std::vector errs; + std::vector mots; + +private: + std::string module_name; + action_class *actions=nullptr; + robotDriver *driver=nullptr; + yarp::os::BufferedPort 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 diff --git a/src/yarpActionsPlayer/controlThread.h b/src/yarpActionsPlayer/controlThread.h index ef54da1c63e..30c683a36d1 100644 --- a/src/yarpActionsPlayer/controlThread.h +++ b/src/yarpActionsPlayer/controlThread.h @@ -8,7 +8,6 @@ #include #include #include -#include #include #include diff --git a/src/yarpActionsPlayer/robotAction.cpp b/src/yarpActionsPlayer/robotAction.cpp index f938b992b49..fc789c9a53e 100644 --- a/src/yarpActionsPlayer/robotAction.cpp +++ b/src/yarpActionsPlayer/robotAction.cpp @@ -5,6 +5,7 @@ #include #include +#include #include