diff --git a/src/devices/CMakeLists.txt b/src/devices/CMakeLists.txt index 45196492a03..9516c116e79 100644 --- a/src/devices/CMakeLists.txt +++ b/src/devices/CMakeLists.txt @@ -41,6 +41,7 @@ yarp_begin_plugin_library(yarpmod add_subdirectory(multipleanalogsensorsremapper) add_subdirectory(map2DStorage) add_subdirectory(usbCamera) + add_subdirectory(controlBoardCouplingHandler) add_subdirectory(controlBoardRemapper) add_subdirectory(JoypadControlNetUtils) add_subdirectory(frameGrabberCropper) diff --git a/src/devices/controlBoardCouplingHandler/CMakeLists.txt b/src/devices/controlBoardCouplingHandler/CMakeLists.txt new file mode 100644 index 00000000000..b007f32bd05 --- /dev/null +++ b/src/devices/controlBoardCouplingHandler/CMakeLists.txt @@ -0,0 +1,56 @@ +# SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +yarp_prepare_plugin(controlBoardCouplingHandler + CATEGORY device + TYPE ControlBoardCouplingHandler + INCLUDE ControlBoardCouplingHandler.h + GENERATE_PARSER + EXTRA_CONFIG + WRAPPER=controlboard_nws_yarp + DEFAULT ON +) + + +if(NOT SKIP_controlBoardCouplingHandler) + yarp_add_plugin(yarp_controlBoardCouplingHandler) + + target_sources(yarp_controlBoardCouplingHandler + PRIVATE + ControlBoardCouplingHandler.cpp + ControlBoardCouplingHandler.h + ControlBoardCouplingHandlerLogComponent.cpp + ControlBoardCouplingHandlerLogComponent.h + ControlBoardCouplingHandler_ParamsParser.cpp + ControlBoardCouplingHandler_ParamsParser.h) + + target_link_libraries(yarp_controlBoardCouplingHandler + PRIVATE + YARP::YARP_os + YARP::YARP_sig + YARP::YARP_dev + ) + list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS + YARP_os + YARP_sig + YARP_dev + ) + + yarp_install( + TARGETS yarp_controlBoardCouplingHandler + EXPORT YARP_${YARP_PLUGIN_MASTER} + COMPONENT ${YARP_PLUGIN_MASTER} + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR} + ) + + set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE) + + set_property(TARGET yarp_controlBoardCouplingHandler PROPERTY FOLDER "Plugins/Device") + + if(YARP_COMPILE_TESTS) + add_subdirectory(tests) + endif() + +endif() diff --git a/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler.cpp b/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler.cpp new file mode 100644 index 00000000000..6e3576daa4f --- /dev/null +++ b/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler.cpp @@ -0,0 +1,297 @@ +/* + * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include "ControlBoardCouplingHandler.h" +#include "ControlBoardCouplingHandlerLogComponent.h" + +#include +#include + +#include +#include +#include +#include +#include + +using namespace yarp::os; +using namespace yarp::dev; +using namespace yarp::sig; + +bool ControlBoardCouplingHandler::close() +{ + return detach(); +} + +bool ControlBoardCouplingHandler::open(Searchable& config) +{ + + if(!parseParams(config)) + { + yCError(CONTROLBOARDCOUPLINGHANDLER) << "Error in parsing parameters"; + return false; + } + + Property joint_coupling_config; + joint_coupling_config.fromString(config.toString()); + joint_coupling_config.unput("device"); // remove the device parameter from the config otherwise we have recursion + joint_coupling_config.put("device", m_coupling_device); + if(!jointCouplingHandler.open(joint_coupling_config)) { + yCError(CONTROLBOARDCOUPLINGHANDLER) << "Error in opening jointCouplingHandler device"; + return false; + } + + if(!jointCouplingHandler.view(iJntCoupling)) { + yCError(CONTROLBOARDCOUPLINGHANDLER) << "Error viewing the IJointCoupling interface"; + return false; + } + + configureBuffers(); + + return true; +} + +bool ControlBoardCouplingHandler::attach(yarp::dev::PolyDriver* poly) +{ + // For both cases, now configure everything that need + // all the attribute to be correctly configured + bool ok {false}; + + if (!poly->isValid()) { + yCError(CONTROLBOARDCOUPLINGHANDLER) << "Device " << poly << " to attach to is not valid ... cannot proceed"; + return false; + } + + ok = poly->view(iJntEnc); + ok = ok && poly->view(iAxInfo); + + if (!ok) + { + yCError(CONTROLBOARDCOUPLINGHANDLER, "attachAll failed some subdevice was not found or its attach failed"); + return false; + } + + return ok; +} + +void ControlBoardCouplingHandler::configureBuffers() { + if (!iJntCoupling) { + yCError(CONTROLBOARDCOUPLINGHANDLER) << "IJointCoupling interface not available"; + return; + } + size_t nrOfPhysicalJoints; + size_t nrOfActuatedAxes; + auto ok = iJntCoupling->getNrOfPhysicalJoints(nrOfPhysicalJoints); + ok = ok && iJntCoupling->getNrOfActuatedAxes(nrOfActuatedAxes); + if(!ok) + { + yCError(CONTROLBOARDCOUPLINGHANDLER) << "Error in getting the number of physical joints or actuated axes"; + return; + } + physJointsPos.resize(nrOfPhysicalJoints); + physJointsVel.resize(nrOfPhysicalJoints); + physJointsAcc.resize(nrOfPhysicalJoints); + physJointsTime.resize(nrOfPhysicalJoints); + actAxesPos.resize(nrOfActuatedAxes); + actAxesVel.resize(nrOfActuatedAxes); + actAxesAcc.resize(nrOfActuatedAxes); + actAxesTime.resize(nrOfActuatedAxes); + return; +} + +bool ControlBoardCouplingHandler::detach() +{ + iJntEnc = nullptr; + iAxInfo = nullptr; + iJntCoupling = nullptr; + jointCouplingHandler.close(); + return true; +} + + +////////////////////////////////////////////////////////////////////////////// +/// ControlBoard methods +////////////////////////////////////////////////////////////////////////////// + + +/* IEncoders */ +bool ControlBoardCouplingHandler::resetEncoder(int j) +{ + return false; +} + +bool ControlBoardCouplingHandler::resetEncoders() +{ + return false; +} + +bool ControlBoardCouplingHandler::setEncoder(int j, double val) +{ + return false; +} + +bool ControlBoardCouplingHandler::setEncoders(const double *vals) +{ + return false; +} + +bool ControlBoardCouplingHandler::getEncoder(int j, double *v) +{ + bool ok = false; + if (iJntEnc && iJntCoupling) { + ok = iJntEnc->getEncoders(actAxesPos.data()); + ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsPos(actAxesPos, physJointsPos); + if (ok) { + *v = physJointsPos[j]; + return ok; + } + } + return ok; +} + +bool ControlBoardCouplingHandler::getEncoders(double *encs) +{ + bool ok = false; + if (iJntEnc && iJntCoupling) { + ok = iJntEnc->getEncoders(actAxesPos.data()); + ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsPos(actAxesPos, physJointsPos); + if (ok) { + std::copy(physJointsPos.begin(), physJointsPos.end(), encs); + return ok; + } + } + return ok; +} + +bool ControlBoardCouplingHandler::getEncodersTimed(double *encs, double *t) +{ + bool ok = false; + if (iJntEnc && iJntCoupling) { + // TODO t has to be probably resized + ok = iJntEnc->getEncodersTimed(actAxesPos.data(), actAxesTime.data()); + ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsPos(actAxesPos, physJointsPos); + if (ok) { + std::copy(physJointsPos.begin(), physJointsPos.end(), encs); + for(size_t i = 0; i < physJointsTime.size(); i++) + { + //TODO check if this is the correct way to take the time + t[i] = actAxesTime[0]; + } + return ok; + } + } + return ok; +} + +bool ControlBoardCouplingHandler::getEncoderTimed(int j, double *v, double *t) +{ + bool ok = false; + if (iJntEnc && iJntCoupling) { + ok = iJntEnc->getEncodersTimed(actAxesPos.data(), actAxesTime.data()); + ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsPos(actAxesPos, physJointsPos); + if (ok) { + *v = physJointsPos[j]; + //TODO check if this is the correct way to take the time + *t = actAxesTime[0]; + return ok; + } + } + return ok; +} + +bool ControlBoardCouplingHandler::getEncoderSpeed(int j, double *sp) +{ + bool ok = false; + if (iJntEnc && iJntCoupling) { + ok = iJntEnc->getEncoders(actAxesPos.data()); + ok = ok && iJntEnc->getEncoderSpeeds(actAxesVel.data()); + ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsVel(actAxesPos, actAxesVel, physJointsVel); + if (ok) { + *sp = physJointsVel[j]; + return ok; + } + } + return ok; +} + +bool ControlBoardCouplingHandler::getEncoderSpeeds(double *spds) +{ + bool ok = false; + if (iJntEnc && iJntCoupling) { + ok = iJntEnc->getEncoders(actAxesPos.data()); + ok = ok && iJntEnc->getEncoderSpeeds(actAxesVel.data()); + ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsVel(actAxesPos, actAxesVel, physJointsVel); + if (ok) { + std::copy(physJointsVel.begin(), physJointsVel.end(), spds); + return ok; + } + } + return ok; +} + +bool ControlBoardCouplingHandler::getEncoderAcceleration(int j, double *acc) +{ + bool ok = false; + if (iJntEnc && iJntCoupling) { + ok = iJntEnc->getEncoders(actAxesPos.data()); + ok = ok && iJntEnc->getEncoderSpeeds(actAxesVel.data()); + ok = ok && iJntEnc->getEncoderAccelerations(actAxesAcc.data()); + ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsAcc(actAxesPos, actAxesVel, actAxesAcc, physJointsAcc); + if (ok) { + *acc = physJointsAcc[j]; + return ok; + } + } + return ok; +} + +bool ControlBoardCouplingHandler::getEncoderAccelerations(double *accs) +{ + bool ok = false; + if (iJntEnc && iJntCoupling) { + ok = iJntEnc->getEncoders(actAxesPos.data()); + ok = ok && iJntEnc->getEncoderSpeeds(actAxesVel.data()); + ok = ok && iJntEnc->getEncoderAccelerations(actAxesAcc.data()); + ok = ok && iJntCoupling->convertFromActuatedAxesToPhysicalJointsAcc(actAxesPos, actAxesVel, actAxesAcc, physJointsAcc); + if (ok) { + std::copy(physJointsAcc.begin(), physJointsAcc.end(), accs); + return ok; + } + } + return ok; +} + +/* IAxisInfo */ + +bool ControlBoardCouplingHandler::getAxes(int *ax) +{ + bool ok{false}; + if (iJntCoupling) { + size_t nrOfPhysicalJoints {0}; + ok = iJntCoupling->getNrOfPhysicalJoints(nrOfPhysicalJoints); + if (ok) { + *ax = nrOfPhysicalJoints; + return ok; + } + } + return ok; +} + +bool ControlBoardCouplingHandler::getAxisName(int j, std::string& name) +{ + bool ok{false}; + if (iJntCoupling) { + ok = iJntCoupling->getPhysicalJointName(j, name); + return ok; + } + return ok; +} + +bool ControlBoardCouplingHandler::getJointType(int j, yarp::dev::JointTypeEnum& type) +{ + bool ok{false}; + // TODO I am not sure how to handle this function + type = VOCAB_JOINTTYPE_REVOLUTE; + return true; +} diff --git a/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler.h b/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler.h new file mode 100644 index 00000000000..fe112d713a0 --- /dev/null +++ b/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler.h @@ -0,0 +1,127 @@ +/* + * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef YARP_DEV_CONTROLBOARDREMAPPER_CONTROLBOARDREMAPPER_H +#define YARP_DEV_CONTROLBOARDREMAPPER_CONTROLBOARDREMAPPER_H + +#include + +#include +#include +#include +#include +#include +#include +#include +#include "ControlBoardCouplingHandler_ParamsParser.h" + +#include +#include + + +#ifdef MSVC + #pragma warning(disable:4355) +#endif +class ControlBoardCouplingHandler : + public yarp::dev::DeviceDriver, + public yarp::dev::IEncodersTimed, + public yarp::dev::WrapperSingle, + public yarp::dev::IAxisInfo, + public ControlBoardCouplingHandler_ParamsParser { +private: + yarp::dev::PolyDriver jointCouplingHandler; + yarp::dev::IEncodersTimed *iJntEnc{nullptr}; + //yarp::dev::IControlLimits *lim; + yarp::dev::IAxisInfo *iAxInfo{nullptr}; + yarp::dev::IJointCoupling *iJntCoupling{nullptr}; + + yarp::sig::Vector physJointsPos; + yarp::sig::Vector physJointsVel; + yarp::sig::Vector physJointsAcc; + yarp::sig::Vector physJointsTime; + yarp::sig::Vector actAxesPos; + yarp::sig::Vector actAxesVel; + yarp::sig::Vector actAxesAcc; + yarp::sig::Vector actAxesTime; + + /** Verbosity of the class */ + bool _verb{false}; + + // to open ports and print more detailed debug messages + std::string partName; + + /** + * Configure buffers used by the device + */ + void configureBuffers(); + + +public: + ControlBoardCouplingHandler() = default; + ControlBoardCouplingHandler(const ControlBoardCouplingHandler&) = delete; + ControlBoardCouplingHandler(ControlBoardCouplingHandler&&) = delete; + ControlBoardCouplingHandler& operator=(const ControlBoardCouplingHandler&) = delete; + ControlBoardCouplingHandler& operator=(ControlBoardCouplingHandler&&) = delete; + ~ControlBoardCouplingHandler() override = default; + + /** + * Return the value of the verbose flag. + * @return the verbose flag. + */ + bool verbose() const { return _verb; } + + /** + * Close the device driver by deallocating all resources and closing ports. + * @return true if successful or false otherwise. + */ + bool close() override; + + + /** + * Open the device driver. + * @param prop is a Searchable object which contains the parameters. + * Allowed parameters are described in the class documentation. + */ + bool open(yarp::os::Searchable &prop) override; + + bool detach() override; + + bool attach(yarp::dev::PolyDriver* poly) override; + + /* IEncoders */ + bool resetEncoder(int j) override; + + bool resetEncoders() override; + + bool setEncoder(int j, double val) override; + + bool setEncoders(const double *vals) override; + + bool getEncoder(int j, double *v) override; + + bool getEncoders(double *encs) override; + + bool getEncodersTimed(double *encs, double *t) override; + + bool getEncoderTimed(int j, double *v, double *t) override; + + bool getEncoderSpeed(int j, double *sp) override; + + bool getEncoderSpeeds(double *spds) override; + + bool getEncoderAcceleration(int j, double *acc) override; + + bool getEncoderAccelerations(double *accs) override; + + /* IAxisInfo */ + bool getAxes(int *ax) override; + + bool getAxisName(int j, std::string &name) override; + + bool getJointType(int j, yarp::dev::JointTypeEnum &type) override; + +}; + +#endif // YARP_DEV_CONTROLBOARDREMAPPER_CONTROLBOARDREMAPPER_H diff --git a/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandlerLogComponent.cpp b/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandlerLogComponent.cpp new file mode 100644 index 00000000000..935ebe3148b --- /dev/null +++ b/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandlerLogComponent.cpp @@ -0,0 +1,8 @@ +/* + * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include "ControlBoardCouplingHandlerLogComponent.h" + +YARP_LOG_COMPONENT(CONTROLBOARDCOUPLINGHANDLER, "yarp.device.controlBoardCouplingHandler") diff --git a/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandlerLogComponent.h b/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandlerLogComponent.h new file mode 100644 index 00000000000..378ab8f2492 --- /dev/null +++ b/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandlerLogComponent.h @@ -0,0 +1,13 @@ +/* + * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef YARP_CONTROLBOARDCOUPLINGHANDLERLOGCOMPONENT_H +#define YARP_CONTROLBOARDCOUPLINGHANDLERLOGCOMPONENT_H + +#include + +YARP_DECLARE_LOG_COMPONENT(CONTROLBOARDCOUPLINGHANDLER) + +#endif // YARP_CONTROLBOARDCOUPLINGHANDLERLOGCOMPONENT_H diff --git a/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler_ParamsParser.cpp b/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler_ParamsParser.cpp new file mode 100644 index 00000000000..026a4ff8e10 --- /dev/null +++ b/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler_ParamsParser.cpp @@ -0,0 +1,105 @@ +/* + * SPDX-FileCopyrightText: 2023-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: LGPL-2.1-or-later + */ + + +// Generated by yarpDeviceParamParserGenerator (1.0) +// This is an automatically generated file. Please do not edit it. +// It will be re-generated if the cmake flag ALLOW_DEVICE_PARAM_PARSER_GERNERATION is ON. + +// Generated on: Wed Mar 6 13:18:29 2024 + + +#include "ControlBoardCouplingHandler_ParamsParser.h" +#include +#include + +namespace { + YARP_LOG_COMPONENT(ControlBoardCouplingHandlerParamsCOMPONENT, "yarp.device.ControlBoardCouplingHandler") +} + + +ControlBoardCouplingHandler_ParamsParser::ControlBoardCouplingHandler_ParamsParser() +{ +} + + +std::vector ControlBoardCouplingHandler_ParamsParser::getListOfParams() const +{ + std::vector params; + params.push_back("coupling_device"); + return params; +} + + +bool ControlBoardCouplingHandler_ParamsParser::parseParams(const yarp::os::Searchable & config) +{ + //Check for --help option + if (config.check("help")) + { + yCInfo(ControlBoardCouplingHandlerParamsCOMPONENT) << getDocumentationOfDeviceParams(); + } + + std::string config_string = config.toString(); + yarp::os::Property prop_check(config_string.c_str()); + //Parser of parameter coupling_device + { + if (config.check("coupling_device")) + { + m_coupling_device = config.find("coupling_device").asString(); + yCInfo(ControlBoardCouplingHandlerParamsCOMPONENT) << "Parameter 'coupling_device' using value:" << m_coupling_device; + } + else + { + yCError(ControlBoardCouplingHandlerParamsCOMPONENT) << "Mandatory parameter 'coupling_device' not found!"; + yCError(ControlBoardCouplingHandlerParamsCOMPONENT) << "Description of the parameter: Name of the device that handles the coupling"; + return false; + } + prop_check.unput("coupling_device"); + } + + /* + //This code check if the user set some parameter which are not check by the parser + //If the parser is set in strict mode, this will generate an error + if (prop_check.size() > 0) + { + bool extra_params_found = false; + for (auto it=prop_check.begin(); it!=prop_check.end(); it++) + { + if (m_parser_is_strict) + { + yCError(ControlBoardCouplingHandlerParamsCOMPONENT) << "User asking for parameter: "<name <<" which is unknown to this parser!"; + extra_params_found = true; + } + else + { + yCWarning(ControlBoardCouplingHandlerParamsCOMPONENT) << "User asking for parameter: "<< it->name <<" which is unknown to this parser!"; + } + } + + if (m_parser_is_strict && extra_params_found) + { + return false; + } + } + */ + return true; +} + + +std::string ControlBoardCouplingHandler_ParamsParser::getDocumentationOfDeviceParams() const +{ + std::string doc; + doc = doc + std::string("\n=============================================\n"); + doc = doc + std::string("This is the help for device: ControlBoardCouplingHandler\n"); + doc = doc + std::string("\n"); + doc = doc + std::string("This is the list of the parameters accepted by the device:\n"); + doc = doc + std::string("'coupling_device': Name of the device that handles the coupling\n"); + doc = doc + std::string("\n"); + doc = doc + std::string("Here are some examples of invocation command with yarpdev, with all params:\n"); + doc = doc + " yarpdev --device controlBoardCouplingHandler --coupling_device \n"; + doc = doc + std::string("Using only mandatory params:\n"); + doc = doc + " yarpdev --device controlBoardCouplingHandler --coupling_device \n"; + doc = doc + std::string("=============================================\n\n"); return doc; +} diff --git a/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler_ParamsParser.h b/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler_ParamsParser.h new file mode 100644 index 00000000000..3638cf49761 --- /dev/null +++ b/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler_ParamsParser.h @@ -0,0 +1,69 @@ +/* + * SPDX-FileCopyrightText: 2023-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: LGPL-2.1-or-later + */ + + +// Generated by yarpDeviceParamParserGenerator (1.0) +// This is an automatically generated file. Please do not edit it. +// It will be re-generated if the cmake flag ALLOW_DEVICE_PARAM_PARSER_GERNERATION is ON. + +// Generated on: Wed Mar 6 13:18:29 2024 + + +#ifndef CONTROLBOARDCOUPLINGHANDLER_PARAMSPARSER_H +#define CONTROLBOARDCOUPLINGHANDLER_PARAMSPARSER_H + +#include +#include +#include +#include + +/** +* This class is the parameters parser for class ControlBoardCouplingHandler. +* +* These are the used parameters: +* | Group name | Parameter name | Type | Units | Default Value | Required | Description | Notes | +* |:----------:|:---------------:|:------:|:-----:|:-------------:|:--------:|:--------------------------------------------:|:-----:| +* | - | coupling_device | string | - | - | 1 | Name of the device that handles the coupling | - | +* +* The device can be launched by yarpdev using one of the following examples: +* \code{.unparsed} +* yarpdev --device controlBoardCouplingHandler --coupling_device +* \endcode +* +* \code{.unparsed} +* yarpdev --device controlBoardCouplingHandler --coupling_device +* \endcode +* +*/ + +class ControlBoardCouplingHandler_ParamsParser : public yarp::dev::IDeviceDriverParams +{ +public: + ControlBoardCouplingHandler_ParamsParser(); + ~ControlBoardCouplingHandler_ParamsParser() override = default; + +public: + const std::string m_device_classname = {"ControlBoardCouplingHandler"}; + const std::string m_device_name = {"controlBoardCouplingHandler"}; + bool m_parser_is_strict = false; + struct parser_version_type + { + int major = 1; + int minor = 0; + }; + const parser_version_type m_parser_version = {}; + + const std::string m_coupling_device_defaultValue = {""}; + + std::string m_coupling_device = {}; //This default value is autogenerated. It is highly recommended to provide a suggested value also for mandatory parameters. + + bool parseParams(const yarp::os::Searchable & config) override; + std::string getDeviceClassName() const override { return m_device_classname; } + std::string getDeviceName() const override { return m_device_name; } + std::string getDocumentationOfDeviceParams() const override; + std::vector getListOfParams() const override; +}; + +#endif diff --git a/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler_params.md b/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler_params.md new file mode 100644 index 00000000000..c45c6398b62 --- /dev/null +++ b/src/devices/controlBoardCouplingHandler/ControlBoardCouplingHandler_params.md @@ -0,0 +1,3 @@ +| Group name | Parameter name | Type | Units | Default Value | Required | Description | Notes | +|:---------------:|:------------------:|:---------------:|:-------:|:--------------:|:--------:|:----------------------------------------------:|:--------------------:|| +| | coupling_device | string | - | - | Yes | Name of the device that handles the coupling | | diff --git a/src/devices/controlBoardCouplingHandler/tests/CMakeLists.txt b/src/devices/controlBoardCouplingHandler/tests/CMakeLists.txt new file mode 100644 index 00000000000..940170479c2 --- /dev/null +++ b/src/devices/controlBoardCouplingHandler/tests/CMakeLists.txt @@ -0,0 +1,4 @@ +# SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + +create_device_test(ControlBoardCouplingHandler_t1) diff --git a/src/devices/controlBoardCouplingHandler/tests/ControlBoardCouplingHandler_t1_test.cpp b/src/devices/controlBoardCouplingHandler/tests/ControlBoardCouplingHandler_t1_test.cpp new file mode 100644 index 00000000000..640bc249153 --- /dev/null +++ b/src/devices/controlBoardCouplingHandler/tests/ControlBoardCouplingHandler_t1_test.cpp @@ -0,0 +1,139 @@ +/* + * SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +using namespace yarp::os; +using namespace yarp::dev; + + +TEST_CASE("dev::ControlBoardCouplingHandlerTest", "[yarp::dev]") +{ + YARP_REQUIRE_PLUGIN("fakeJointCoupling", "device"); + YARP_REQUIRE_PLUGIN("controlBoardCouplingHandler", "device"); + YARP_REQUIRE_PLUGIN("controlBoard_nws_yarp", "device"); + YARP_REQUIRE_PLUGIN("fakeMotionControl", "device"); + + Network::setLocalMode(true); + + SECTION("Test the controlboard coupling handler") + { + PolyDriver ddjc; + IEncodersTimed* iet=nullptr; + IAxisInfo* iai=nullptr, *iaifmc=nullptr; + IPositionControl* ipc=nullptr; + PolyDriver ddcch, ddfmc; + + ////////"Checking opening polydrivers" + { + Property p_cfg; + p_cfg.put("device", "fakeMotionControl"); + Property& grp = p_cfg.addGroup("GENERAL"); + grp.put("Joints", 3); + REQUIRE(ddfmc.open(p_cfg)); + REQUIRE(ddfmc.view(ipc)); + REQUIRE(ipc!=nullptr); + REQUIRE(ddfmc.view(iaifmc)); + } + { + Property p_cfg; + p_cfg.put("device", "controlBoardCouplingHandler"); + p_cfg.put("coupling_device", "fakeJointCoupling"); // 3 act axes, 4 phys joints + REQUIRE(ddcch.open(p_cfg)); + } + //attach + { + WrapperSingle* ww_nws; ddcch.view(ww_nws); + REQUIRE(ww_nws); + bool result_att = ww_nws->attach(&ddfmc); + REQUIRE(result_att); + } + { + REQUIRE(ddcch.view(iet)); + REQUIRE(iet!=nullptr); + REQUIRE(ddcch.view(iai)); + REQUIRE(iai!=nullptr); + } + { + int axes = 0; + CHECK(iaifmc->getAxes(&axes)); + CHECK(axes == 3); + } + { + int axes = 0; + std::string physical_joint_name; + CHECK(iai->getAxes(&axes)); + CHECK(axes == 4); + CHECK(iai->getAxisName(0, physical_joint_name)); + CHECK(physical_joint_name == "phys_joint_0"); + CHECK(iai->getAxisName(1, physical_joint_name)); + CHECK(physical_joint_name == "phys_joint_1"); + CHECK(iai->getAxisName(2, physical_joint_name)); + CHECK(physical_joint_name == "phys_joint_2"); + CHECK(iai->getAxisName(3, physical_joint_name)); + CHECK(physical_joint_name == "phys_joint_3"); + } + { + int axes = 0; + CHECK(iaifmc->getAxes(&axes)); + CHECK(axes == 3); + } + + { + // Let's move the underlyng motor control and check the coupling + CHECK(ipc->setRefSpeed(0, 1000)); + CHECK(ipc->setRefSpeed(1, 1000)); + CHECK(ipc->setRefSpeed(2, 1000)); + CHECK(ipc->positionMove(0, 10)); + CHECK(ipc->positionMove(1, 20)); + CHECK(ipc->positionMove(2, 30)); + yarp::os::Time::delay(0.1); + std::vector physJointsPos; + physJointsPos.resize(4); + CHECK(iet->getEncoders(physJointsPos.data())); + CHECK(physJointsPos[0] == 10); + CHECK(physJointsPos[1] == 20); + CHECK(physJointsPos[2] == 15); + CHECK(physJointsPos[3] == 15); + double singleEncVal {0}; + CHECK(iet->getEncoder(0, &singleEncVal)); + CHECK(singleEncVal == 10); + CHECK(iet->getEncoder(1, &singleEncVal)); + CHECK(singleEncVal == 20); + CHECK(iet->getEncoder(2, &singleEncVal)); + CHECK(singleEncVal == 15); + CHECK(iet->getEncoder(3, &singleEncVal)); + CHECK(singleEncVal == 15); + } + { + std::vector physJointsVel,physJointsAcc; + physJointsVel.resize(4); + physJointsAcc.resize(4); + CHECK(iet->getEncoderSpeeds(physJointsVel.data())); + CHECK(iet->getEncoderAccelerations(physJointsAcc.data())); + CHECK(physJointsVel[0] == 0); + CHECK(physJointsVel[1] == 0); + CHECK(physJointsVel[2] == 15); + CHECK(physJointsVel[3] == 15); + CHECK(physJointsAcc[0] == 0); + CHECK(physJointsAcc[1] == 0); + CHECK(physJointsAcc[2] == 15); + CHECK(physJointsAcc[3] == 15); + } + + } + + Network::setLocalMode(false); +}