From 48e77188134f7dac5d623f1a9da21f80abeec433 Mon Sep 17 00:00:00 2001 From: Jacopo Date: Wed, 6 Mar 2024 16:53:06 +0100 Subject: [PATCH] Add MotorTemperaturePublisher tool --- app/motorTemperaturePublisher/conf/config.ini | 5 + .../motorTemperaturePublisher.yml | 41 ++++ .../plotTemperatureLeftLeg.xml | 50 +++++ .../temperatureScopeUpperArm.xml | 54 +++++ .../motorTemperaturePublisher/CMakeLists.txt | 10 + .../motorTemperaturePublisher.xml | 94 ++++++++ .../src/MotorTemperaturePublisher.cpp | 209 ++++++++++++++++++ .../src/MotorTemperaturePublisher.h | 68 ++++++ .../motorTemperaturePublisher/src/main.cpp | 51 +++++ 9 files changed, 582 insertions(+) create mode 100644 app/motorTemperaturePublisher/conf/config.ini create mode 100644 app/motorTemperaturePublisher/motorTemperaturePublisher.yml create mode 100644 app/motorTemperaturePublisher/plotTemperatureLeftLeg.xml create mode 100644 app/motorTemperaturePublisher/temperatureScopeUpperArm.xml create mode 100644 src/tools/motorTemperaturePublisher/CMakeLists.txt create mode 100644 src/tools/motorTemperaturePublisher/motorTemperaturePublisher.xml create mode 100644 src/tools/motorTemperaturePublisher/src/MotorTemperaturePublisher.cpp create mode 100644 src/tools/motorTemperaturePublisher/src/MotorTemperaturePublisher.h create mode 100644 src/tools/motorTemperaturePublisher/src/main.cpp diff --git a/app/motorTemperaturePublisher/conf/config.ini b/app/motorTemperaturePublisher/conf/config.ini new file mode 100644 index 0000000000..a578f23310 --- /dev/null +++ b/app/motorTemperaturePublisher/conf/config.ini @@ -0,0 +1,5 @@ +[GENERAL] +robotname ergocub +period 0.002 +portprefix /left_leg +listofjoints (0 1 2 3 4 5) diff --git a/app/motorTemperaturePublisher/motorTemperaturePublisher.yml b/app/motorTemperaturePublisher/motorTemperaturePublisher.yml new file mode 100644 index 0000000000..ed8bf01545 --- /dev/null +++ b/app/motorTemperaturePublisher/motorTemperaturePublisher.yml @@ -0,0 +1,41 @@ +version: "3.7" + +# the directory defined as ${ROOT_DIR}=/home/icub is basically the home directory of the default icub user +# which is the default user created with the user-based image. +# ROOT_DIR is an environmental variable defined in the .bashrc file of the user home directory +# if you are using this deployment files in your machine remember to choose one of the following options +# to run the containers correctly: +# - declare that env varibale when running the docker compose command and so then export the variable per each terminal opened +# - add the variable exporting in your .bashrc file so each time you open a terminal you have it declared +# - add it to the .bash_aliases file considering that that special file is looked for by the .bashrc + +x-MotorTemperaturePublisher: &MotorTemperaturePublisher + image: ghcr.io/icub-tech-iit/docker-deployment-images/cd_superbuild-icubhead-withuser:master-unstable_sources + environment: + - YARP_FORWARD_LOG_ENABLE=1 + - DISPLAY=${DISPLAY} + - QT_X11_NO_MITSHM=1 + - XAUTHORITY=${ROOT_DIR}/.Xauthority + volumes: + - "${HOME}/.config/yarp:${ROOT_DIR}/.config/yarp" + - "${HOME}/workspace:${ROOT_DIR}/workspace" + - "/tmp/.X11-unix:/tmp/.X11-unix" + - "${XAUTHORITY}:${ROOT_DIR}/.Xauthority" + working_dir: ${ROOT_DIR}/workspace/ICUB_FIRM/icub-main/src/tools/motorTemperaturePublisher + network_mode: "host" + +services: + app: + <<: *MotorTemperaturePublisher + restart: on-failure + command: ./build/MotorTemperaturePublisher --from app/conf/config.ini + + splitter: + <<: *MotorTemperaturePublisher + restart: on-failure + command: yarp split /ergocub2S/upper_arm/stateExt:o + + scope: + <<: *MotorTemperaturePublisher + restart: on-failure + command: yarpscope --xml temperatureScopeUpperArm.xml \ No newline at end of file diff --git a/app/motorTemperaturePublisher/plotTemperatureLeftLeg.xml b/app/motorTemperaturePublisher/plotTemperatureLeftLeg.xml new file mode 100644 index 0000000000..50852952ef --- /dev/null +++ b/app/motorTemperaturePublisher/plotTemperatureLeftLeg.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + diff --git a/app/motorTemperaturePublisher/temperatureScopeUpperArm.xml b/app/motorTemperaturePublisher/temperatureScopeUpperArm.xml new file mode 100644 index 0000000000..991d82e5c4 --- /dev/null +++ b/app/motorTemperaturePublisher/temperatureScopeUpperArm.xml @@ -0,0 +1,54 @@ + + + + + + + + + + + + diff --git a/src/tools/motorTemperaturePublisher/CMakeLists.txt b/src/tools/motorTemperaturePublisher/CMakeLists.txt new file mode 100644 index 0000000000..ae939c6771 --- /dev/null +++ b/src/tools/motorTemperaturePublisher/CMakeLists.txt @@ -0,0 +1,10 @@ +project(MotorTemperaturePublisher) +# find YARP +find_package(YARP COMPONENTS os sig dev) +# set up our program +add_executable(${PROJECT_NAME} ./src/MotorTemperaturePublisher.h ./src/MotorTemperaturePublisher.cpp ./src/main.cpp ) + +# link with YARP libraries +target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} DESTINATION bin) diff --git a/src/tools/motorTemperaturePublisher/motorTemperaturePublisher.xml b/src/tools/motorTemperaturePublisher/motorTemperaturePublisher.xml new file mode 100644 index 0000000000..6bc9f81962 --- /dev/null +++ b/src/tools/motorTemperaturePublisher/motorTemperaturePublisher.xml @@ -0,0 +1,94 @@ + + + + + + MotorTemperaturePublisher + icub_tools + Motor Temperature Detector and Publisher + Released under the terms of the GNU GPL v2.0 + 1.0 + + + This module is a working example for building a YARP device that exploits the iMotor YARP + interfaces to interact with the motion control module. + Specifically to this example, we are defining a PolyDriver device that interacts with the + pointer to the iMotor yarp interface and prints the motor temperatures to an output port. + Those temperatures value can be obtained exploiting the interface method getTemperatures() and + saved to a c-array of doubles. + Considering that, a properly built yarp module should derive from the public interface + yarp::os::RFModule, whose complete explanation is available at this link the + following methods should be overridden: + + + configure() + + + getPeriod() + + + updateModule() + + + close() + + + + In order to use this module, whose final target is to print at the port called /portprefix/motor_temperatures:o the temperatures of the motors connected, the following steps + should be done: + + + supposed to have the correct setup running on the yarpserver sending data to state ports + + + ./build/MotorTemperaturePublisher --from app/conf/config.ini + + + + This will instantiate the aformentioned port, where with a frequence of _updatePeriod time, + the following values will be streamed on the port: + + + + timeStamp | temperature_motor_0 | boolean_limit_overcame_0 | ... | temperature_motor_N | boolean_limit_overcame_N | + + + + the config.ini file should be filled in with the following data: + + + robotname: name of the robot as set in the configuration files + + + period: times used for streaming the data to the port + + + listofjoints: joints, as list a list, i.e. (0 1 2), which you wanna get the temperatures + + + + + + + context + from + name + robot + + + + Jacopo Losi + + + + + yarp::os::Bottle + /ergocub/subpart/motor_temperature:o + + show the temperature of the motors of the subpart + + + + + \ No newline at end of file diff --git a/src/tools/motorTemperaturePublisher/src/MotorTemperaturePublisher.cpp b/src/tools/motorTemperaturePublisher/src/MotorTemperaturePublisher.cpp new file mode 100644 index 0000000000..d7e73cefb7 --- /dev/null +++ b/src/tools/motorTemperaturePublisher/src/MotorTemperaturePublisher.cpp @@ -0,0 +1,209 @@ +/* + * Copyright (C) 2024 iCub Facility - Istituto Italiano di Tecnologia + * Author: Jacopo Losi + * email: jacopo.losi@iit.it + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program 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 General + * Public License for more details +*/ + +#include "MotorTemperaturePublisher.h" + +#include +#include +#include +#include + +using namespace std; +using namespace yarp::os; + + +bool MotorTemperaturePublisher::configure(yarp::os::ResourceFinder& rf) +{ + // Read configuration file + Bottle &conf_group = rf.findGroup("GENERAL"); + Bottle* jointsBottle = nullptr; + if (conf_group.isNull()) + { + yWarning() << "Missing GENERAL group! The module uses the default values"; + } + else + { + if(conf_group.check("portprefix")) { _portPrefix = conf_group.find("portprefix").asString(); } + if(conf_group.check("period")) { _updatePeriod = conf_group.find("period").asFloat64(); } + if(conf_group.check("robotname")) { _robotName = conf_group.find("robotname").asString(); } + if (conf_group.check("listofjoints")) + { + jointsBottle = conf_group.find("listofjoints").asList(); + _nEnabledMotors = jointsBottle->size(); + for(int i=0; i < _nEnabledMotors; i++) _listOfJoints.push_back(jointsBottle->get(i).asInt32()); + } + + } + + // Create remote motion control device + Property options; + options.put("device", "remote_controlboard"); + options.put("remote", "/"+ _robotName + _portPrefix); + options.put("local", _portPrefix + "/mc"); + + + yDebug() << "++++ config:\n" + << "\t portprefix: " << _portPrefix << "\n" + << "\t period: " << _updatePeriod << "\n" + << "\t robotname: " << _robotName << "\n" + << "\t listofjoints: " << jointsBottle->toString() << "\n"; + + _motionControlDevice.open(options); + + if (!_motionControlDevice.isValid()) + { + yError() << "Unable to open device driver. Aborting..."; + return false; + } + + if (!_motionControlDevice.view(_imot) || _imot==nullptr) + { + yError() << "Unable to open motor raw interface. Aborting..."; + return false; + } + + if (!_imot->getNumberOfMotors(&_nmotors)) + { + yError() << "Unable to retrieve the number of motors"; + return false; + } + else + { + yDebug() << "Working with" << _nmotors << "motors"; + yDebug() << "Enabling" << _nEnabledMotors << "motors of the subpart"; + } + + // Allocate memory for pointer + if (!alloc(_nEnabledMotors)) + { + yError() << "Error allocating memory for pointers. Aborting..."; + return false; + } + + + // open the communication port towards motor controller module + if(!_outputPort.open(_portPrefix +"/motor_temperatures:o")) + { + yError() << "Error opening output port for motor control"; + return false; + } + + for (uint8_t i = 0; i < _listOfJoints.size(); i++) + { + if (!_imot->getTemperatureLimit(i, &_motorTemperatureLimits[i])) + { + yError() << "Unable to get motor temperature Limits. Aborting..."; + return false; + } + else + { + yDebug() << "Limit for motor#" << i << "value:" << _motorTemperatureLimits[i]; + } + + } + + return true; +} + +bool MotorTemperaturePublisher::close() +{ + // Closing port explicitely + yInfo() << "Calling close functionality\n"; + + // Deallocating memory for pointers + if (!dealloc()) + { + yError() << "Error deallocating memory for pointer. Failing..."; + return false; + } + + return true; +} + +double MotorTemperaturePublisher::getPeriod() +{ + return _updatePeriod; +} + +bool MotorTemperaturePublisher::updateModule() +{ + + for (int i = 0; i < _listOfJoints.size(); i++) + { + _motorTemperatures[i]= 0; + int jointNib = (int)_listOfJoints[i]; + if (!_imot->getTemperature(jointNib, &_motorTemperatures[jointNib])) + { + yError() << "Unable to get motor " << jointNib << " temperature.\n"; + } + } + + sendData2OutputPort(_motorTemperatures); + + return true; +} + + +MotorTemperaturePublisher::MotorTemperaturePublisher(): _imot(nullptr) +{;} + +MotorTemperaturePublisher::~MotorTemperaturePublisher() +{;} + +// Private methods +bool MotorTemperaturePublisher::sendData2OutputPort(double * temperatures) +{ + static yarp::os::Stamp stamp; + + stamp.update(); + + Bottle &b = _outputPort.prepare(); + _outputPort.setEnvelope(stamp); + + b.clear(); + + b.addFloat64(stamp.getTime()); + for (size_t i = 0; i < _nEnabledMotors; i++) + { + b.addFloat64(temperatures[i]); + uint8_t allarm=0; + if(temperatures[i] >= _motorTemperatureLimits[i]) + allarm=1; + b.addInt8(allarm); + } + _outputPort.write(); + + + return true; +} + +bool MotorTemperaturePublisher::alloc(int nm) +{ + _motorTemperatures = allocAndCheck(nm); + _motorTemperatureLimits = allocAndCheck(nm); + + return true; + +} + +bool MotorTemperaturePublisher::dealloc() +{ + checkAndDestroy(_motorTemperatures); + checkAndDestroy(_motorTemperatureLimits); + + return true; +} diff --git a/src/tools/motorTemperaturePublisher/src/MotorTemperaturePublisher.h b/src/tools/motorTemperaturePublisher/src/MotorTemperaturePublisher.h new file mode 100644 index 0000000000..c6ab4744f6 --- /dev/null +++ b/src/tools/motorTemperaturePublisher/src/MotorTemperaturePublisher.h @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2024 iCub Facility - Istituto Italiano di Tecnologia + * Author: Jacopo Losi + * email: jacopo.losi@iit.it + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program 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 General + * Public License for more details +*/ + + +#ifndef __MOTORTEMPERATUREPUBLISHER__ +#define __MOTORTEMPERATUREPUBLISHER__ + +#include +#include +#include +#include +#include +#include +#include + +class MotorTemperaturePublisher: public yarp::os::RFModule +{ +private: + yarp::dev::IMotor *_imot; + yarp::os::BufferedPort _outputPort; + + double *_motorTemperatures; + double *_motorTemperatureLimits; + int _nmotors; + int _nEnabledMotors = 0; + + std::string _portPrefix="/5-setup"; + double _updatePeriod = 1; //seconds + std::string _robotName= "icub"; + yarp::sig::Vector _listOfJoints = 0; + + yarp::dev::PolyDriver _motionControlDevice; + + bool sendData2OutputPort(double *temperatures); + bool alloc(int nm); + bool dealloc(); + +public: + + MotorTemperaturePublisher(); + ~MotorTemperaturePublisher() override; + + MotorTemperaturePublisher(const MotorTemperaturePublisher&) = default; + MotorTemperaturePublisher(MotorTemperaturePublisher&&) = default; + MotorTemperaturePublisher& operator=(const MotorTemperaturePublisher&) = default; + MotorTemperaturePublisher& operator=(MotorTemperaturePublisher&&) = default; + + bool configure(yarp::os::ResourceFinder &rf); + bool close(); + double getPeriod(); + bool updateModule(); +}; + +#endif diff --git a/src/tools/motorTemperaturePublisher/src/main.cpp b/src/tools/motorTemperaturePublisher/src/main.cpp new file mode 100644 index 0000000000..dfef7e7db0 --- /dev/null +++ b/src/tools/motorTemperaturePublisher/src/main.cpp @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2024 iCub Facility - Istituto Italiano di Tecnologia + * Author: Jacopo Losi + * email: jacopo.losi@iit.it + * Permission is granted to copy, distribute, and/or modify this program + * under the terms of the GNU General Public License, version 2 or any + * later version published by the Free Software Foundation. + * + * A copy of the license can be found at + * http://www.robotcub.org/icub/license/gpl.txt + * + * This program 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 General + * Public License for more details +*/ + +#include "TemperatureManager.h" +#include +#include + +#include +#include + + + +using namespace yarp::os; + +int main(int argc, char * argv[]) +{ + /* initialize yarp network */ + Network yarp; + if (!yarp.checkNetwork()) + { + yError() << " YARP network does not work. Aborting..."; + return EXIT_FAILURE; + } + + /* create your module */ + TemperatureManager module; + /* prepare and configure the resource finder */ + ResourceFinder rf; + //rf.seDefaultContext("TemperatureManager"); + //rf.setDefaultConfigFile("config.ini"); + rf.configure(argc, argv); + rf.setVerbose(true); + yDebug() << "Configuring and starting module. \n"; + module.runModule(rf); // This calls configure(rf) and, upon success, the module execution begins with a call to updateModule() + yDebug()<<"Main returning..."; + return 0; +} \ No newline at end of file