From 0885d4dc39368b0775e48501d61c3862e28fa6b0 Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Thu, 3 Aug 2023 13:58:26 +0200 Subject: [PATCH] Removed deprecated device controlboardwrapper2 --- src/devices/CMakeLists.txt | 1 - .../controlBoardWrapper/CMakeLists.txt | 111 - .../ControlBoardLogComponent.cpp | 8 - .../ControlBoardLogComponent.h | 13 - .../ControlBoardWrapper.cpp | 872 ------ .../controlBoardWrapper/ControlBoardWrapper.h | 300 -- .../ControlBoardWrapperAmplifierControl.cpp | 308 -- .../ControlBoardWrapperAmplifierControl.h | 36 - .../ControlBoardWrapperAxisInfo.cpp | 47 - .../ControlBoardWrapperAxisInfo.h | 26 - .../ControlBoardWrapperCommon.cpp | 349 --- .../ControlBoardWrapperCommon.h | 86 - .../ControlBoardWrapperControlCalibration.cpp | 78 - .../ControlBoardWrapperControlCalibration.h | 25 - .../ControlBoardWrapperControlLimits.cpp | 104 - .../ControlBoardWrapperControlLimits.h | 24 - .../ControlBoardWrapperControlMode.cpp | 171 -- .../ControlBoardWrapperControlMode.h | 26 - .../ControlBoardWrapperCurrentControl.cpp | 183 -- .../ControlBoardWrapperCurrentControl.h | 30 - .../ControlBoardWrapperEncodersTimed.cpp | 306 -- .../ControlBoardWrapperEncodersTimed.h | 33 - .../ControlBoardWrapperImpedanceControl.cpp | 128 - .../ControlBoardWrapperImpedanceControl.h | 26 - .../ControlBoardWrapperInteractionMode.cpp | 190 -- .../ControlBoardWrapperInteractionMode.h | 27 - .../ControlBoardWrapperLogComponent.cpp | 8 - .../ControlBoardWrapperLogComponent.h | 13 - .../ControlBoardWrapperMotor.cpp | 117 - .../ControlBoardWrapperMotor.h | 27 - .../ControlBoardWrapperMotorEncoders.cpp | 313 -- .../ControlBoardWrapperMotorEncoders.h | 35 - .../ControlBoardWrapperPWMControl.cpp | 149 - .../ControlBoardWrapperPWMControl.h | 28 - .../ControlBoardWrapperPidControl.cpp | 525 ---- .../ControlBoardWrapperPidControl.h | 41 - .../ControlBoardWrapperPositionControl.cpp | 566 ---- .../ControlBoardWrapperPositionControl.h | 52 - .../ControlBoardWrapperPositionDirect.cpp | 185 -- .../ControlBoardWrapperPositionDirect.h | 28 - .../ControlBoardWrapperPreciselyTimed.cpp | 19 - .../ControlBoardWrapperPreciselyTimed.h | 22 - .../ControlBoardWrapperRemoteCalibrator.cpp | 104 - .../ControlBoardWrapperRemoteCalibrator.h | 31 - .../ControlBoardWrapperRemoteVariables.cpp | 81 - .../ControlBoardWrapperRemoteVariables.h | 24 - .../ControlBoardWrapperTorqueControl.cpp | 276 -- .../ControlBoardWrapperTorqueControl.h | 32 - .../ControlBoardWrapperVelocityControl.cpp | 198 -- .../ControlBoardWrapperVelocityControl.h | 37 - .../ControlBoard_nws_yarp.cpp | 536 ---- .../ControlBoard_nws_yarp.h | 143 - .../controlBoardWrapper/MultiJointData.h | 68 - .../controlBoardWrapper/RPCMessagesParser.cpp | 2556 ----------------- .../controlBoardWrapper/RPCMessagesParser.h | 146 - .../StreamingMessagesParser.cpp | 292 -- .../StreamingMessagesParser.h | 86 - src/devices/controlBoardWrapper/SubDevice.cpp | 231 -- src/devices/controlBoardWrapper/SubDevice.h | 135 - .../controlBoardWrapper/tests/CMakeLists.txt | 4 - .../tests/controlBoardWrapper_test.cpp | 48 - 61 files changed, 10664 deletions(-) delete mode 100644 src/devices/controlBoardWrapper/CMakeLists.txt delete mode 100644 src/devices/controlBoardWrapper/ControlBoardLogComponent.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardLogComponent.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapper.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapper.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperAmplifierControl.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperAmplifierControl.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperAxisInfo.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperAxisInfo.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperCommon.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperCommon.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperControlCalibration.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperControlCalibration.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperControlLimits.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperControlLimits.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperControlMode.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperControlMode.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperCurrentControl.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperCurrentControl.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperEncodersTimed.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperEncodersTimed.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperImpedanceControl.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperImpedanceControl.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperInteractionMode.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperInteractionMode.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperLogComponent.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperLogComponent.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperMotor.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperMotor.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperMotorEncoders.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperMotorEncoders.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperPWMControl.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperPWMControl.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperPidControl.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperPidControl.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperPositionControl.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperPositionControl.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperPositionDirect.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperPositionDirect.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperPreciselyTimed.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperPreciselyTimed.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperRemoteCalibrator.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperRemoteCalibrator.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperRemoteVariables.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperRemoteVariables.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperTorqueControl.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperTorqueControl.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperVelocityControl.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoardWrapperVelocityControl.h delete mode 100644 src/devices/controlBoardWrapper/ControlBoard_nws_yarp.cpp delete mode 100644 src/devices/controlBoardWrapper/ControlBoard_nws_yarp.h delete mode 100644 src/devices/controlBoardWrapper/MultiJointData.h delete mode 100644 src/devices/controlBoardWrapper/RPCMessagesParser.cpp delete mode 100644 src/devices/controlBoardWrapper/RPCMessagesParser.h delete mode 100644 src/devices/controlBoardWrapper/StreamingMessagesParser.cpp delete mode 100644 src/devices/controlBoardWrapper/StreamingMessagesParser.h delete mode 100644 src/devices/controlBoardWrapper/SubDevice.cpp delete mode 100644 src/devices/controlBoardWrapper/SubDevice.h delete mode 100644 src/devices/controlBoardWrapper/tests/CMakeLists.txt delete mode 100644 src/devices/controlBoardWrapper/tests/controlBoardWrapper_test.cpp diff --git a/src/devices/CMakeLists.txt b/src/devices/CMakeLists.txt index 108c3dafbaa..98a5520603c 100644 --- a/src/devices/CMakeLists.txt +++ b/src/devices/CMakeLists.txt @@ -104,7 +104,6 @@ yarp_begin_plugin_library(yarpmod add_subdirectory(VirtualAnalogWrapper) add_subdirectory(RGBDSensorClient) add_subdirectory(RGBDSensor_nws_yarp) - add_subdirectory(controlBoardWrapper) add_subdirectory(controlBoard_nws_yarp) add_subdirectory(controlBoardRemapper) add_subdirectory(RobotDescriptionClient) diff --git a/src/devices/controlBoardWrapper/CMakeLists.txt b/src/devices/controlBoardWrapper/CMakeLists.txt deleted file mode 100644 index c63b0d52d7a..00000000000 --- a/src/devices/controlBoardWrapper/CMakeLists.txt +++ /dev/null @@ -1,111 +0,0 @@ -# SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) -# SPDX-License-Identifier: BSD-3-Clause - -yarp_prepare_plugin( - controlboardwrapper2 - CATEGORY device - TYPE ControlBoardWrapper - INCLUDE ControlBoardWrapper.h - EXTRA_CONFIG - WRAPPER=controlboardwrapper2 - DEFAULT OFF - DEPENDS "NOT YARP_NO_DEPRECATED" # DEPRECATED Since YARP 3.7 -) - -if(NOT SKIP_controlboardwrapper2) - yarp_add_plugin(yarp_controlboardwrapper2) - - target_compile_definitions(yarp_controlboardwrapper2 - PRIVATE - LOG_COMPONENT="yarp.devices.controlboardwrapper2" - ) - - target_sources(yarp_controlboardwrapper2 - PRIVATE - ControlBoardWrapper.cpp - ControlBoardWrapper.h - ControlBoardWrapperAmplifierControl.cpp - ControlBoardWrapperAmplifierControl.h - ControlBoardWrapperAxisInfo.cpp - ControlBoardWrapperAxisInfo.h - ControlBoardWrapperCommon.cpp - ControlBoardWrapperCommon.h - ControlBoardWrapperControlCalibration.cpp - ControlBoardWrapperControlCalibration.h - ControlBoardWrapperControlLimits.cpp - ControlBoardWrapperControlLimits.h - ControlBoardWrapperControlMode.cpp - ControlBoardWrapperControlMode.h - ControlBoardWrapperCurrentControl.cpp - ControlBoardWrapperCurrentControl.h - ControlBoardWrapperEncodersTimed.cpp - ControlBoardWrapperEncodersTimed.h - ControlBoardWrapperImpedanceControl.cpp - ControlBoardWrapperImpedanceControl.h - ControlBoardWrapperInteractionMode.cpp - ControlBoardWrapperInteractionMode.h - ControlBoardLogComponent.cpp - ControlBoardLogComponent.h - ControlBoardWrapperMotor.cpp - ControlBoardWrapperMotor.h - ControlBoardWrapperMotorEncoders.cpp - ControlBoardWrapperMotorEncoders.h - ControlBoardWrapperPWMControl.cpp - ControlBoardWrapperPWMControl.h - ControlBoardWrapperPidControl.cpp - ControlBoardWrapperPidControl.h - ControlBoardWrapperPositionControl.cpp - ControlBoardWrapperPositionControl.h - ControlBoardWrapperPositionDirect.cpp - ControlBoardWrapperPositionDirect.h - ControlBoardWrapperPreciselyTimed.cpp - ControlBoardWrapperPreciselyTimed.h - ControlBoardWrapperRemoteCalibrator.cpp - ControlBoardWrapperRemoteCalibrator.h - ControlBoardWrapperRemoteVariables.cpp - ControlBoardWrapperRemoteVariables.h - ControlBoardWrapperTorqueControl.cpp - ControlBoardWrapperTorqueControl.h - ControlBoardWrapperVelocityControl.cpp - ControlBoardWrapperVelocityControl.h - MultiJointData.h - RPCMessagesParser.cpp - RPCMessagesParser.h - StreamingMessagesParser.cpp - StreamingMessagesParser.h - SubDevice.cpp - SubDevice.h - ) - - target_link_libraries(yarp_controlboardwrapper2 - PRIVATE - YARP::YARP_os - YARP::YARP_sig - YARP::YARP_dev - YARP::YARP_rosmsg - ) - list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS - YARP_os - YARP_sig - YARP_dev - YARP_rosmsg - ) - - yarp_install( - TARGETS yarp_controlboardwrapper2 - 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_controlboardwrapper2 PROPERTY FOLDER "Plugins/Device") - - if(YARP_COMPILE_TESTS) - add_subdirectory(tests) - endif() - -endif() diff --git a/src/devices/controlBoardWrapper/ControlBoardLogComponent.cpp b/src/devices/controlBoardWrapper/ControlBoardLogComponent.cpp deleted file mode 100644 index 38ccb98581b..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardLogComponent.cpp +++ /dev/null @@ -1,8 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardLogComponent.h" - -YARP_LOG_COMPONENT(CONTROLBOARD, LOG_COMPONENT) diff --git a/src/devices/controlBoardWrapper/ControlBoardLogComponent.h b/src/devices/controlBoardWrapper/ControlBoardLogComponent.h deleted file mode 100644 index 11ea3f0be34..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardLogComponent.h +++ /dev/null @@ -1,13 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERLOGCOMPONENT_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERLOGCOMPONENT_H - -#include - -YARP_DECLARE_LOG_COMPONENT(CONTROLBOARD) - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERLOGCOMPONENT_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapper.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapper.cpp deleted file mode 100644 index 36ac3da3b74..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapper.cpp +++ /dev/null @@ -1,872 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapper.h" - -#include -#include - -#include - -#include "ControlBoardLogComponent.h" -#include "RPCMessagesParser.h" -#include "StreamingMessagesParser.h" -#include -#include // for memset function -#include -#include -#include - -using namespace yarp::os; -using namespace yarp::dev; -using namespace yarp::dev::impl; -using namespace yarp::sig; - - -ControlBoardWrapper::ControlBoardWrapper() : - yarp::os::PeriodicThread(default_period) -{ - streaming_parser.init(this); - RPC_parser.init(this); -} - -void ControlBoardWrapper::cleanup_yarpPorts() -{ - //shut down control port - inputRPCPort.interrupt(); - inputRPCPort.removeCallbackLock(); - inputRPCPort.close(); - - inputStreamingPort.interrupt(); - inputStreamingPort.close(); - - outputPositionStatePort.interrupt(); - outputPositionStatePort.close(); - - extendedOutputStatePort.interrupt(); - extendedOutputStatePort.close(); - - rpcData.destroy(); -} - -ControlBoardWrapper::~ControlBoardWrapper() = default; - -bool ControlBoardWrapper::close() -{ - //stop thread if running - detachAll(); - - if (yarp::os::PeriodicThread::isRunning()) { - yarp::os::PeriodicThread::stop(); - } - - if (useROS != ROS_only) { - cleanup_yarpPorts(); - } - - if (rosNode != nullptr) { - delete rosNode; - rosNode = nullptr; - } - - //if we own a deviced we have to close and delete it - if (ownDevices) { - // we should have created a new devices which we need to delete - if (subDeviceOwned != nullptr) { - subDeviceOwned->close(); - delete subDeviceOwned; - subDeviceOwned = nullptr; - } - } else { - detachAll(); - } - return true; -} - -bool ControlBoardWrapper::checkPortName(Searchable& params) -{ - /* see if rootName is present in the config file, this param is not used from long time, so it'll be - * marked as deprecated. - */ - if (params.check("rootName")) { - yCWarning(CONTROLBOARD) << - "************************************************************************************\n" - "* controlboardwrapper2 is using the deprecated parameter 'rootName' for port name, *\n" - "* It has to be removed and substituted with: *\n" - "* name: full port prefix name with leading '/', e.g. /robotName/part/ *\n" - "************************************************************************************"; - rootName = params.find("rootName").asString(); - } - - // find name as port name (similar both in new and old policy - if (!params.check("name")) { - yCError(CONTROLBOARD) << - "************************************************************************************\n" - "* controlboardwrapper2 missing mandatory parameter 'name' for port name, usage is: *\n" - "* name: full port prefix name with leading '/', e.g. /robotName/part/ *\n" - "************************************************************************************"; - return false; - } - - partName = params.find("name").asString(); - if (partName[0] != '/') { - yCWarning(CONTROLBOARD) << - "************************************************************************************\n" - "* controlboardwrapper2 'name' parameter for port name does not follow convention, *\n" - "* it MUST start with a leading '/' since it is used as the full prefix port name *\n" - "* name: full port prefix name with leading '/', e.g. /robotName/part/ *\n" - "* A temporary automatic fix will be done for you, but please fix your config file *\n" - "************************************************************************************"; - rootName = "/" + partName; - } else { - rootName = partName; - } - - return true; -} - -bool ControlBoardWrapper::checkROSParams(Searchable& config) -{ - // check for ROS parameter group - if (!config.check("ROS")) { - useROS = ROS_disabled; - return true; - } - - yCInfo(CONTROLBOARD) << "ROS group was FOUND in config file."; - - Bottle& rosGroup = config.findGroup("ROS"); - if (rosGroup.isNull()) { - yCError(CONTROLBOARD) << partName << "ROS group params is not a valid group or empty"; - useROS = ROS_config_error; - return false; - } - - // check for useROS parameter - if (!rosGroup.check("useROS")) { - yCError(CONTROLBOARD) << partName << " cannot find useROS parameter, mandatory when using ROS message. \n \ - Allowed values are true, false, ROS_only"; - useROS = ROS_config_error; - return false; - } - std::string ros_use_type = rosGroup.find("useROS").asString(); - if (ros_use_type == "false") { - yCInfo(CONTROLBOARD) << partName << "useROS topic if set to 'false'"; - useROS = ROS_disabled; - return true; - } - - if (ros_use_type == "true") { - yCInfo(CONTROLBOARD) << partName << "useROS topic if set to 'true'"; - useROS = ROS_enabled; - } else if (ros_use_type == "only") { - yCInfo(CONTROLBOARD) << partName << "useROS topic if set to 'only"; - useROS = ROS_only; - } else { - yCInfo(CONTROLBOARD) << partName << "useROS parameter is seet to unvalid value ('" << ros_use_type << "'), supported values are 'true', 'false', 'only'"; - useROS = ROS_config_error; - return false; - } - - // check for ROS_nodeName parameter - if (!rosGroup.check("ROS_nodeName")) { - yCError(CONTROLBOARD) << partName << " cannot find ROS_nodeName parameter, mandatory when using ROS message"; - useROS = ROS_config_error; - return false; - } - rosNodeName = rosGroup.find("ROS_nodeName").asString(); - if(rosNodeName[0] != '/'){ - yCError(CONTROLBOARD) << "ros node name must begin with an initial /"; - return false; - } - yCInfo(CONTROLBOARD) << partName << "rosNodeName is " << rosNodeName; - - // check for ROS_topicName parameter - if (!rosGroup.check("ROS_topicName")) { - yCError(CONTROLBOARD) << partName << " cannot find rosTopicName parameter, mandatory when using ROS message"; - useROS = ROS_config_error; - return false; - } - rosTopicName = rosGroup.find("ROS_topicName").asString(); - yCInfo(CONTROLBOARD) << partName << "rosTopicName is " << rosTopicName; - - // check for rosNodeName parameter - // UPDATE: joint names are got from MotionControl subdevice now. - // An error should be thrown later on in case we fail getting names from device - if (!rosGroup.check("jointNames")) { - yCInfo(CONTROLBOARD) << partName << "ROS topic has been required, jointNames will be got from motionControl subdevice."; - } else // if names are there, store them. They will be used for back compatibility if old policy is used. - { - Bottle nameList = rosGroup.findGroup("jointNames").tail(); - if (nameList.isNull()) { - yCError(CONTROLBOARD) << partName << " jointNames not found"; - useROS = ROS_config_error; - return false; - } - - if (nameList.size() != static_cast(controlledJoints)) { - yCError(CONTROLBOARD) << partName << " jointNames incorrect number of entries. \n jointNames is " << nameList.toString() << "while expected length is " << controlledJoints; - useROS = ROS_config_error; - return false; - } - - jointNames.clear(); - for (size_t i = 0; i < controlledJoints; i++) { - jointNames.push_back(nameList.get(i).toString()); - } - } - return true; -} - -bool ControlBoardWrapper::initialize_ROS() -{ - bool success = false; - switch (useROS) { - case ROS_enabled: - case ROS_only: { - rosNode = new yarp::os::Node(rosNodeName); // add a ROS node - - if (rosNode == nullptr) { - yCError(CONTROLBOARD) << " opening " << rosNodeName << " Node, check your yarp-ROS network configuration"; - success = false; - break; - } - - if (!rosPublisherPort.topic(rosTopicName)) { - yCError(CONTROLBOARD) << " opening " << rosTopicName << " Topic, check your yarp-ROS network configuration"; - success = false; - break; - } - success = true; - } break; - - case ROS_disabled: { - yCInfo(CONTROLBOARD) << partName << ": no ROS initialization required"; - success = true; - } break; - - case ROS_config_error: { - yCError(CONTROLBOARD) << partName << " ROS parameter are not correct, check your configuration file"; - success = false; - } break; - - default: - { - yCError(CONTROLBOARD) << partName << " something went wrong with ROS configuration, we should never be here!!!"; - success = false; - } break; - } - return success; -} - -bool ControlBoardWrapper::initialize_YARP(yarp::os::Searchable& prop) -{ - bool success = false; - - switch (useROS) { - case ROS_only: { - yCInfo(CONTROLBOARD) << partName << " No YARP initialization required"; - success = true; - } break; - - default: - { - yCInfo(CONTROLBOARD) << partName << " initting YARP initialization"; - // initialize callback - if (!streaming_parser.initialize()) { - yCError(CONTROLBOARD) << "Error could not initialize callback object"; - success = false; - break; - } - - rootName = prop.check("rootName", Value("/"), "starting '/' if needed.").asString(); - partName = prop.check("name", Value("controlboard"), "prefix for port names").asString(); - rootName += (partName); - if (rootName.find("//") != std::string::npos) { - rootName.replace(rootName.find("//"), 2, "/"); - } - - ///// We now open ports, then attach the readers or callbacks - if (!inputRPCPort.open((rootName + "/rpc:i"))) { - yCError(CONTROLBOARD) << "Error opening port " << rootName + "/rpc:i"; - success = false; - break; - } - inputRPCPort.setReader(RPC_parser); - inputRPC_buffer.attach(inputRPCPort); - RPC_parser.attach(inputRPC_buffer); - - if (!inputStreamingPort.open(rootName + "/command:i")) { - yCError(CONTROLBOARD) << "Error opening port " << rootName + "/rpc:i"; - success = false; - break; - } - - // attach callback. - inputStreamingPort.setStrict(); - inputStreamingPort.useCallback(streaming_parser); - - if (!outputPositionStatePort.open(rootName + "/state:o")) { - yCError(CONTROLBOARD) << "Error opening port " << rootName + "/state:o"; - success = false; - break; - } - - // new extended output state port - if (!extendedOutputStatePort.open(rootName + "/stateExt:o")) { - yCError(CONTROLBOARD) << "Error opening port " << rootName + "/state:o"; - success = false; - break; - } - extendedOutputState_buffer.attach(extendedOutputStatePort); - success = true; - } break; - } // end switch - - // cleanup if something went wrong - if (!success) { - cleanup_yarpPorts(); - } - return success; -} - - -bool ControlBoardWrapper::open(Searchable& config) -{ - yCWarning(CONTROLBOARD) << "The 'controlboardwrapper2' device is deprecated in favour of 'controlboardremapper' + 'controlBoard_nws_yarp'."; - yCWarning(CONTROLBOARD) << "The old device is no longer supported, and it will be deprecated in YARP 3.7 and removed in YARP 4."; - yCWarning(CONTROLBOARD) << "Please update your scripts."; - - Property prop; - prop.fromString(config.toString()); - - if (prop.check("verbose", "Deprecated flag. Use log components instead")) { - yCWarning(CONTROLBOARD) << "'verbose' flag is deprecated. Use log components instead"; - } - - if (!checkPortName(config)) { - yCError(CONTROLBOARD) << "'portName' was not correctly set, check you r configuration file"; - return false; - } - - // check FIRST for deprecated parameter - if (prop.check("threadrate")) { - yCError(CONTROLBOARD) << "Using removed parameter 'threadrate', use 'period' instead"; - return false; - } - - // NOW, check for correct parameter, so if both are present we use the correct one - if (prop.check("period")) { - if (!prop.find("period").isInt32()) { - yCError(CONTROLBOARD) << "'period' parameter is not an integer value"; - return false; - } - period = prop.find("period").asInt32() / 1000.0; - if (period <= 0) { - yCError(CONTROLBOARD) << "'period' parameter is not valid, read value is" << period; - return false; - } - } else { - yCDebug(CONTROLBOARD) << "'period' parameter missing, using default thread period = 20ms"; - period = default_period; - } - - // check if we need to create subdevice or if they are - // passed later on thorugh attachAll() - if (prop.check("subdevice")) { - ownDevices = true; - prop.setMonitor(config.getMonitor()); - if (!openAndAttachSubDevice(prop)) { - yCError(CONTROLBOARD, "Error while opening subdevice"); - return false; - } - } else { - ownDevices = false; - if (!openDeferredAttach(prop)) { - return false; - } - } - - // using controlledJoints here will allocate more memory than required, but not so much. - rpcData.resize(device.subdevices.size(), controlledJoints, &device); - - /* This must be after the openAndAttachSubDevice() or openDeferredAttach() in order to have the correct number of controlledJoints, - but before the initialize_ROS and initialize_YARP */ - if (!checkROSParams(config)) { - yCError(CONTROLBOARD) << partName << " ROS parameter are not correct, check your configuration file"; - return false; - } - - // call ROS node/topic initialization, if needed - if (!initialize_ROS()) { - return false; - } - - // call YARP port initialization, if needed - if (!initialize_YARP(prop)) { - yCError(CONTROLBOARD) << partName << "Something wrong when initting yarp ports"; - return false; - } - - times.resize(controlledJoints); - ros_struct.name.resize(controlledJoints); - ros_struct.position.resize(controlledJoints); - ros_struct.velocity.resize(controlledJoints); - ros_struct.effort.resize(controlledJoints); - - // In case attach is not deferred and the controlboard already owns a valid device - // we can start the thread. Otherwise this will happen when attachAll is called - if (ownDevices) { - PeriodicThread::setPeriod(period); - if (!PeriodicThread::start()) { - return false; - } - } - return true; -} - - -// Default usage -// Open the wrapper only, the attach method needs to be called before using it -bool ControlBoardWrapper::openDeferredAttach(Property& prop) -{ - if (!prop.check("networks", "list of networks merged by this wrapper")) { - yCError(CONTROLBOARD) << "List of networks to attach to was not found."; - return false; - } - - Bottle* nets = prop.find("networks").asList(); - if (nets == nullptr) { - yCError(CONTROLBOARD) << "Error parsing parameters: \"networks\" should be followed by a list"; - return false; - } - - if (!prop.check("joints", "number of joints of the part")) { - return false; - } - - controlledJoints = prop.find("joints").asInt32(); - - size_t nsubdevices = nets->size(); - device.lut.resize(controlledJoints); - device.subdevices.resize(nsubdevices); - - // configure the devices - size_t totalJ = 0; - for (size_t k = 0; k < nets->size(); k++) { - Bottle parameters; - size_t wBase; - size_t wTop; - size_t base; - size_t top; - - parameters = prop.findGroup(nets->get(k).asString()); - - if (parameters.size() == 2) { - Bottle* bot = parameters.get(1).asList(); - Bottle tmpBot; - if (bot == nullptr) { - // probably data are not passed in the correct way, try to read them as a string. - std::string bString(parameters.get(1).asString()); - tmpBot.fromString(bString); - - if (tmpBot.size() != 4) { - yCError(CONTROLBOARD) << "Error: check network parameters in part description" - << "--> I was expecting " << nets->get(k).asString() << " followed by a list of four integers in parenthesis" - << "Got: " << parameters.toString(); - return false; - } - - bot = &tmpBot; - } - - // If I came here, bot is correct - wBase = static_cast(bot->get(0).asInt32()); - wTop = static_cast(bot->get(1).asInt32()); - base = static_cast(bot->get(2).asInt32()); - top = static_cast(bot->get(3).asInt32()); - } else if (parameters.size() == 5) { - yCError(CONTROLBOARD) << "Parameter networks use deprecated syntax"; - wBase = static_cast(parameters.get(1).asInt32()); - wTop = static_cast(parameters.get(2).asInt32()); - base = static_cast(parameters.get(3).asInt32()); - top = static_cast(parameters.get(4).asInt32()); - } else { - yCError(CONTROLBOARD) << "Error: check network parameters in part description" - << "--> I was expecting " << nets->get(k).asString() << " followed by a list of four integers in parenthesis" - << "Got: " << parameters.toString(); - return false; - } - - SubDevice* tmpDevice = device.getSubdevice(k); - if (!tmpDevice) { - yCError(CONTROLBOARD) << "Get of subdevice returned null"; - return false; - } - - size_t axes = top - base + 1; - if (!tmpDevice->configure(wBase, wTop, base, top, axes, nets->get(k).asString(), getId())) { - yCError(CONTROLBOARD) << "Configure of subdevice ret false"; - return false; - } - - // Check input values are in range - if ((wBase == static_cast(-1)) || (wBase >= controlledJoints)) { - yCError(CONTROLBOARD) << "Input configuration for device " << partName << "has a wrong attach map." - << "First index " << wBase << "must be inside range from 0 to 'joints' (" << controlledJoints << ")"; - return false; - } - - if ((wTop == static_cast(-1)) || (wTop >= controlledJoints)) { - yCError(CONTROLBOARD) << "Input configuration for device " << partName << "has a wrong attach map." - << "Second index " << wTop << "must be inside range from 0 to 'joints' (" << controlledJoints << ")"; - return false; - } - - if (wBase > wTop) { - yCError(CONTROLBOARD) << "Input configuration for device " << partName << "has a wrong attach map." - << "First index " << wBase << "must be lower than second index " << wTop; - return false; - } - - for (size_t j = wBase, jInDev = base; j <= wTop; j++, jInDev++) { - device.lut[j].deviceEntry = k; - device.lut[j].offset = static_cast(j - wBase); - device.lut[j].jointIndexInDev = jInDev; - } - - totalJ += axes; - } - - if (totalJ != controlledJoints) { - yCError(CONTROLBOARD) << "Error total number of mapped joints (" << totalJ << ") does not correspond to part joints (" << controlledJoints << ")"; - return false; - } - return true; -} - -// For the simulator, if a subdevice parameter is given to the wrapper, it will -// open it and attach to immediately. -bool ControlBoardWrapper::openAndAttachSubDevice(Property& prop) -{ - Property p; - subDeviceOwned = new PolyDriver; - p.fromString(prop.toString()); - - std::string subdevice = prop.find("subdevice").asString(); - p.setMonitor(prop.getMonitor(), subdevice.c_str()); // pass on any monitoring - p.unput("device"); - p.put("device", subdevice); // subdevice was already checked before - - // if errors occurred during open, quit here. - yCDebug(CONTROLBOARD, "opening subdevice"); - subDeviceOwned->open(p); - - if (!subDeviceOwned->isValid()) { - yCError(CONTROLBOARD, "opening subdevice... FAILED"); - return false; - } - - yarp::dev::IEncoders* iencs = nullptr; - - subDeviceOwned->view(iencs); - - if (iencs == nullptr) { - yCError(CONTROLBOARD, "Opening IEncoders interface of subdevice... FAILED"); - return false; - } - - int tmp_axes; - bool getAx = iencs->getAxes(&tmp_axes); - - if (!getAx) { - yCError(CONTROLBOARD, "Calling getAxes of subdevice... FAILED"); - return false; - } - controlledJoints = static_cast(tmp_axes); - - yCDebug(CONTROLBOARD, "Joints parameter is %zu", controlledJoints); - - - device.lut.resize(controlledJoints); - device.subdevices.resize(1); - - // configure the device - size_t base = 0; - size_t top = controlledJoints - 1; - - size_t wbase = base; - size_t wtop = top; - SubDevice* tmpDevice = device.getSubdevice(0); - - std::string subDevName((partName + "_" + subdevice)); - if (!tmpDevice->configure(wbase, wtop, base, top, controlledJoints, subDevName, getId())) { - yCError(CONTROLBOARD) << "Configure of subdevice ret false"; - return false; - } - - for (size_t j = 0; j < controlledJoints; j++) { - device.lut[j].deviceEntry = 0; - device.lut[j].offset = j; - } - - - if (!device.subdevices[0].attach(subDeviceOwned, subDevName)) { - return false; - } - - // initialization. - RPC_parser.initialize(); - updateAxisName(); - calculateMaxNumOfJointsInDevices(); - return true; -} - -void ControlBoardWrapper::calculateMaxNumOfJointsInDevices() -{ - device.maxNumOfJointsInDevices = 0; - - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (p->totalAxis > device.maxNumOfJointsInDevices) { - device.maxNumOfJointsInDevices = p->totalAxis; - } - } -} - -bool ControlBoardWrapper::updateAxisName() -{ - // If attached device has axisName update the internal values, otherwise keep the on from wrapper - // config file, if any. - // IMPORTANT!! This function has to be called BEFORE the thread starts, because if ROS is enabled, - // the name has to be correct right from the first message!! - - // FOR THE FUTURE: this double version will be dropped because it'll create confusion. Only the names - // from the motionControl device will be considered good - - // no need to update this variable if we are not using ROS. YARP RPC will always call the sudevice. - if (useROS == ROS_disabled) { - return true; - } - - std::string tmp; - // I need a temporary vector because if I'm wrapping more than one subdevice, and one of them - // does not have the axesName, then I'd stick with the old names from wrpper config file, if any. - std::vector tmpVect; - bool ret = true; - - tmpVect.clear(); - for (size_t i = 0; i < controlledJoints; i++) { - if ((ret = getAxisName(i, tmp) && ret)) { - std::string tmp2(tmp); - tmpVect.push_back(tmp2); - } - } - - if (ret) { - if (!jointNames.empty()) { - yCWarning(CONTROLBOARD) << "Found 2 instance of jointNames parameter: one in the wrapper [ROS] group and another one in the subdevice, the latter one will be used."; - std::string fullNames; - for (size_t i = 0; i < controlledJoints; i++) { - fullNames.append(tmpVect[i]); - } - } - - jointNames.clear(); - jointNames = tmpVect; - } else { - if (jointNames.empty()) { - yCError(CONTROLBOARD) << "Joint names were not found! they are mandatory when using ROS topic"; - return false; - } - - yCWarning(CONTROLBOARD) << "\n" << - "************************************************************************************************** \n" << - "* Joint names for ROS topic were found in the [ROS] group in the wrapper config file for\n" << - "* '" << partName << "' device.\n" << - "* They should be in the MotionControl device(s) instead. Please update the config files.\n" << - "**************************************************************************************************"; - } - return true; -} - - -bool ControlBoardWrapper::attachAll(const PolyDriverList& polylist) -{ - //check if we already instantiated a subdevice previously - if (ownDevices) { - return false; - } - - for (int p = 0; p < polylist.size(); p++) { - // look if we have to attach to a calibrator - std::string tmpKey = polylist[p]->key; - if (tmpKey == "Calibrator" || tmpKey == "calibrator") { - // Set the IRemoteCalibrator interface, the wrapper must point to the calibrator device - yarp::dev::IRemoteCalibrator* calibrator; - polylist[p]->poly->view(calibrator); - - IRemoteCalibrator::setCalibratorDevice(calibrator); - continue; - } - - // find appropriate entry in list of subdevices and attach - size_t k = 0; - for (k = 0; k < device.subdevices.size(); k++) { - if (device.subdevices[k].id == tmpKey) { - if (!device.subdevices[k].attach(polylist[p]->poly, tmpKey)) { - yCError(CONTROLBOARD, "Attach to subdevice %s failed", polylist[p]->key.c_str()); - return false; - } - } - } - } - - //check if all devices are attached to the driver - bool ready = true; - for (auto& subdevice : device.subdevices) { - if (!subdevice.isAttached()) { - yCError(CONTROLBOARD, "Device %s was not found in the list passed to attachAll", subdevice.id.c_str()); - ready = false; - } - } - - if (!ready) { - yCError(CONTROLBOARD, "AttachAll failed, some subdevice was not found or its attach failed"); - std::stringstream ss; - for (int p = 0; p < polylist.size(); p++) { - ss << polylist[p]->key.c_str() << " "; - } - yCError(CONTROLBOARD, "List of devices keys passed to attachAll: %s", ss.str().c_str()); - return false; - } - - // initialization. - RPC_parser.initialize(); - - updateAxisName(); - calculateMaxNumOfJointsInDevices(); - PeriodicThread::setPeriod(period); - return PeriodicThread::start(); -} - -bool ControlBoardWrapper::detachAll() -{ - //check if we already instantiated a subdevice previously - if (ownDevices) { - return false; - } - - if (yarp::os::PeriodicThread::isRunning()) { - yarp::os::PeriodicThread::stop(); - } - - int devices = device.subdevices.size(); - - for (int k = 0; k < devices; k++) { - SubDevice* sub = device.getSubdevice(k); - if (sub) { - sub->detach(); - } - } - - IRemoteCalibrator::releaseCalibratorDevice(); - return true; -} - -void ControlBoardWrapper::run() -{ - // check we are not overflowing with input messages - if (inputStreamingPort.getPendingReads() >= 20) { - yCWarning(CONTROLBOARD) << "Number of streaming input messages to be read is " << inputStreamingPort.getPendingReads() << " and can overflow"; - } - - // Small optimization: Avoid to call getEncoders twice, one for YARP port - // and again for ROS topic. - // - // Calling getStuff here on ros_struct because it is a class member, hence - // always available. In the other side, to have the yarp struct to write into - // it will be rewuired to call port.prepare, that it is something I should - // not do if the wrapper is in ROS_only configuration. - - bool positionsOk = getEncodersTimed(ros_struct.position.data(), times.data()); - bool speedsOk = getEncoderSpeeds(ros_struct.velocity.data()); - bool torqueOk = getTorques(ros_struct.effort.data()); - - // Update the port envelope time by averaging all timestamps - timeMutex.lock(); - time.update(std::accumulate(times.begin(), times.end(), 0.0) / controlledJoints); - yarp::os::Stamp averageTime = time; - timeMutex.unlock(); - - if (useROS != ROS_only) { - // handle stateExt first - jointData& yarp_struct = extendedOutputState_buffer.get(); - - yarp_struct.jointPosition.resize(controlledJoints); - yarp_struct.jointVelocity.resize(controlledJoints); - yarp_struct.jointAcceleration.resize(controlledJoints); - yarp_struct.motorPosition.resize(controlledJoints); - yarp_struct.motorVelocity.resize(controlledJoints); - yarp_struct.motorAcceleration.resize(controlledJoints); - yarp_struct.torque.resize(controlledJoints); - yarp_struct.pwmDutycycle.resize(controlledJoints); - yarp_struct.current.resize(controlledJoints); - yarp_struct.controlMode.resize(controlledJoints); - yarp_struct.interactionMode.resize(controlledJoints); - - // Get already stored data from before. This is to avoid a double call to HW device, - // which may require more time. // - yarp_struct.jointPosition_isValid = positionsOk; - std::copy(ros_struct.position.begin(), ros_struct.position.end(), yarp_struct.jointPosition.begin()); - - yarp_struct.jointVelocity_isValid = speedsOk; - std::copy(ros_struct.velocity.begin(), ros_struct.velocity.end(), yarp_struct.jointVelocity.begin()); - - yarp_struct.torque_isValid = torqueOk; - std::copy(ros_struct.effort.begin(), ros_struct.effort.end(), yarp_struct.torque.begin()); - - // Get remaining data from HW - yarp_struct.jointAcceleration_isValid = getEncoderAccelerations(yarp_struct.jointAcceleration.data()); - yarp_struct.motorPosition_isValid = getMotorEncoders(yarp_struct.motorPosition.data()); - yarp_struct.motorVelocity_isValid = getMotorEncoderSpeeds(yarp_struct.motorVelocity.data()); - yarp_struct.motorAcceleration_isValid = getMotorEncoderAccelerations(yarp_struct.motorAcceleration.data()); - yarp_struct.pwmDutycycle_isValid = getDutyCycles(yarp_struct.pwmDutycycle.data()); - yarp_struct.current_isValid = ControlBoardWrapperCommon::getCurrents(yarp_struct.current.data()); - yarp_struct.controlMode_isValid = getControlModes(yarp_struct.controlMode.data()); - yarp_struct.interactionMode_isValid = getInteractionModes(reinterpret_cast(yarp_struct.interactionMode.data())); - - extendedOutputStatePort.setEnvelope(averageTime); - extendedOutputState_buffer.write(); - - // handle state:o - yarp::sig::Vector& v = outputPositionStatePort.prepare(); - v.resize(controlledJoints); - std::copy(yarp_struct.jointPosition.begin(), yarp_struct.jointPosition.end(), v.begin()); - - outputPositionStatePort.setEnvelope(averageTime); - outputPositionStatePort.write(); - } - - if (useROS != ROS_disabled) { - // Data from HW have been gathered few lines before - JointTypeEnum jType; - for (size_t i = 0; i < controlledJoints; i++) { - getJointType(i, jType); - if (jType == VOCAB_JOINTTYPE_REVOLUTE) { - ros_struct.position[i] = convertDegreesToRadians(ros_struct.position[i]); - ros_struct.velocity[i] = convertDegreesToRadians(ros_struct.velocity[i]); - } - } - - ros_struct.name = jointNames; - - ros_struct.header.seq = rosMsgCounter++; - ros_struct.header.stamp = averageTime.getTime(); - - rosPublisherPort.write(ros_struct); - } -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapper.h b/src/devices/controlBoardWrapper/ControlBoardWrapper.h deleted file mode 100644 index b6c9c39ebd5..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapper.h +++ /dev/null @@ -1,300 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPER_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPER_H - - -// ControlBoardWrapper -// A modified version of the remote control board class -// which remaps joints, it can also merge networks into a single part. - - -#include -#include - -#include -#include -#include -#include -#include - -#include // struct for YARP extended port - -#include -#include -#include - -// ROS state publisher -#include -#include -#include -#include - - -#include "ControlBoardWrapperPidControl.h" -#include "ControlBoardWrapperPositionControl.h" -#include "ControlBoardWrapperPositionDirect.h" -#include "ControlBoardWrapperVelocityControl.h" -#include "ControlBoardWrapperEncodersTimed.h" -#include "ControlBoardWrapperMotor.h" -#include "ControlBoardWrapperMotorEncoders.h" -#include "ControlBoardWrapperAmplifierControl.h" -#include "ControlBoardWrapperControlLimits.h" -#include "ControlBoardWrapperRemoteCalibrator.h" -#include "ControlBoardWrapperControlCalibration.h" -#include "ControlBoardWrapperTorqueControl.h" -#include "ControlBoardWrapperImpedanceControl.h" -#include "ControlBoardWrapperControlMode.h" -#include "ControlBoardWrapperAxisInfo.h" -#include "ControlBoardWrapperInteractionMode.h" -#include "ControlBoardWrapperRemoteVariables.h" -#include "ControlBoardWrapperPWMControl.h" -#include "ControlBoardWrapperCurrentControl.h" -#include "ControlBoardWrapperPreciselyTimed.h" - -#include "SubDevice.h" -#include "StreamingMessagesParser.h" -#include "RPCMessagesParser.h" - - -#ifdef MSVC - #pragma warning(disable:4355) -#endif - -/* - * To optimize memory allocation, for group of joints we can have one mem reserver for rpc port - * and on e for streaming. The size could be numOfSubDevices*maxNumOfjointForSubdevice. - * (we could also use the actual joint number for each subdevice using a for loop). TODO - */ - -class CommandsHelper; -class SubDevice; - - -/** - * @ingroup dev_impl_wrapper dev_impl_deprecated - * - * \brief `controlboardwrapper2` *deprecated*: An updated version of the controlBoard network wrapper. - * It can merge together more than one control board device, or use only a - * portion of it by remapping functionality. - * Allows also deferred attach/detach of a subdevice. - * - * \section controlboardwrapper2_device_parameters Description of input parameters - * - * Parameters required by this device are: - * | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes | - * |:--------------:|:--------------:|:-------:|:--------------:|:-------------:|:--------------------------: |:-----------------------------------------------------------------:|:-----:| - * | name | - | string | - | - | Yes | full name of the port opened by the device, like /robotName/part/ | MUST start with a '/' character | - * | period | - | int | ms | 20 | No | refresh period of the broadcasted values in ms | optional, default 20ms | - * | subdevice | - | string | - | - | alternative to netwok group | name of the subdevice to instantiate | when used, parameters for the subdevice must be provided as well | - * | networks | - | group | - | - | alternative to subdevice | this is expected to be a group parameter in xml format, a list in .ini file format. SubParameter are mandatory if this is used| - | - * | - | networkName_1 | 4 * int | joint number | - | if networks is used | describe how to match subdevice_1 joints with the wrapper joints. First 2 numbers indicate first/last wrapper joint, last 2 numbers are subdevice first/last joint | The joints are intended to be consequent | - * | - | ... | 4 * int | joint number | - | if networks is used | same as above | The joints are intended to be consequent | - * | - | networkName_n | 4 * int | joint number | - | if networks is used | same as above | The joints are intended to be consequent | - * | - | joints | int | - | - | if networks is used | total number of joints handled by the wrapper | MUST match the sum of joints from all the networks | - * | ROS | - | group | - | - | No | Group containing parameter for ROS topic initialization | if missing, it is assumed to not use ROS topics | - * | - | useROS | string | true/false/only| - | if ROS group is present | set 'true' to have both yarp ports and ROS topic, set 'only' to have only ROS topic and no yarp port| - | - * | - | ROS_topicName | string | - | - | if ROS group is present | set the name for ROS topic | must start with a leading '/' | - * | - | ROS_nodeName | string | - | - | if ROS group is present | set the name for ROS node | must start with a leading '/' | - * | - | jointNames | string | - | - | deprecated | joints names are now got from attached motionControl device | names order must match with the joint order, from 0 to N | - * - * ROS message type used is sensor_msgs/JointState.msg (http://docs.ros.org/api/sensor_msgs/html/msg/JointState.html) - * Some example of configuration files: - * - * Configuration file using .ini format, using subdevice keyword. - * - * \code{.unparsed} - * device controlboardwrapper2 - * subdevice fakebot - * name /icub/head - * - * ** parameter for 'fakebot' subdevice follows here ** - * ... - * \endcode - * - * XML format, using 'networks' keyword. This file is meant to be used in junction with yarprobotinterface executable, - * therefore has an addictional section at the end. - * - * \code{.xml} - * - * - * (0 3 0 3) - * (4 6 0 2) - * - * - * 20 - * /icub/left_arm - * 7 - * - * - * - * - * - * - * left_upper_arm_mc - * left_lower_arm_mc - * - * - * - * \endcode - * - * Configuration file using .ini format, using network keyword - * - * \code{.unparsed} - * device controlboardwrapper2 - * name /robotName/partName - * period 10 - * networks (net_larm net_lhand) - * joints 16 - * net_larm 0 3 0 3 - * net_lhand 4 6 0 2 - * \endcode - * - * Configuration for ROS topic using .ini format - * \code{.unparsed} - * [ROS] - * useROS true - * ROS_topicName /JointState - * ROS_nodeName /robotPublisher - * jointNames r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow r_wrist_prosup r_wrist_pitch r_wrist_yaw - * \endcode - * - * Configuration for ROS topic using .xml format - * \code{.unparsed} - * - * true // use 'only' if you want only ROS topic and NOT yarp port - * /JointState - * /robotPublisher - * r_shoulder_pitch r_shoulder_roll r_shoulder_yaw r_elbow r_wrist_prosup r_wrist_pitch r_wrist_yaw - * - * \endcode - */ - -class ControlBoardWrapper : - public yarp::dev::DeprecatedDeviceDriver, - public yarp::os::PeriodicThread, - public yarp::dev::IMultipleWrapper, - virtual public ControlBoardWrapperCommon, - public ControlBoardWrapperPidControl, - public ControlBoardWrapperPositionControl, - public ControlBoardWrapperPositionDirect, - public ControlBoardWrapperVelocityControl, - public ControlBoardWrapperEncodersTimed, - public ControlBoardWrapperMotor, - public ControlBoardWrapperMotorEncoders, - public ControlBoardWrapperAmplifierControl, - public ControlBoardWrapperControlLimits, - public ControlBoardWrapperRemoteCalibrator, - public ControlBoardWrapperControlCalibration, - public ControlBoardWrapperTorqueControl, - public ControlBoardWrapperImpedanceControl, - public ControlBoardWrapperControlMode, - public ControlBoardWrapperAxisInfo, - public ControlBoardWrapperPreciselyTimed, - public ControlBoardWrapperInteractionMode, - public ControlBoardWrapperRemoteVariables, - public ControlBoardWrapperPWMControl, - public ControlBoardWrapperCurrentControl -{ -private: - std::string rootName; - - bool checkPortName(yarp::os::Searchable ¶ms); - - yarp::rosmsg::sensor_msgs::JointState ros_struct; - - yarp::os::BufferedPort outputPositionStatePort; // Port /state:o streaming out the encoder positions - yarp::os::BufferedPort inputStreamingPort; // Input streaming port for high frequency commands - yarp::os::Port inputRPCPort; // Input RPC port for set/get remote calls - yarp::sig::Vector times; // time for each joint - - // Buffer associated to the extendedOutputStatePort port; in this case we will use the type generated - // from the YARP .thrift file - yarp::os::PortWriterBuffer extendedOutputState_buffer; - yarp::os::Port extendedOutputStatePort; // Port /stateExt:o streaming out the struct with the robot data - - // ROS state publisher - ROSTopicUsageType useROS {ROS_disabled}; // decide if open ROS topic or not - std::vector jointNames; // name of the joints - std::string rosNodeName; // name of the rosNode - std::string rosTopicName; // name of the rosTopic - yarp::os::Node *rosNode {nullptr}; // add a ROS node - yarp::os::NetUint32 rosMsgCounter {0}; // incremental counter in the ROS message - yarp::os::PortWriterBuffer rosOutputState_buffer; // Buffer associated to the ROS topic - yarp::os::Publisher rosPublisherPort; // Dedicated ROS topic publisher - - yarp::os::PortReaderBuffer inputRPC_buffer; // Buffer associated to the inputRPCPort port - RPCMessagesParser RPC_parser; // Message parser associated to the inputRPCPort port - StreamingMessagesParser streaming_parser; // Message parser associated to the inputStreamingPort port - - static constexpr double default_period = 0.02; // s - double period {default_period}; - - yarp::os::Bottle getOptions(); - bool updateAxisName(); - bool checkROSParams(yarp::os::Searchable &config); - bool initialize_ROS(); - bool initialize_YARP(yarp::os::Searchable &prop); - void cleanup_yarpPorts(); - - // Default usage - // Open the wrapper only, the attach method needs to be called before using it - bool openDeferredAttach(yarp::os::Property& prop); - - // For the simulator, if a subdevice parameter is given to the wrapper, it will - // open it and attach to it immediately. - yarp::dev::PolyDriver *subDeviceOwned {nullptr}; - bool openAndAttachSubDevice(yarp::os::Property& prop); - - bool ownDevices {true}; - - void calculateMaxNumOfJointsInDevices(); - -public: - ControlBoardWrapper(); - ControlBoardWrapper(const ControlBoardWrapper&) = delete; - ControlBoardWrapper(ControlBoardWrapper&&) = delete; - ControlBoardWrapper& operator=(const ControlBoardWrapper&) = delete; - ControlBoardWrapper& operator=(ControlBoardWrapper&&) = delete; - ~ControlBoardWrapper() override; - - - /* Return id of this device */ - std::string getId() - { - return partName; - } - - /** - * 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: - * - name to specify the prefix of the port names. - * - subdevice [optional] if specified, the openAndAttachSubDevice will be - * called, otherwise openDeferredAttach is called. - * and all parameters required by the wrapper. - */ - bool open(yarp::os::Searchable& prop) override; - - bool detachAll() override; - - bool attachAll(const yarp::dev::PolyDriverList &l) override; - - /** - * The thread main loop deals with writing on ports here. - */ - void run() override; -}; - - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPER_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperAmplifierControl.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperAmplifierControl.cpp deleted file mode 100644 index 617eabf5797..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperAmplifierControl.cpp +++ /dev/null @@ -1,308 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperAmplifierControl.h" - -#include - -#include "ControlBoardLogComponent.h" - - -bool ControlBoardWrapperAmplifierControl::enableAmp(int j) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->amp) { - return p->amp->enableAmp(static_cast(off + p->base)); - } - return false; -} - - -bool ControlBoardWrapperAmplifierControl::disableAmp(int j) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - bool ret = true; - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - // Use the newer interface if available, otherwise fallback on the old one. - if (p->iMode) { - ret = p->iMode->setControlMode(static_cast(off + p->base), VOCAB_CM_IDLE); - } else { - if (p->pos) { - ret = p->amp->disableAmp(static_cast(off + p->base)); - } else { - ret = false; - } - } - return ret; -} - - -bool ControlBoardWrapperAmplifierControl::getAmpStatus(int* st) -{ - int* status = new int[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->amp) && (ret = p->amp->getAmpStatus(status))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - st[juser] = status[jdevice]; - } - } else { - printError("getAmpStatus", p->id, ret); - ret = false; - break; - } - } - - delete[] status; - return ret; -} - - -bool ControlBoardWrapperAmplifierControl::getAmpStatus(int j, int* v) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (p && p->amp) { - return p->amp->getAmpStatus(static_cast(off + p->base), v); - } - *v = 0; - return false; -} - - -bool ControlBoardWrapperAmplifierControl::setMaxCurrent(int j, double v) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->amp) { - return p->amp->setMaxCurrent(static_cast(off + p->base), v); - } - return false; -} - - -bool ControlBoardWrapperAmplifierControl::getMaxCurrent(int j, double* v) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - *v = 0.0; - return false; - } - - if (p->amp) { - return p->amp->getMaxCurrent(static_cast(off + p->base), v); - } - *v = 0.0; - return false; -} - - -bool ControlBoardWrapperAmplifierControl::getNominalCurrent(int m, double* val) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - *val = 0.0; - return false; - } - - if (!p->amp) { - *val = 0.0; - return false; - } - return p->amp->getNominalCurrent(static_cast(off + p->base), val); -} - - -bool ControlBoardWrapperAmplifierControl::getPeakCurrent(int m, double* val) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - *val = 0.0; - return false; - } - - if (!p->amp) { - *val = 0.0; - return false; - } - return p->amp->getPeakCurrent(static_cast(off + p->base), val); -} - - -bool ControlBoardWrapperAmplifierControl::setPeakCurrent(int m, const double val) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (!p->amp) { - return false; - } - return p->amp->setPeakCurrent(static_cast(off + p->base), val); -} - - -bool ControlBoardWrapperAmplifierControl::setNominalCurrent(int m, const double val) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (!p->amp) { - return false; - } - return p->amp->setNominalCurrent(static_cast(off + p->base), val); -} - - -bool ControlBoardWrapperAmplifierControl::getPWM(int m, double* val) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - SubDevice* p = device.getSubdevice(subIndex); - - yCTrace(CONTROLBOARD) << "CBW2::getPWMlimit j" << static_cast(off + p->base) << " p " << (p ? "1" : "0") << " amp " << (p->amp ? "1" : "0"); - if (!p) { - *val = 0.0; - return false; - } - - if (!p->amp) { - *val = 0.0; - return false; - } - return p->amp->getPWM(static_cast(off + p->base), val); -} - - -bool ControlBoardWrapperAmplifierControl::getPWMLimit(int m, double* val) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - yCTrace(CONTROLBOARD) << "CBW2::getPWMlimit j" << static_cast(off + p->base) << " p " << (p ? "1" : "0") << " amp " << (p->amp ? "1" : "0"); - - if (!p) { - *val = 0.0; - return false; - } - - if (!p->amp) { - *val = 0.0; - return false; - } - return p->amp->getPWMLimit(static_cast(off + p->base), val); -} - - -bool ControlBoardWrapperAmplifierControl::setPWMLimit(int m, const double val) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (!p->amp) { - return false; - } - return p->amp->setPWMLimit(static_cast(off + p->base), val); -} - - -bool ControlBoardWrapperAmplifierControl::getPowerSupplyVoltage(int m, double* val) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - *val = 0.0; - return false; - } - - if (!p->amp) { - *val = 0.0; - return false; - } - return p->amp->getPowerSupplyVoltage(static_cast(off + p->base), val); -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperAmplifierControl.h b/src/devices/controlBoardWrapper/ControlBoardWrapperAmplifierControl.h deleted file mode 100644 index 36ce7ba0ac3..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperAmplifierControl.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERAMPLIFIERCONTROL_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERAMPLIFIERCONTROL_H - -#include - -#include "ControlBoardWrapperCommon.h" - -class ControlBoardWrapperAmplifierControl : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IAmplifierControl -{ -public: - bool enableAmp(int j) override; - bool disableAmp(int j) override; - bool getAmpStatus(int* st) override; - bool getAmpStatus(int j, int* v) override; - inline bool getCurrent(int m, double *curr) override { return ControlBoardWrapperCommon::getCurrent(m, curr); } - inline bool getCurrents(double *currs) override { return ControlBoardWrapperCommon::getCurrents(currs); } - bool setMaxCurrent(int j, double v) override; - bool getMaxCurrent(int j, double* v) override; - bool getNominalCurrent(int m, double* val) override; - bool setNominalCurrent(int m, const double val) override; - bool getPeakCurrent(int m, double* val) override; - bool setPeakCurrent(int m, const double val) override; - bool getPWM(int m, double* val) override; - bool getPWMLimit(int m, double* val) override; - bool setPWMLimit(int m, const double val) override; - bool getPowerSupplyVoltage(int m, double* val) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERAMPLIFIERCONTROL_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperAxisInfo.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperAxisInfo.cpp deleted file mode 100644 index 90e15405160..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperAxisInfo.cpp +++ /dev/null @@ -1,47 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperAxisInfo.h" - -#include "ControlBoardLogComponent.h" - - -bool ControlBoardWrapperAxisInfo::getAxisName(int j, std::string& name) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->info) { - return p->info->getAxisName(static_cast(off + p->base), name); - } - return false; -} - -bool ControlBoardWrapperAxisInfo::getJointType(int j, yarp::dev::JointTypeEnum& type) -{ - int off = device.lut[j].offset; - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->info) { - return p->info->getJointType(static_cast(off + p->base), type); - } - return false; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperAxisInfo.h b/src/devices/controlBoardWrapper/ControlBoardWrapperAxisInfo.h deleted file mode 100644 index 645e09f5c6d..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperAxisInfo.h +++ /dev/null @@ -1,26 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERAXISINFO_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERAXISINFO_H - -#include - -#include "ControlBoardWrapperCommon.h" - -class ControlBoardWrapperAxisInfo : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IAxisInfo -{ -public: - inline bool getAxes(int* ax) override - { - return ControlBoardWrapperCommon::getAxes(ax); - } - bool getAxisName(int j, std::string& name) override; - bool getJointType(int j, yarp::dev::JointTypeEnum& type) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERAXISINFO_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperCommon.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperCommon.cpp deleted file mode 100644 index a376a5e7a47..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperCommon.cpp +++ /dev/null @@ -1,349 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperCommon.h" - -#include "ControlBoardLogComponent.h" - - -bool ControlBoardWrapperCommon::getAxes(int* ax) -{ - *ax = static_cast(controlledJoints); - return true; -} - - -bool ControlBoardWrapperCommon::setRefAcceleration(int j, double acc) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - - if (!p) { - return false; - } - - if (p->pos) { - return p->pos->setRefAcceleration(static_cast(off + p->base), acc); - } - return false; -} - - -bool ControlBoardWrapperCommon::setRefAccelerations(const double* accs) -{ - bool ret = true; - int j_wrap = 0; // index of the joint from the wrapper side (useful if wrapper joins 2 subdevices) - - // for all subdevices - for (size_t subDev_idx = 0; subDev_idx < device.subdevices.size(); subDev_idx++) { - SubDevice* p = device.getSubdevice(subDev_idx); - - if (!p) { - return false; - } - - int wrapped_joints = static_cast((p->top - p->base) + 1); - int* joints = new int[wrapped_joints]; // to be defined once and for all? - - if (p->pos) { - // verione comandi su subset di giunti - for (int j_dev = 0; j_dev < wrapped_joints; j_dev++) { - joints[j_dev] = static_cast(p->base + j_dev); - } - - ret = ret && p->pos->setRefAccelerations(wrapped_joints, joints, &accs[j_wrap]); - j_wrap += wrapped_joints; - } else { - ret = false; - } - - if (joints != nullptr) { - delete[] joints; - joints = nullptr; - } - } - - return ret; -} - - -bool ControlBoardWrapperCommon::setRefAccelerations(const int n_joints, const int* joints, const double* accs) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = accs[j]; - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->pos) { - ret = ret && rpcData.subdevices_p[subIndex]->pos->setRefAccelerations(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.values[subIndex]); - } else { - ret = false; - } - } - rpcDataMutex.unlock(); - return ret; -} - - -bool ControlBoardWrapperCommon::getRefAcceleration(int j, double* acc) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - - if (!p) { - return false; - } - - if (p->pos) { - return p->pos->getRefAcceleration(static_cast(off + p->base), acc); - } - *acc = 0; - return false; -} - - -bool ControlBoardWrapperCommon::getRefAccelerations(double* accs) -{ - auto* references = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->pos) && (ret = p->pos->getRefAccelerations(references))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - accs[juser] = references[jdevice]; - } - } else { - printError("getRefAccelerations", p->id, ret); - ret = false; - break; - } - } - - delete[] references; - return ret; -} - - -bool ControlBoardWrapperCommon::getRefAccelerations(const int n_joints, const int* joints, double* accs) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->pos) { - ret = ret && rpcData.subdevices_p[subIndex]->pos->getRefAccelerations(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.values[subIndex]); - } else { - ret = false; - } - } - - if (ret) { - // ReMix values by user expectations - for (size_t i = 0; i < rpcData.deviceNum; i++) { - rpcData.subdev_jointsVectorLen[i] = 0; // reset tmp index - } - - // fill the output vector - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - accs[j] = rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]]; - rpcData.subdev_jointsVectorLen[subIndex]++; - } - } else { - for (int j = 0; j < n_joints; j++) { - accs[j] = 0; - } - } - rpcDataMutex.unlock(); - return ret; -} - - -bool ControlBoardWrapperCommon::stop(int j) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - - if (!p) { - return false; - } - - if (p->pos) { - return p->pos->stop(static_cast(off + p->base)); - } - return false; -} - - -bool ControlBoardWrapperCommon::stop() -{ - bool ret = true; - - for (size_t l = 0; l < controlledJoints; l++) { - size_t off = device.lut[l].offset; - size_t subIndex = device.lut[l].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - - if (!p) { - return false; - } - - if (p->pos) { - ret = ret && p->pos->stop(static_cast(off + p->base)); - } else { - ret = false; - } - } - return ret; -} - - -bool ControlBoardWrapperCommon::stop(const int n_joints, const int* joints) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->pos) { - ret = ret && rpcData.subdevices_p[subIndex]->pos->stop(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex]); - } else { - ret = false; - } - } - rpcDataMutex.unlock(); - return ret; -} - - -bool ControlBoardWrapperCommon::getNumberOfMotors(int* num) -{ - *num = static_cast(controlledJoints); - return true; -} - - -bool ControlBoardWrapperCommon::getCurrents(double* vals) -{ - auto* currs = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - ret = false; - SubDevice* p = device.getSubdevice(d); - if (!p) { - break; - } - - if (p->iCurr) { - ret = p->iCurr->getCurrents(currs); - } else if (p->amp) { - ret = p->amp->getCurrents(currs); - } - - if (ret) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - vals[juser] = currs[jdevice]; - } - } else { - printError("getCurrents", p->id, ret); - break; - } - } - delete[] currs; - return ret; -} - - -bool ControlBoardWrapperCommon::getCurrent(int j, double* val) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iCurr) { - return p->iCurr->getCurrent(static_cast(off + p->base), val); - } - - if (p->amp) { - return p->amp->getCurrent(static_cast(off + p->base), val); - } - *val = 0.0; - return false; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperCommon.h b/src/devices/controlBoardWrapper/ControlBoardWrapperCommon.h deleted file mode 100644 index 1143f9596cc..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperCommon.h +++ /dev/null @@ -1,86 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCOMMON_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCOMMON_H - -#include "MultiJointData.h" - -constexpr int PROTOCOL_VERSION_MAJOR = 1; -constexpr int PROTOCOL_VERSION_MINOR = 9; -constexpr int PROTOCOL_VERSION_TWEAK = 0; - -class ControlBoardWrapperCommon -{ -public: -// COMMON MEMBERS - WrappedDevice device; - size_t controlledJoints {0}; - std::string partName; // to open ports and print more detailed debug messages - - // RPC calls are concurrent from multiple clients, data used inside the calls has to be protected - std::mutex rpcDataMutex; // mutex to avoid concurrency between more clients using rppc port - MultiJointData rpcData; // Structure used to re-arrange data from "multiple_joints" calls. - - std::mutex timeMutex; // mutex to protect access to time member - yarp::os::Stamp time; // envelope to attach to the state port - -// METHODS SHARED BY MULTIPLE INTERFACES - /* - * IEncodersTimed - * IImpedanceControl - * IPositionControl - * IPositionDirect - * ITorqueControl - * IVelocityControl - */ - bool getAxes(int* ax); - - /* - * IPositionControl - * IVelocityControl - */ - bool setRefAcceleration(int j, double acc); - bool setRefAccelerations(const double* accs); - bool setRefAccelerations(const int n_joints, const int* joints, const double* accs); - bool getRefAcceleration(int j, double* acc); - bool getRefAccelerations(double* accs); - bool getRefAccelerations(const int n_joints, const int* joints, double* accs); - bool stop(int j); - bool stop(); - bool stop(const int n_joint, const int* joints); - - /* - * IMotor - * IPWMControl - */ - bool getNumberOfMotors(int* num); - - /* - * IAmplifierControl - * ICurrentControl - */ - bool getCurrent(int m, double* curr); - bool getCurrents(double* currs); - -// UTILITIES - inline void printError(const std::string& func_name, const std::string& info, bool result) - { - // FIXME: Check if it is still required. - // This method was commented out by these commits: - // afc039962f3667cc954e7a50ce6963ec60886611 - // 0c0de4a9331b9b843ac4b3d3746c074dd0427249 - - // If result is false, this means that en error occurred in function named func_name, otherwise means that the device doesn't implement the interface to witch func_name belongs to. - // if(false == result) { - // yCError(CONTROLBOARDREMAPPER) << "CBW(" << partName << "): " << func_name.c_str() << " on device" << info.c_str() << " returns false"; - // } else { - // Commented in order to maintain the old behaviour (none message appear if device desn't implement the interface) - // yCError(CONTROLBOARDREMAPPER) << "CBW(" << partName << "): " << func_name.c_str() << " on device" << info.c_str() << ": the interface is not available."; - // } - } -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCOMMON_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperControlCalibration.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperControlCalibration.cpp deleted file mode 100644 index 3899478abd5..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperControlCalibration.cpp +++ /dev/null @@ -1,78 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperControlCalibration.h" - -#include "ControlBoardLogComponent.h" - -using yarp::dev::CalibrationParameters; - -bool ControlBoardWrapperControlCalibration::calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (p && p->calib) { - return p->calib->calibrateAxisWithParams(static_cast(off + p->base), ui, v1, v2, v3); - } - return false; -} - - -bool ControlBoardWrapperControlCalibration::setCalibrationParameters(int j, const CalibrationParameters& params) -{ - int off = device.lut[j].offset; - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (p && p->calib) { - return p->calib->setCalibrationParameters(static_cast(off + p->base), params); - } - return false; -} - - -bool ControlBoardWrapperControlCalibration::calibrationDone(int j) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->calib) { - return p->calib->calibrationDone(static_cast(off + p->base)); - } - return false; -} - - -bool ControlBoardWrapperControlCalibration::abortPark() -{ - yCError(CONTROLBOARD, "Calling abortPark -- not implemented"); - return false; -} - - -bool ControlBoardWrapperControlCalibration::abortCalibration() -{ - yCError(CONTROLBOARD, "Calling abortCalibration -- not implemented"); - return false; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperControlCalibration.h b/src/devices/controlBoardWrapper/ControlBoardWrapperControlCalibration.h deleted file mode 100644 index 7ae40871944..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperControlCalibration.h +++ /dev/null @@ -1,25 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCONTROLCALIBRATION_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCONTROLCALIBRATION_H - -#include - -#include "ControlBoardWrapperCommon.h" - -class ControlBoardWrapperControlCalibration : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IControlCalibration -{ -public: - bool calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3) override; - bool setCalibrationParameters(int j, const yarp::dev::CalibrationParameters& params) override; - bool calibrationDone(int j) override; - bool abortPark() override; - bool abortCalibration() override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCONTROLCALIBRATION_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperControlLimits.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperControlLimits.cpp deleted file mode 100644 index 7900ba64bc8..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperControlLimits.cpp +++ /dev/null @@ -1,104 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperControlLimits.h" - -#include "ControlBoardLogComponent.h" - - -bool ControlBoardWrapperControlLimits::setLimits(int j, double min, double max) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->lim) { - return p->lim->setLimits(static_cast(off + p->base), min, max); - } - return false; -} - -bool ControlBoardWrapperControlLimits::getLimits(int j, double* min, double* max) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - *min = 0.0; - *max = 0.0; - return false; - } - - if (p->lim) { - return p->lim->getLimits(static_cast(off + p->base), min, max); - } - *min = 0.0; - *max = 0.0; - return false; -} - -bool ControlBoardWrapperControlLimits::setVelLimits(int j, double min, double max) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (!p->lim) { - return false; - } - return p->lim->setVelLimits(static_cast(off + p->base), min, max); -} - -bool ControlBoardWrapperControlLimits::getVelLimits(int j, double* min, double* max) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - *min = 0.0; - *max = 0.0; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (!p->lim) { - return false; - } - return p->lim->getVelLimits(static_cast(off + p->base), min, max); -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperControlLimits.h b/src/devices/controlBoardWrapper/ControlBoardWrapperControlLimits.h deleted file mode 100644 index 9d84488fb5b..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperControlLimits.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCONTROLLIMITS_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCONTROLLIMITS_H - -#include - -#include "ControlBoardWrapperCommon.h" - -class ControlBoardWrapperControlLimits : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IControlLimits -{ -public: - bool setLimits(int j, double min, double max) override; - bool getLimits(int j, double* min, double* max) override; - bool setVelLimits(int j, double min, double max) override; - bool getVelLimits(int j, double* min, double* max) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCONTROLLIMITS_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperControlMode.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperControlMode.cpp deleted file mode 100644 index c77a10aca60..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperControlMode.cpp +++ /dev/null @@ -1,171 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperControlMode.h" - -#include "ControlBoardLogComponent.h" - - -bool ControlBoardWrapperControlMode::getControlMode(int j, int* mode) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iMode) { - return p->iMode->getControlMode(static_cast(off + p->base), mode); - } - return false; -} - - -bool ControlBoardWrapperControlMode::getControlModes(int* modes) -{ - int* all_mode = new int[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iMode) && (ret = p->iMode->getControlModes(all_mode))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - modes[juser] = all_mode[jdevice]; - } - } else { - printError("getControlModes", p->id, ret); - ret = false; - break; - } - } - - delete[] all_mode; - return ret; -} - - -bool ControlBoardWrapperControlMode::getControlModes(const int n_joint, const int* joints, int* modes) -{ - bool ret = true; - - for (int l = 0; l < n_joint; l++) { - int off = device.lut[joints[l]].offset; - size_t subIndex = device.lut[joints[l]].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iMode) { - ret = ret && p->iMode->getControlMode(static_cast(off + p->base), &modes[l]); - } else { - ret = false; - } - } - return ret; -} - - -bool ControlBoardWrapperControlMode::setControlMode(const int j, const int mode) -{ - bool ret = true; - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iMode) { - ret = p->iMode->setControlMode(static_cast(off + p->base), mode); - } - return ret; -} - - -bool ControlBoardWrapperControlMode::setControlModes(const int n_joints, const int* joints, int* modes) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.modes[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = modes[j]; - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->iMode) { - ret = ret && rpcData.subdevices_p[subIndex]->iMode->setControlModes(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.modes[subIndex]); - } else { - ret = false; - } - } - rpcDataMutex.unlock(); - return ret; -} - - -bool ControlBoardWrapperControlMode::setControlModes(int* modes) -{ - bool ret = true; - int j_wrap = 0; // index of the wrapper joint - - int nDev = device.subdevices.size(); - for (int subDev_idx = 0; subDev_idx < nDev; subDev_idx++) { - size_t subIndex = device.lut[j_wrap].deviceEntry; - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - int wrapped_joints = static_cast((p->top - p->base) + 1); - int* joints = new int[wrapped_joints]; - - if (p->iMode) { - // versione comandi su subset di giunti - for (int j_dev = 0; j_dev < wrapped_joints; j_dev++) { - joints[j_dev] = static_cast(p->base + j_dev); // for all joints is equivalent to add offset term - } - - ret = ret && p->iMode->setControlModes(wrapped_joints, joints, &modes[j_wrap]); - j_wrap += wrapped_joints; - } - - if (joints != nullptr) { - delete[] joints; - joints = nullptr; - } - } - - return ret; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperControlMode.h b/src/devices/controlBoardWrapper/ControlBoardWrapperControlMode.h deleted file mode 100644 index 7bfe28b9966..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperControlMode.h +++ /dev/null @@ -1,26 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCONTROLMODE_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCONTROLMODE_H - -#include - -#include "ControlBoardWrapperCommon.h" - -class ControlBoardWrapperControlMode : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IControlMode -{ -public: - bool getControlMode(int j, int* mode) override; - bool getControlModes(int* modes) override; - bool getControlModes(const int n_joint, const int* joints, int* modes) override; - bool setControlMode(const int j, const int mode) override; - bool setControlModes(const int n_joints, const int* joints, int* modes) override; - bool setControlModes(int* modes) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCONTROLMODE_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperCurrentControl.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperCurrentControl.cpp deleted file mode 100644 index ef0db107a5f..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperCurrentControl.cpp +++ /dev/null @@ -1,183 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperCurrentControl.h" - -#include "ControlBoardLogComponent.h" - - -bool ControlBoardWrapperCurrentControl::getCurrentRange(int j, double* min, double* max) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iCurr) { - return p->iCurr->getCurrentRange(static_cast(off + p->base), min, max); - } - - return false; -} - -bool ControlBoardWrapperCurrentControl::getCurrentRanges(double* min, double* max) -{ - auto* c_min = new double[device.maxNumOfJointsInDevices]; - auto* c_max = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iCurr) && (ret = p->iCurr->getCurrentRanges(c_min, c_max))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - min[juser] = c_min[jdevice]; - max[juser] = c_max[jdevice]; - } - } else { - printError("getCurrentRanges", p->id, ret); - ret = false; - break; - } - } - - delete[] c_min; - delete[] c_max; - return ret; -} - -bool ControlBoardWrapperCurrentControl::setRefCurrents(const double* t) -{ - bool ret = true; - - for (size_t l = 0; l < controlledJoints; l++) { - int off = device.lut[l].offset; - size_t subIndex = device.lut[l].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iCurr) { - ret = ret && p->iCurr->setRefCurrent(static_cast(off + p->base), t[l]); - } else { - ret = false; - } - } - return ret; -} - -bool ControlBoardWrapperCurrentControl::setRefCurrent(int j, double t) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iCurr) { - return p->iCurr->setRefCurrent(static_cast(off + p->base), t); - } - return false; -} - -bool ControlBoardWrapperCurrentControl::setRefCurrents(const int n_joint, const int* joints, const double* t) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joint; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = t[j]; - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->iCurr) { - ret = ret && rpcData.subdevices_p[subIndex]->iCurr->setRefCurrents(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.values[subIndex]); - } else { - ret = false; - } - } - rpcDataMutex.unlock(); - return ret; -} - -bool ControlBoardWrapperCurrentControl::getRefCurrents(double* t) -{ - auto* references = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iCurr) && (ret = p->iCurr->getRefCurrents(references))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - t[juser] = references[jdevice]; - } - } else { - printError("getRefCurrents", p->id, ret); - ret = false; - break; - } - } - - delete[] references; - return ret; -} - -bool ControlBoardWrapperCurrentControl::getRefCurrent(int j, double* t) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iCurr) { - return p->iCurr->getRefCurrent(static_cast(off + p->base), t); - } - - return false; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperCurrentControl.h b/src/devices/controlBoardWrapper/ControlBoardWrapperCurrentControl.h deleted file mode 100644 index 10c1138799e..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperCurrentControl.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCURRENTCONTROL_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCURRENTCONTROL_H - -#include - -#include "ControlBoardWrapperCommon.h" - -class ControlBoardWrapperCurrentControl : - virtual public ControlBoardWrapperCommon, - public yarp::dev::ICurrentControl -{ -public: - inline bool getNumberOfMotors(int* num) override { return ControlBoardWrapperCommon::getNumberOfMotors(num); } - inline bool getCurrent(int m, double *curr) override { return ControlBoardWrapperCommon::getCurrent(m, curr); } - inline bool getCurrents(double *currs) override { return ControlBoardWrapperCommon::getCurrents(currs); } - bool getCurrentRange(int j, double* min, double* max) override; - bool getCurrentRanges(double* min, double* max) override; - bool setRefCurrents(const double* t) override; - bool setRefCurrent(int j, double t) override; - bool setRefCurrents(const int n_joint, const int* joints, const double* t) override; - bool getRefCurrents(double* t) override; - bool getRefCurrent(int j, double* t) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERCURRENTCONTROL_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperEncodersTimed.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperEncodersTimed.cpp deleted file mode 100644 index fe241fb6349..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperEncodersTimed.cpp +++ /dev/null @@ -1,306 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperEncodersTimed.h" - -#include "ControlBoardLogComponent.h" - -bool ControlBoardWrapperEncodersTimed::resetEncoder(int j) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iJntEnc) { - return p->iJntEnc->resetEncoder(static_cast(off + p->base)); - } - return false; -} - - -bool ControlBoardWrapperEncodersTimed::resetEncoders() -{ - bool ret = true; - - for (size_t l = 0; l < controlledJoints; l++) { - int off = device.lut[l].offset; - size_t subIndex = device.lut[l].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iJntEnc) { - ret = ret && p->iJntEnc->resetEncoder(static_cast(off + p->base)); - } else { - ret = false; - } - } - return ret; -} - - -bool ControlBoardWrapperEncodersTimed::setEncoder(int j, double val) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iJntEnc) { - return p->iJntEnc->setEncoder(static_cast(off + p->base), val); - } - return false; -} - - -bool ControlBoardWrapperEncodersTimed::setEncoders(const double* vals) -{ - bool ret = true; - - for (size_t l = 0; l < controlledJoints; l++) { - int off = device.lut[l].offset; - size_t subIndex = device.lut[l].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iJntEnc) { - ret = ret && p->iJntEnc->setEncoder(static_cast(off + p->base), vals[l]); - } else { - ret = false; - } - } - return ret; -} - - -bool ControlBoardWrapperEncodersTimed::getEncoder(int j, double* v) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iJntEnc) { - return p->iJntEnc->getEncoder(static_cast(off + p->base), v); - } - *v = 0.0; - return false; -} - - -bool ControlBoardWrapperEncodersTimed::getEncoders(double* encs) -{ - auto* encValues = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iJntEnc) && (ret = p->iJntEnc->getEncoders(encValues))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - encs[juser] = encValues[jdevice]; - } - } else { - printError("getEncoders", p->id, ret); - ret = false; - break; - } - } - - delete[] encValues; - return ret; -} - - -bool ControlBoardWrapperEncodersTimed::getEncodersTimed(double* encs, double* t) -{ - auto* encValues = new double[device.maxNumOfJointsInDevices]; - auto* tValues = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iJntEnc) && (ret = p->iJntEnc->getEncodersTimed(encValues, tValues))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - encs[juser] = encValues[jdevice]; - t[juser] = tValues[jdevice]; - } - } else { - printError("getEncodersTimed", p->id, ret); - ret = false; - break; - } - } - - delete[] encValues; - delete[] tValues; - return ret; -} - - -bool ControlBoardWrapperEncodersTimed::getEncoderTimed(int j, double* v, double* t) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iJntEnc) { - return p->iJntEnc->getEncoderTimed(static_cast(off + p->base), v, t); - } - *v = 0.0; - return false; -} - - -bool ControlBoardWrapperEncodersTimed::getEncoderSpeed(int j, double* sp) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iJntEnc) { - return p->iJntEnc->getEncoderSpeed(static_cast(off + p->base), sp); - } - *sp = 0.0; - return false; -} - - -bool ControlBoardWrapperEncodersTimed::getEncoderSpeeds(double* spds) -{ - auto* sValues = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iJntEnc) && (ret = p->iJntEnc->getEncoderSpeeds(sValues))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - spds[juser] = sValues[jdevice]; - } - } else { - printError("getEncoderSpeeds", p->id, ret); - ret = false; - break; - } - } - - delete[] sValues; - return ret; -} - - -bool ControlBoardWrapperEncodersTimed::getEncoderAcceleration(int j, double* acc) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iJntEnc) { - return p->iJntEnc->getEncoderAcceleration(static_cast(off + p->base), acc); - } - *acc = 0.0; - return false; -} - - -bool ControlBoardWrapperEncodersTimed::getEncoderAccelerations(double* accs) -{ - auto* aValues = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iJntEnc) && (ret = p->iJntEnc->getEncoderAccelerations(aValues))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - accs[juser] = aValues[jdevice]; - } - } else { - printError("getEncoderAccelerations", p->id, ret); - ret = false; - break; - } - } - - delete[] aValues; - return ret; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperEncodersTimed.h b/src/devices/controlBoardWrapper/ControlBoardWrapperEncodersTimed.h deleted file mode 100644 index d899d7f0174..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperEncodersTimed.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERENCODERSTIMED_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERENCODERSTIMED_H - -#include - -#include "ControlBoardWrapperCommon.h" - -class ControlBoardWrapperEncodersTimed : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IEncodersTimed -{ -public: - inline bool getAxes(int *ax) override { return ControlBoardWrapperCommon::getAxes(ax); } - 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 getEncoderSpeed(int j, double* sp) override; - bool getEncoderSpeeds(double* spds) override; - bool getEncoderAcceleration(int j, double* acc) override; - bool getEncoderAccelerations(double* accs) override; - bool getEncodersTimed(double* encs, double* t) override; - bool getEncoderTimed(int j, double* v, double* t) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERENCODERSTIMED_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperImpedanceControl.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperImpedanceControl.cpp deleted file mode 100644 index c89b6089110..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperImpedanceControl.cpp +++ /dev/null @@ -1,128 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperImpedanceControl.h" - -#include "ControlBoardLogComponent.h" - - -bool ControlBoardWrapperImpedanceControl::setImpedance(int j, double stiff, double damp) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iImpedance) { - return p->iImpedance->setImpedance(static_cast(off + p->base), stiff, damp); - } - - return false; -} - - -bool ControlBoardWrapperImpedanceControl::setImpedanceOffset(int j, double offset) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iImpedance) { - return p->iImpedance->setImpedanceOffset(static_cast(off + p->base), offset); - } - - return false; -} - - -bool ControlBoardWrapperImpedanceControl::getImpedance(int j, double* stiff, double* damp) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iImpedance) { - return p->iImpedance->getImpedance(static_cast(off + p->base), stiff, damp); - } - - return false; -} - - -bool ControlBoardWrapperImpedanceControl::getImpedanceOffset(int j, double* offset) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iImpedance) { - return p->iImpedance->getImpedanceOffset(static_cast(off + p->base), offset); - } - - return false; -} - - -bool ControlBoardWrapperImpedanceControl::getCurrentImpedanceLimit(int j, double* min_stiff, double* max_stiff, double* min_damp, double* max_damp) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iImpedance) { - return p->iImpedance->getCurrentImpedanceLimit(static_cast(off + p->base), min_stiff, max_stiff, min_damp, max_damp); - } - - return false; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperImpedanceControl.h b/src/devices/controlBoardWrapper/ControlBoardWrapperImpedanceControl.h deleted file mode 100644 index 2ec3cb388ee..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperImpedanceControl.h +++ /dev/null @@ -1,26 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERIMPEDANCECONTROL_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERIMPEDANCECONTROL_H - -#include - -#include "ControlBoardWrapperCommon.h" - -class ControlBoardWrapperImpedanceControl : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IImpedanceControl -{ -public: - inline bool getAxes(int* ax) override { return ControlBoardWrapperCommon::getAxes(ax); } - bool setImpedance(int j, double stiff, double damp) override; - bool setImpedanceOffset(int j, double offset) override; - bool getImpedance(int j, double* stiff, double* damp) override; - bool getImpedanceOffset(int j, double* offset) override; - bool getCurrentImpedanceLimit(int j, double* min_stiff, double* max_stiff, double* min_damp, double* max_damp) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERIMPEDANCECONTROL_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperInteractionMode.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperInteractionMode.cpp deleted file mode 100644 index 4fe940b74b8..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperInteractionMode.cpp +++ /dev/null @@ -1,190 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperInteractionMode.h" - -#include "ControlBoardLogComponent.h" - -using yarp::dev::VOCAB_IM_UNKNOWN; - -bool ControlBoardWrapperInteractionMode::getInteractionMode(int j, yarp::dev::InteractionModeEnum* mode) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* s = device.getSubdevice(subIndex); - if (!s) { - return false; - } - - if (s->iInteract) { - return s->iInteract->getInteractionMode(static_cast(off + s->base), mode); - } - return false; -} - -bool ControlBoardWrapperInteractionMode::getInteractionModes(int n_joints, int* joints, yarp::dev::InteractionModeEnum* modes) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->iInteract) { - ret = ret && rpcData.subdevices_p[subIndex]->iInteract->getInteractionModes( - rpcData.subdev_jointsVectorLen[subIndex], - rpcData.jointNumbers[subIndex], - reinterpret_cast(rpcData.modes[subIndex])); - } else { - ret = false; - } - } - - if (ret) { - // ReMix values by user expectations - for (size_t i = 0; i < rpcData.deviceNum; i++) { - rpcData.subdev_jointsVectorLen[i] = 0; // reset tmp index - } - - // fill the output vector - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - modes[j] = static_cast(rpcData.modes[subIndex][rpcData.subdev_jointsVectorLen[subIndex]]); - rpcData.subdev_jointsVectorLen[subIndex]++; - } - } else { - for (int j = 0; j < n_joints; j++) { - modes[j] = VOCAB_IM_UNKNOWN; - } - } - rpcDataMutex.unlock(); - return ret; -} - -bool ControlBoardWrapperInteractionMode::getInteractionModes(yarp::dev::InteractionModeEnum* modes) -{ - - auto* imodes = new yarp::dev::InteractionModeEnum[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iInteract) && (ret = p->iInteract->getInteractionModes(imodes))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - modes[juser] = imodes[jdevice]; - } - } else { - printError("getInteractionModes", p->id, ret); - ret = false; - break; - } - } - - delete[] imodes; - return ret; -} - -bool ControlBoardWrapperInteractionMode::setInteractionMode(int j, yarp::dev::InteractionModeEnum mode) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* s = device.getSubdevice(subIndex); - if (!s) { - return false; - } - - if (s->iInteract) { - return s->iInteract->setInteractionMode(static_cast(off + s->base), mode); - } - return false; -} - -bool ControlBoardWrapperInteractionMode::setInteractionModes(int n_joints, int* joints, yarp::dev::InteractionModeEnum* modes) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.modes[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = static_cast(modes[j]); - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->iInteract) { - ret = ret && rpcData.subdevices_p[subIndex]->iInteract->setInteractionModes( - rpcData.subdev_jointsVectorLen[subIndex], - rpcData.jointNumbers[subIndex], - reinterpret_cast(rpcData.modes[subIndex])); - } else { - ret = false; - } - } - rpcDataMutex.unlock(); - return ret; -} - -bool ControlBoardWrapperInteractionMode::setInteractionModes(yarp::dev::InteractionModeEnum* modes) -{ - bool ret = true; - - for (size_t j = 0; j < controlledJoints; j++) { - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %zu out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iInteract) { - ret = ret && p->iInteract->setInteractionMode(static_cast(off + p->base), modes[j]); - } else { - ret = false; - } - } - return ret; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperInteractionMode.h b/src/devices/controlBoardWrapper/ControlBoardWrapperInteractionMode.h deleted file mode 100644 index 8cd77770d60..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperInteractionMode.h +++ /dev/null @@ -1,27 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERINTERACTIONMODE_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERINTERACTIONMODE_H - -#include - -#include "ControlBoardWrapperCommon.h" - - -class ControlBoardWrapperInteractionMode : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IInteractionMode -{ -public: - bool getInteractionMode(int j, yarp::dev::InteractionModeEnum* mode) override; - bool getInteractionModes(int n_joints, int* joints, yarp::dev::InteractionModeEnum* modes) override; - bool getInteractionModes(yarp::dev::InteractionModeEnum* modes) override; - bool setInteractionMode(int j, yarp::dev::InteractionModeEnum mode) override; - bool setInteractionModes(int n_joints, int* joints, yarp::dev::InteractionModeEnum* modes) override; - bool setInteractionModes(yarp::dev::InteractionModeEnum* modes) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERINTERACTIONMODE_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperLogComponent.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperLogComponent.cpp deleted file mode 100644 index aaa6cef4e1b..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperLogComponent.cpp +++ /dev/null @@ -1,8 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperLogComponent.h" - -YARP_LOG_COMPONENT(CONTROLBOARD, LOG_COMPONENT) diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperLogComponent.h b/src/devices/controlBoardWrapper/ControlBoardWrapperLogComponent.h deleted file mode 100644 index 11ea3f0be34..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperLogComponent.h +++ /dev/null @@ -1,13 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERLOGCOMPONENT_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERLOGCOMPONENT_H - -#include - -YARP_DECLARE_LOG_COMPONENT(CONTROLBOARD) - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERLOGCOMPONENT_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperMotor.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperMotor.cpp deleted file mode 100644 index bae58f5f532..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperMotor.cpp +++ /dev/null @@ -1,117 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperMotor.h" - -#include "ControlBoardLogComponent.h" - -bool ControlBoardWrapperMotor::getTemperature(int m, double* val) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->imotor) { - return p->imotor->getTemperature(static_cast(off + p->base), val); - } - *val = 0.0; - return false; -} - -bool ControlBoardWrapperMotor::getTemperatures(double* vals) -{ - auto* temps = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->imotor) && (ret = p->imotor->getTemperatures(temps))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - vals[juser] = temps[jdevice]; - } - } else { - printError("getTemperatures", p->id, ret); - ret = false; - break; - } - } - - delete[] temps; - return ret; -} - -bool ControlBoardWrapperMotor::getTemperatureLimit(int m, double* val) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->imotor) { - return p->imotor->getTemperatureLimit(static_cast(off + p->base), val); - } - *val = 0.0; - return false; -} - -bool ControlBoardWrapperMotor::setTemperatureLimit(int m, const double val) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->imotor) { - return p->imotor->setTemperatureLimit(static_cast(off + p->base), val); - } - return false; -} - -bool ControlBoardWrapperMotor::getGearboxRatio(int m, double* val) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->imotor) { - return p->imotor->getGearboxRatio(static_cast(off + p->base), val); - } - *val = 0.0; - return false; -} - -bool ControlBoardWrapperMotor::setGearboxRatio(int m, const double val) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->imotor) { - return p->imotor->setGearboxRatio(static_cast(off + p->base), val); - } - return false; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperMotor.h b/src/devices/controlBoardWrapper/ControlBoardWrapperMotor.h deleted file mode 100644 index 9a98c955fa8..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperMotor.h +++ /dev/null @@ -1,27 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERMOTOR_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERMOTOR_H - -#include - -#include "ControlBoardWrapperCommon.h" - -class ControlBoardWrapperMotor : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IMotor -{ -public: - inline bool getNumberOfMotors(int* num) override { return ControlBoardWrapperCommon::getNumberOfMotors(num); } - bool getTemperature(int m, double* val) override; - bool getTemperatures(double* vals) override; - bool getTemperatureLimit(int m, double* val) override; - bool setTemperatureLimit(int m, const double val) override; - bool getGearboxRatio(int m, double* val) override; - bool setGearboxRatio(int m, const double val) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERMOTOR_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperMotorEncoders.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperMotorEncoders.cpp deleted file mode 100644 index cea36d511e2..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperMotorEncoders.cpp +++ /dev/null @@ -1,313 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperMotorEncoders.h" - -#include "ControlBoardLogComponent.h" - -bool ControlBoardWrapperMotorEncoders::resetMotorEncoder(int m) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iMotEnc) { - return p->iMotEnc->resetMotorEncoder(static_cast(off + p->base)); - } - return false; -} - - -bool ControlBoardWrapperMotorEncoders::resetMotorEncoders() -{ - bool ret = true; - - for (size_t l = 0; l < controlledJoints; l++) { - int off = device.lut[l].offset; - size_t subIndex = device.lut[l].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iMotEnc) { - ret = ret && p->iMotEnc->resetMotorEncoder(static_cast(off + p->base)); - } else { - ret = false; - } - } - return ret; -} - - -bool ControlBoardWrapperMotorEncoders::setMotorEncoder(int m, const double val) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iMotEnc) { - return p->iMotEnc->setMotorEncoder(static_cast(off + p->base), val); - } - return false; -} - - -bool ControlBoardWrapperMotorEncoders::setMotorEncoders(const double* vals) -{ - bool ret = true; - - for (size_t l = 0; l < controlledJoints; l++) { - int off = device.lut[l].offset; - size_t subIndex = device.lut[l].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iMotEnc) { - ret = ret && p->iMotEnc->setMotorEncoder(static_cast(off + p->base), vals[l]); - } else { - ret = false; - } - } - return ret; -} - - -bool ControlBoardWrapperMotorEncoders::setMotorEncoderCountsPerRevolution(int m, double cpr) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iMotEnc) { - return p->iMotEnc->setMotorEncoderCountsPerRevolution(static_cast(off + p->base), cpr); - } - return false; -} - - -bool ControlBoardWrapperMotorEncoders::getMotorEncoderCountsPerRevolution(int m, double* cpr) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iMotEnc) { - return p->iMotEnc->getMotorEncoderCountsPerRevolution(static_cast(off + p->base), cpr); - } - *cpr = 0.0; - return false; -} - - -bool ControlBoardWrapperMotorEncoders::getMotorEncoder(int m, double* v) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iMotEnc) { - return p->iMotEnc->getMotorEncoder(static_cast(off + p->base), v); - } - *v = 0.0; - return false; -} - - -bool ControlBoardWrapperMotorEncoders::getMotorEncoders(double* encs) -{ - - auto* encValues = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iMotEnc) && (ret = p->iMotEnc->getMotorEncoders(encValues))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - encs[juser] = encValues[jdevice]; - } - } else { - printError("getMotorEncoders", p->id, ret); - ret = false; - break; - } - } - - delete[] encValues; - return ret; -} - - -bool ControlBoardWrapperMotorEncoders::getMotorEncodersTimed(double* encs, double* t) -{ - auto* encValues = new double[device.maxNumOfJointsInDevices]; - auto* tValues = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iMotEnc) && (ret = p->iMotEnc->getMotorEncodersTimed(encValues, tValues))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - encs[juser] = encValues[jdevice]; - t[juser] = tValues[jdevice]; - } - } else { - printError("getMotorEncodersTimed", p->id, ret); - ret = false; - break; - } - } - - delete[] encValues; - delete[] tValues; - return ret; -} - - -bool ControlBoardWrapperMotorEncoders::getMotorEncoderTimed(int m, double* v, double* t) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iMotEnc) { - return p->iMotEnc->getMotorEncoderTimed(static_cast(off + p->base), v, t); - } - *v = 0.0; - return false; -} - - -bool ControlBoardWrapperMotorEncoders::getMotorEncoderSpeed(int m, double* sp) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iMotEnc) { - return p->iMotEnc->getMotorEncoderSpeed(static_cast(off + p->base), sp); - } - *sp = 0.0; - return false; -} - - -bool ControlBoardWrapperMotorEncoders::getMotorEncoderSpeeds(double* spds) -{ - auto* sValues = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iMotEnc) && (ret = p->iMotEnc->getMotorEncoderSpeeds(sValues))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - spds[juser] = sValues[jdevice]; - } - } else { - printError("getMotorEncoderSpeeds", p->id, ret); - ret = false; - break; - } - } - - delete[] sValues; - return ret; -} - - -bool ControlBoardWrapperMotorEncoders::getMotorEncoderAcceleration(int m, double* acc) -{ - int off = device.lut[m].offset; - size_t subIndex = device.lut[m].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iMotEnc) { - return p->iMotEnc->getMotorEncoderAcceleration(static_cast(off + p->base), acc); - } - *acc = 0.0; - return false; -} - - -bool ControlBoardWrapperMotorEncoders::getMotorEncoderAccelerations(double* accs) -{ - auto* aValues = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iMotEnc) && (ret = p->iMotEnc->getMotorEncoderAccelerations(aValues))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - accs[juser] = aValues[jdevice]; - } - } else { - printError("getMotorEncoderAccelerations", p->id, ret); - ret = false; - break; - } - } - - delete[] aValues; - return ret; -} - - -bool ControlBoardWrapperMotorEncoders::getNumberOfMotorEncoders(int* num) -{ - *num = controlledJoints; - return true; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperMotorEncoders.h b/src/devices/controlBoardWrapper/ControlBoardWrapperMotorEncoders.h deleted file mode 100644 index 0f52048e0ec..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperMotorEncoders.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERMOTORENCODERS_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERMOTORENCODERS_H - -#include - -#include "ControlBoardWrapperCommon.h" - -class ControlBoardWrapperMotorEncoders : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IMotorEncoders -{ -public: - bool getNumberOfMotorEncoders(int* num) override; - bool resetMotorEncoder(int m) override; - bool resetMotorEncoders() override; - bool setMotorEncoderCountsPerRevolution(int m, const double cpr) override; - bool getMotorEncoderCountsPerRevolution(int m, double* cpr) override; - bool setMotorEncoder(int m, const double val) override; - bool setMotorEncoders(const double* vals) override; - bool getMotorEncoder(int m, double* v) override; - bool getMotorEncoders(double* encs) override; - bool getMotorEncodersTimed(double* encs, double* t) override; - bool getMotorEncoderTimed(int m, double* v, double* t) override; - bool getMotorEncoderSpeed(int m, double* sp) override; - bool getMotorEncoderSpeeds(double* spds) override; - bool getMotorEncoderAcceleration(int m, double* acc) override; - bool getMotorEncoderAccelerations(double* accs) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERMOTORENCODERS_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperPWMControl.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperPWMControl.cpp deleted file mode 100644 index 10060f27441..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperPWMControl.cpp +++ /dev/null @@ -1,149 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperPWMControl.h" - -#include "ControlBoardLogComponent.h" - - -bool ControlBoardWrapperPWMControl::setRefDutyCycle(int j, double v) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iPWM) { - return p->iPWM->setRefDutyCycle(static_cast(off + p->base), v); - } - return false; -} - -bool ControlBoardWrapperPWMControl::setRefDutyCycles(const double* v) -{ - bool ret = true; - - for (size_t l = 0; l < controlledJoints; l++) { - int off = device.lut[l].offset; - size_t subIndex = device.lut[l].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iPWM) { - ret = ret && p->iPWM->setRefDutyCycle(static_cast(off + p->base), v[l]); - } else { - ret = false; - } - } - return ret; -} - -bool ControlBoardWrapperPWMControl::getRefDutyCycle(int j, double* v) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iPWM) { - return p->iPWM->getRefDutyCycle(static_cast(off + p->base), v); - } - return false; -} - -bool ControlBoardWrapperPWMControl::getRefDutyCycles(double* v) -{ - auto* references = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iPWM) && (ret = p->iPWM->getRefDutyCycles(references))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - v[juser] = references[jdevice]; - } - } else { - printError("getRefDutyCycles", p->id, ret); - ret = false; - break; - } - } - - delete[] references; - return ret; -} - -bool ControlBoardWrapperPWMControl::getDutyCycle(int j, double* v) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iPWM) { - return p->iPWM->getDutyCycle(static_cast(off + p->base), v); - } - return false; -} - -bool ControlBoardWrapperPWMControl::getDutyCycles(double* v) -{ - auto* dutyCicles = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iPWM) && (ret = p->iPWM->getDutyCycles(dutyCicles))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - v[juser] = dutyCicles[jdevice]; - } - } else { - printError("getDutyCycles", p->id, ret); - ret = false; - break; - } - } - - delete[] dutyCicles; - return ret; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperPWMControl.h b/src/devices/controlBoardWrapper/ControlBoardWrapperPWMControl.h deleted file mode 100644 index b1e2425ae54..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperPWMControl.h +++ /dev/null @@ -1,28 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPWMCONTROL_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPWMCONTROL_H - -#include - -#include "ControlBoardWrapperCommon.h" - - -class ControlBoardWrapperPWMControl : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IPWMControl -{ -public: - inline bool getNumberOfMotors(int* num) override { return ControlBoardWrapperCommon::getNumberOfMotors(num); } - bool setRefDutyCycle(int j, double v) override; - bool setRefDutyCycles(const double* v) override; - bool getRefDutyCycle(int j, double* v) override; - bool getRefDutyCycles(double* v) override; - bool getDutyCycle(int j, double* v) override; - bool getDutyCycles(double* v) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPWMCONTROL_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperPidControl.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperPidControl.cpp deleted file mode 100644 index 0dcd95eb441..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperPidControl.cpp +++ /dev/null @@ -1,525 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperPidControl.h" - -#include "ControlBoardLogComponent.h" - -using yarp::dev::Pid; -using yarp::dev::PidControlTypeEnum; - - -bool ControlBoardWrapperPidControl::setPid(const PidControlTypeEnum& pidtype, int j, const Pid& p) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, - "Joint number %d out of bound [0-%zu] for part %s", - j, - controlledJoints, - partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* s = device.getSubdevice(subIndex); - if (!s) { - return false; - } - - if (s->pid) { - return s->pid->setPid(pidtype, static_cast(off + s->base), p); - } - return false; -} - - -bool ControlBoardWrapperPidControl::setPids(const PidControlTypeEnum& pidtype, const Pid* ps) -{ - bool ret = true; - - for (size_t l = 0; l < controlledJoints; l++) { - int off = device.lut[l].offset; - size_t subIndex = device.lut[l].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pid) { - ret = ret && p->pid->setPid(pidtype, static_cast(off + p->base), ps[l]); - } else { - ret = false; - } - } - return ret; -} - - -bool ControlBoardWrapperPidControl::setPidReference(const PidControlTypeEnum& pidtype, int j, double ref) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, - "Joint number %d out of bound [0-%zu] for part %s", - j, - controlledJoints, - partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pid) { - return p->pid->setPidReference(pidtype, static_cast(off + p->base), ref); - } - return false; -} - - -bool ControlBoardWrapperPidControl::setPidReferences(const PidControlTypeEnum& pidtype, const double* refs) -{ - bool ret = true; - - for (size_t l = 0; l < controlledJoints; l++) { - int off = device.lut[l].offset; - size_t subIndex = device.lut[l].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pid) { - ret = ret && p->pid->setPidReference(pidtype, static_cast(off + p->base), refs[l]); - } else { - ret = false; - } - } - return ret; -} - - -bool ControlBoardWrapperPidControl::setPidErrorLimit(const PidControlTypeEnum& pidtype, int j, double limit) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pid) { - return p->pid->setPidErrorLimit(pidtype, static_cast(off + p->base), limit); - } - return false; -} - - -bool ControlBoardWrapperPidControl::setPidErrorLimits(const PidControlTypeEnum& pidtype, const double* limits) -{ - bool ret = true; - - for (size_t l = 0; l < controlledJoints; l++) { - int off = device.lut[l].offset; - size_t subIndex = device.lut[l].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pid) { - ret = ret && p->pid->setPidErrorLimit(pidtype, static_cast(off + p->base), limits[l]); - } else { - ret = false; - } - } - return ret; -} - - -bool ControlBoardWrapperPidControl::getPidError(const PidControlTypeEnum& pidtype, int j, double* err) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pid) { - return p->pid->getPidError(pidtype, static_cast(off + p->base), err); - } - *err = 0.0; - return false; -} - - -bool ControlBoardWrapperPidControl::getPidErrors(const PidControlTypeEnum& pidtype, double* errs) -{ - auto* errors = new double[device.maxNumOfJointsInDevices]; - - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - if ((p->pid) && (ret = p->pid->getPidErrors(pidtype, errors))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - errs[juser] = errors[jdevice]; - } - } else { - printError("getPidErrors", p->id, ret); - ret = false; - break; - } - } - - delete[] errors; - return ret; -} - - -bool ControlBoardWrapperPidControl::getPidOutput(const PidControlTypeEnum& pidtype, int j, double* out) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pid) { - return p->pid->getPidOutput(pidtype, static_cast(off + p->base), out); - } - *out = 0.0; - return false; -} - - -bool ControlBoardWrapperPidControl::getPidOutputs(const PidControlTypeEnum& pidtype, double* outs) -{ - auto* outputs = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->pid) && (ret = p->pid->getPidOutputs(pidtype, outputs))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - outs[juser] = outputs[jdevice]; - } - } else { - printError("getPidOutouts", p->id, ret); - ret = false; - break; - } - } - - delete[] outputs; - return ret; -} - - -bool ControlBoardWrapperPidControl::setPidOffset(const PidControlTypeEnum& pidtype, int j, double v) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pid) { - return p->pid->setPidOffset(pidtype, static_cast(off + p->base), v); - } - return false; -} - - -bool ControlBoardWrapperPidControl::getPid(const PidControlTypeEnum& pidtype, int j, Pid* p) -{ - //#warning "check for max number of joints!?!?!" - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* s = device.getSubdevice(subIndex); - if (!s) { - return false; - } - - if (s->pid) { - return s->pid->getPid(pidtype, static_cast(off + s->base), p); - } - return false; -} - - -bool ControlBoardWrapperPidControl::getPids(const PidControlTypeEnum& pidtype, Pid* pids) -{ - Pid* pids_device = new Pid[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->pid) && (ret = p->pid->getPids(pidtype, pids_device))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - pids[juser] = pids_device[jdevice]; - } - } else { - printError("getPids", p->id, ret); - ret = false; - break; - } - } - - delete[] pids_device; - return ret; -} - - -bool ControlBoardWrapperPidControl::getPidReference(const PidControlTypeEnum& pidtype, int j, double* ref) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - if (p->pid) { - return p->pid->getPidReference(pidtype, static_cast(off + p->base), ref); - } - return false; -} - - -bool ControlBoardWrapperPidControl::getPidReferences(const PidControlTypeEnum& pidtype, double* refs) -{ - auto* references = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->pid) && (ret = p->pid->getPidReferences(pidtype, references))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - refs[juser] = references[jdevice]; - } - } else { - printError("getPidReferences", p->id, ret); - ret = false; - break; - } - } - - delete[] references; - return ret; -} - - -bool ControlBoardWrapperPidControl::getPidErrorLimit(const PidControlTypeEnum& pidtype, int j, double* limit) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pid) { - return p->pid->getPidErrorLimit(pidtype, static_cast(off + p->base), limit); - } - return false; -} - - -bool ControlBoardWrapperPidControl::getPidErrorLimits(const PidControlTypeEnum& pidtype, double* limits) -{ - auto* lims = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->pid) && (ret = p->pid->getPidErrorLimits(pidtype, lims))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - limits[juser] = lims[jdevice]; - } - } else { - printError("getPidErrorLimits", p->id, ret); - ret = false; - break; - } - } - - delete[] lims; - return ret; -} - - -bool ControlBoardWrapperPidControl::resetPid(const PidControlTypeEnum& pidtype, int j) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pid) { - return p->pid->resetPid(pidtype, static_cast(off + p->base)); - } - return false; -} - - -bool ControlBoardWrapperPidControl::disablePid(const PidControlTypeEnum& pidtype, int j) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pid) { - return p->pid->disablePid(pidtype, static_cast(off + p->base)); - } - return false; -} - - -bool ControlBoardWrapperPidControl::enablePid(const PidControlTypeEnum& pidtype, int j) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pid) { - return p->pid->enablePid(pidtype, static_cast(off + p->base)); - } - return false; -} - - -bool ControlBoardWrapperPidControl::isPidEnabled(const PidControlTypeEnum& pidtype, int j, bool* enabled) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pid) { - return p->pid->isPidEnabled(pidtype, static_cast(off + p->base), enabled); - } - - return false; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperPidControl.h b/src/devices/controlBoardWrapper/ControlBoardWrapperPidControl.h deleted file mode 100644 index 0126a808742..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperPidControl.h +++ /dev/null @@ -1,41 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPIDCONTROL_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPIDCONTROL_H - -#include - -#include "ControlBoardWrapperCommon.h" - -class ControlBoardWrapperPidControl : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IPidControl -{ -public: - bool setPid(const yarp::dev::PidControlTypeEnum& pidtype, int j, const yarp::dev::Pid& p) override; - bool setPids(const yarp::dev::PidControlTypeEnum& pidtype, const yarp::dev::Pid* ps) override; - bool setPidReference(const yarp::dev::PidControlTypeEnum& pidtype, int j, double ref) override; - bool setPidReferences(const yarp::dev::PidControlTypeEnum& pidtype, const double* refs) override; - bool setPidErrorLimit(const yarp::dev::PidControlTypeEnum& pidtype, int j, double limit) override; - bool setPidErrorLimits(const yarp::dev::PidControlTypeEnum& pidtype, const double* limits) override; - bool getPidError(const yarp::dev::PidControlTypeEnum& pidtype, int j, double* err) override; - bool getPidErrors(const yarp::dev::PidControlTypeEnum& pidtype, double* errs) override; - bool getPidOutput(const yarp::dev::PidControlTypeEnum& pidtype, int j, double* out) override; - bool getPidOutputs(const yarp::dev::PidControlTypeEnum& pidtype, double* outs) override; - bool setPidOffset(const yarp::dev::PidControlTypeEnum& pidtype, int j, double v) override; - bool getPid(const yarp::dev::PidControlTypeEnum& pidtype, int j, yarp::dev::Pid* p) override; - bool getPids(const yarp::dev::PidControlTypeEnum& pidtype, yarp::dev::Pid* pids) override; - bool getPidReference(const yarp::dev::PidControlTypeEnum& pidtype, int j, double* ref) override; - bool getPidReferences(const yarp::dev::PidControlTypeEnum& pidtype, double* refs) override; - bool getPidErrorLimit(const yarp::dev::PidControlTypeEnum& pidtype, int j, double* limit) override; - bool getPidErrorLimits(const yarp::dev::PidControlTypeEnum& pidtype, double* limits) override; - bool resetPid(const yarp::dev::PidControlTypeEnum& pidtype, int j) override; - bool disablePid(const yarp::dev::PidControlTypeEnum& pidtype, int j) override; - bool enablePid(const yarp::dev::PidControlTypeEnum& pidtype, int j) override; - bool isPidEnabled(const yarp::dev::PidControlTypeEnum& pidtype, int j, bool* enabled) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPIDCONTROL_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperPositionControl.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperPositionControl.cpp deleted file mode 100644 index ce4f324191b..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperPositionControl.cpp +++ /dev/null @@ -1,566 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperPositionControl.h" - -#include "ControlBoardLogComponent.h" - - -bool ControlBoardWrapperPositionControl::positionMove(int j, double ref) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pos) { - return p->pos->positionMove(static_cast(off + p->base), ref); - } - - return false; -} - - -bool ControlBoardWrapperPositionControl::positionMove(const double* refs) -{ - bool ret = true; - int j_wrap = 0; // index of the wrapper joint - - int nDev = device.subdevices.size(); - for (int subDev_idx = 0; subDev_idx < nDev; subDev_idx++) { - size_t subIndex = device.lut[j_wrap].deviceEntry; - SubDevice* p = device.getSubdevice(subIndex); - - if (!p) { - return false; - } - - int wrapped_joints = static_cast((p->top - p->base) + 1); - int* joints = new int[wrapped_joints]; - - if (p->pos) { - // versione comandi su subset di giunti - for (int j_dev = 0; j_dev < wrapped_joints; j_dev++) { - joints[j_dev] = static_cast(p->base + j_dev); // for all joints is equivalent to add offset term - } - - ret = ret && p->pos->positionMove(wrapped_joints, joints, &refs[j_wrap]); - j_wrap += wrapped_joints; - } else { - ret = false; - } - - if (joints != nullptr) { - delete[] joints; - joints = nullptr; - } - } - - return ret; -} - - -bool ControlBoardWrapperPositionControl::positionMove(const int n_joints, const int* joints, const double* refs) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = refs[j]; - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->pos) { - ret = ret && rpcData.subdevices_p[subIndex]->pos->positionMove(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.values[subIndex]); - } else { - ret = false; - } - } - rpcDataMutex.unlock(); - return ret; -} - - -bool ControlBoardWrapperPositionControl::getTargetPosition(const int j, double* ref) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - - if (!p) { - return false; - } - - if (p->pos) { - bool ret = p->pos->getTargetPosition(static_cast(off + p->base), ref); - return ret; - } - *ref = 0; - return false; -} - - -bool ControlBoardWrapperPositionControl::getTargetPositions(double* spds) -{ - auto* targets = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->pos) && (ret = p->pos->getTargetPositions(targets))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - spds[juser] = targets[jdevice]; - } - } else { - printError("getTargetPositions", p->id, ret); - ret = false; - break; - } - } - - delete[] targets; - return ret; -} - - -bool ControlBoardWrapperPositionControl::getTargetPositions(const int n_joints, const int* joints, double* targets) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->pos) { - ret = ret && rpcData.subdevices_p[subIndex]->pos->getTargetPositions(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.values[subIndex]); - } - } - - if (ret) { - // ReMix values by user expectations - for (size_t i = 0; i < rpcData.deviceNum; i++) { - rpcData.subdev_jointsVectorLen[i] = 0; // reset tmp index - } - - // fill the output vector - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - targets[j] = rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]]; - rpcData.subdev_jointsVectorLen[subIndex]++; - } - } else { - for (int j = 0; j < n_joints; j++) { - targets[j] = 0; - } - } - rpcDataMutex.unlock(); - return ret; -} - - -bool ControlBoardWrapperPositionControl::relativeMove(int j, double delta) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pos) { - return p->pos->relativeMove(static_cast(off + p->base), delta); - } - - return false; -} - - -bool ControlBoardWrapperPositionControl::relativeMove(const double* deltas) -{ - bool ret = true; - - for (size_t l = 0; l < controlledJoints; l++) { - int off = device.lut[l].offset; - size_t subIndex = device.lut[l].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pos) { - ret = ret && p->pos->relativeMove(static_cast(off + p->base), deltas[l]); - } else { - ret = false; - } - } - return ret; -} - - -bool ControlBoardWrapperPositionControl::relativeMove(const int n_joints, const int* joints, const double* deltas) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = deltas[j]; - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->pos) { - ret = ret && rpcData.subdevices_p[subIndex]->pos->relativeMove(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.values[subIndex]); - } else { - ret = false; - } - } - rpcDataMutex.unlock(); - return ret; -} - - -bool ControlBoardWrapperPositionControl::checkMotionDone(int j, bool* flag) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - - if (!p) { - return false; - } - - if (p->pos) { - return p->pos->checkMotionDone(static_cast(off + p->base), flag); - } - - return false; -} - - -bool ControlBoardWrapperPositionControl::checkMotionDone(bool* flag) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - // In this case the "all joint version" of checkMotionDone(bool *flag) cannot be - // called because the return value is an 'and' of all joints. - // Therefore only the corret joints must be evaluated. - - size_t subIndex = 0; - for (size_t j = 0; j < controlledJoints; j++) { - subIndex = device.lut[j].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[j].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - bool tmp_subdeviceDone = true; - bool tmp_deviceDone = true; - - // for each subdevice wrapped call checkmotiondone only on interested joints - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->pos) { - ret = ret && rpcData.subdevices_p[subIndex]->pos->checkMotionDone(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], &tmp_subdeviceDone); - tmp_deviceDone &= tmp_subdeviceDone; - } - } - rpcDataMutex.unlock(); - - // return a single value to the caller - *flag = tmp_deviceDone; - return ret; -} - - -bool ControlBoardWrapperPositionControl::checkMotionDone(const int n_joints, const int* joints, bool* flags) -{ - bool ret = true; - bool tmp = true; - bool XFlags = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->pos) { - ret = ret && rpcData.subdevices_p[subIndex]->pos->checkMotionDone(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], &XFlags); - tmp = tmp && XFlags; - } else { - ret = false; - } - } - if (ret) { - *flags = tmp; - } else { - *flags = false; - } - rpcDataMutex.unlock(); - return ret; -} - - -bool ControlBoardWrapperPositionControl::setRefSpeed(int j, double sp) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->pos) { - return p->pos->setRefSpeed(static_cast(off + p->base), sp); - } - return false; -} - - -bool ControlBoardWrapperPositionControl::setRefSpeeds(const double* spds) -{ - bool ret = true; - int j_wrap = 0; // index of the wrapper joint - - for (size_t subDev_idx = 0; subDev_idx < device.subdevices.size(); subDev_idx++) { - SubDevice* p = device.getSubdevice(subDev_idx); - - if (!p) { - return false; - } - - int wrapped_joints = static_cast((p->top - p->base) + 1); - int* joints = new int[wrapped_joints]; - - if (p->pos) { - // verione comandi su subset di giunti - for (int j_dev = 0; j_dev < wrapped_joints; j_dev++) { - joints[j_dev] = static_cast(p->base + j_dev); - } - - ret = ret && p->pos->setRefSpeeds(wrapped_joints, joints, &spds[j_wrap]); - j_wrap += wrapped_joints; - } else { - ret = false; - } - - if (joints != nullptr) { - delete[] joints; - joints = nullptr; - } - } - - return ret; -} - - -bool ControlBoardWrapperPositionControl::setRefSpeeds(const int n_joints, const int* joints, const double* spds) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = spds[j]; - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->pos) { - ret = ret && rpcData.subdevices_p[subIndex]->pos->setRefSpeeds(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.values[subIndex]); - } else { - ret = false; - } - } - rpcDataMutex.unlock(); - return ret; -} - - -bool ControlBoardWrapperPositionControl::getRefSpeed(int j, double* ref) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - - if (!p) { - return false; - } - - if (p->pos) { - return p->pos->getRefSpeed(static_cast(off + p->base), ref); - } - *ref = 0; - return false; -} - - -bool ControlBoardWrapperPositionControl::getRefSpeeds(double* spds) -{ - auto* references = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->pos) && (ret = p->pos->getRefSpeeds(references))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - spds[juser] = references[jdevice]; - } - } else { - printError("getRefSpeeds", p->id, ret); - ret = false; - break; - } - } - - delete[] references; - return ret; -} - - -bool ControlBoardWrapperPositionControl::getRefSpeeds(const int n_joints, const int* joints, double* spds) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->pos) { - ret = ret && rpcData.subdevices_p[subIndex]->pos->getRefSpeeds(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.values[subIndex]); - } else { - ret = false; - } - } - - if (ret) { - // ReMix values by user expectations - for (size_t i = 0; i < rpcData.deviceNum; i++) { - rpcData.subdev_jointsVectorLen[i] = 0; // reset tmp index - } - - // fill the output vector - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - spds[j] = rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]]; - rpcData.subdev_jointsVectorLen[subIndex]++; - } - } else { - for (int j = 0; j < n_joints; j++) { - spds[j] = 0; - } - } - rpcDataMutex.unlock(); - return ret; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperPositionControl.h b/src/devices/controlBoardWrapper/ControlBoardWrapperPositionControl.h deleted file mode 100644 index 5c4757d2de4..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperPositionControl.h +++ /dev/null @@ -1,52 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPOSITIONCONTROL_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPOSITIONCONTROL_H - -#include - -#include "ControlBoardWrapperCommon.h" - - -class ControlBoardWrapperPositionControl : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IPositionControl -{ -public: - inline bool getAxes(int* ax) override - { - return ControlBoardWrapperCommon::getAxes(ax); - } - bool positionMove(int j, double ref) override; - bool positionMove(const double* refs) override; - bool positionMove(const int n_joints, const int* joints, const double* refs) override; - bool getTargetPosition(const int joint, double* ref) override; - bool getTargetPositions(double* refs) override; - bool getTargetPositions(const int n_joint, const int* joints, double* refs) override; - bool relativeMove(int j, double delta) override; - bool relativeMove(const double* deltas) override; - bool relativeMove(const int n_joints, const int* joints, const double* deltas) override; - bool checkMotionDone(int j, bool* flag) override; - bool checkMotionDone(bool* flag) override; - bool checkMotionDone(const int n_joints, const int* joints, bool* flags) override; - bool setRefSpeed(int j, double sp) override; - bool setRefSpeeds(const double* spds) override; - bool setRefSpeeds(const int n_joints, const int* joints, const double* spds) override; - inline bool setRefAcceleration(int j, double acc) override { return ControlBoardWrapperCommon::setRefAcceleration(j, acc); } - inline bool setRefAccelerations(const double* accs) override { return ControlBoardWrapperCommon::setRefAccelerations(accs); } - inline bool setRefAccelerations(const int n_joints, const int* joints, const double* accs) override { return ControlBoardWrapperCommon::setRefAccelerations(n_joints, joints, accs); } - bool getRefSpeed(int j, double* ref) override; - bool getRefSpeeds(double* spds) override; - bool getRefSpeeds(const int n_joints, const int* joints, double* spds) override; - inline bool getRefAcceleration(int j, double* acc) override { return ControlBoardWrapperCommon::getRefAcceleration(j, acc); } - inline bool getRefAccelerations(double* accs) override { return ControlBoardWrapperCommon::getRefAccelerations(accs); } - inline bool getRefAccelerations(const int n_joints, const int* joints, double* accs) override { return ControlBoardWrapperCommon::getRefAccelerations(n_joints, joints, accs); } - inline bool stop(int j) override { return ControlBoardWrapperCommon::stop(j); } - inline bool stop() override { return ControlBoardWrapperCommon::stop(); } - inline bool stop(const int n_joints, const int* joints) override { return ControlBoardWrapperCommon::stop(n_joints, joints); } -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPOSITIONCONTROL_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperPositionDirect.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperPositionDirect.cpp deleted file mode 100644 index e9d1467ef89..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperPositionDirect.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperPositionDirect.h" - -#include "ControlBoardLogComponent.h" - - -bool ControlBoardWrapperPositionDirect::setPosition(int j, double ref) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->posDir) { - return p->posDir->setPosition(static_cast(off + p->base), ref); - } - - return false; -} - - -bool ControlBoardWrapperPositionDirect::setPositions(const int n_joints, const int* joints, const double* dpos) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - int offset = device.lut[joints[j]].offset; - int base = rpcData.subdevices_p[subIndex]->base; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = offset + base; - rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = dpos[j]; - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->posDir) // Position Direct - { - ret = ret && rpcData.subdevices_p[subIndex]->posDir->setPositions(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.values[subIndex]); - } else { - ret = false; - } - } - rpcDataMutex.unlock(); - return ret; -} - - -bool ControlBoardWrapperPositionDirect::setPositions(const double* refs) -{ - bool ret = true; - - for (size_t l = 0; l < controlledJoints; l++) { - int off = device.lut[l].offset; - size_t subIndex = device.lut[l].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->posDir) { - ret = p->posDir->setPosition(static_cast(off + p->base), refs[l]) && ret; - } else { - ret = false; - } - } - return ret; -} - - -bool ControlBoardWrapperPositionDirect::getRefPosition(const int j, double* ref) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - - if (!p) { - return false; - } - - if (p->posDir) { - bool ret = p->posDir->getRefPosition(static_cast(off + p->base), ref); - return ret; - } - *ref = 0; - return false; -} - - -bool ControlBoardWrapperPositionDirect::getRefPositions(double* spds) -{ - auto* references = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->posDir) && (ret = p->posDir->getRefPositions(references))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - spds[juser] = references[jdevice]; - } - } else { - printError("getRefPositions", p->id, ret); - ret = false; - break; - } - } - - delete[] references; - return ret; -} - - -bool ControlBoardWrapperPositionDirect::getRefPositions(const int n_joints, const int* joints, double* targets) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->posDir) { - ret = ret && rpcData.subdevices_p[subIndex]->posDir->getRefPositions(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.values[subIndex]); - } - } - - if (ret) { - // ReMix values by user expectations - for (size_t i = 0; i < rpcData.deviceNum; i++) { - rpcData.subdev_jointsVectorLen[i] = 0; // reset tmp index - } - - // fill the output vector - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - targets[j] = rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]]; - rpcData.subdev_jointsVectorLen[subIndex]++; - } - } else { - for (int j = 0; j < n_joints; j++) { - targets[j] = 0; - } - } - rpcDataMutex.unlock(); - return ret; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperPositionDirect.h b/src/devices/controlBoardWrapper/ControlBoardWrapperPositionDirect.h deleted file mode 100644 index bc643a2aee5..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperPositionDirect.h +++ /dev/null @@ -1,28 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPOSITIONDIRECT_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPOSITIONDIRECT_H - -#include - -#include "ControlBoardWrapperCommon.h" - - -class ControlBoardWrapperPositionDirect : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IPositionDirect -{ -public: - inline bool getAxes(int *ax) override { return ControlBoardWrapperCommon::getAxes(ax); } - bool setPosition(int j, double ref) override; - bool setPositions(const int n_joints, const int* joints, const double* dpos) override; - bool setPositions(const double* refs) override; - bool getRefPosition(const int joint, double* ref) override; - bool getRefPositions(double* refs) override; - bool getRefPositions(const int n_joint, const int* joints, double* refs) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPOSITIONDIRECT_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperPreciselyTimed.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperPreciselyTimed.cpp deleted file mode 100644 index 70150d1722d..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperPreciselyTimed.cpp +++ /dev/null @@ -1,19 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperPreciselyTimed.h" - -#include "ControlBoardLogComponent.h" - -#include - - -yarp::os::Stamp ControlBoardWrapperPreciselyTimed::getLastInputStamp() -{ - timeMutex.lock(); - yarp::os::Stamp ret = time; - timeMutex.unlock(); - return ret; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperPreciselyTimed.h b/src/devices/controlBoardWrapper/ControlBoardWrapperPreciselyTimed.h deleted file mode 100644 index f549918ab1c..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperPreciselyTimed.h +++ /dev/null @@ -1,22 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPRECISELYTIMED_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPRECISELYTIMED_H - -#include - -#include "ControlBoardWrapperCommon.h" - - -class ControlBoardWrapperPreciselyTimed : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IPreciselyTimed -{ -public: - yarp::os::Stamp getLastInputStamp() override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERPRECISELYTIMED_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperRemoteCalibrator.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperRemoteCalibrator.cpp deleted file mode 100644 index 018df856530..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperRemoteCalibrator.cpp +++ /dev/null @@ -1,104 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperRemoteCalibrator.h" - -#include - -#include "ControlBoardLogComponent.h" - -using yarp::dev::IRemoteCalibrator; - -IRemoteCalibrator* ControlBoardWrapperRemoteCalibrator::getCalibratorDevice() -{ - yCTrace(CONTROLBOARD); - return yarp::dev::IRemoteCalibrator::getCalibratorDevice(); -} - -bool ControlBoardWrapperRemoteCalibrator::isCalibratorDevicePresent(bool* isCalib) -{ - yCTrace(CONTROLBOARD); - return yarp::dev::IRemoteCalibrator::isCalibratorDevicePresent(isCalib); -} - -bool ControlBoardWrapperRemoteCalibrator::calibrateSingleJoint(int j) -{ - yCTrace(CONTROLBOARD); - if (!getCalibratorDevice()) { - return false; - } - - return IRemoteCalibrator::getCalibratorDevice()->calibrateSingleJoint(j); -} - -bool ControlBoardWrapperRemoteCalibrator::calibrateWholePart() -{ - yCTrace(CONTROLBOARD); - if (!getCalibratorDevice()) { - return false; - } - - return getCalibratorDevice()->calibrateWholePart(); -} - -bool ControlBoardWrapperRemoteCalibrator::homingSingleJoint(int j) -{ - yCTrace(CONTROLBOARD); - if (!getCalibratorDevice()) { - return false; - } - - return getCalibratorDevice()->homingSingleJoint(j); -} - -bool ControlBoardWrapperRemoteCalibrator::homingWholePart() -{ - yCTrace(CONTROLBOARD); - if (!getCalibratorDevice()) { - return false; - } - - return getCalibratorDevice()->homingWholePart(); -} - -bool ControlBoardWrapperRemoteCalibrator::parkSingleJoint(int j, bool _wait) -{ - yCTrace(CONTROLBOARD); - if (!getCalibratorDevice()) { - return false; - } - - return getCalibratorDevice()->parkSingleJoint(j, _wait); -} - -bool ControlBoardWrapperRemoteCalibrator::parkWholePart() -{ - yCTrace(CONTROLBOARD); - if (!getCalibratorDevice()) { - return false; - } - - return getCalibratorDevice()->parkWholePart(); -} - -bool ControlBoardWrapperRemoteCalibrator::quitCalibrate() -{ - yCTrace(CONTROLBOARD); - if (!getCalibratorDevice()) { - return false; - } - - return getCalibratorDevice()->quitCalibrate(); -} - -bool ControlBoardWrapperRemoteCalibrator::quitPark() -{ - yCTrace(CONTROLBOARD); - if (!getCalibratorDevice()) { - return false; - } - - return getCalibratorDevice()->quitPark(); -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperRemoteCalibrator.h b/src/devices/controlBoardWrapper/ControlBoardWrapperRemoteCalibrator.h deleted file mode 100644 index 03b25f2058e..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperRemoteCalibrator.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERREMOTECALIBRATOR_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERREMOTECALIBRATOR_H - -#include - -#include "ControlBoardWrapperCommon.h" - - -class ControlBoardWrapperRemoteCalibrator : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IRemoteCalibrator -{ -public: - bool isCalibratorDevicePresent(bool* isCalib) override; - yarp::dev::IRemoteCalibrator* getCalibratorDevice() override; - bool calibrateSingleJoint(int j) override; - bool calibrateWholePart() override; - bool homingSingleJoint(int j) override; - bool homingWholePart() override; - bool parkSingleJoint(int j, bool _wait = true) override; - bool parkWholePart() override; - bool quitCalibrate() override; - bool quitPark() override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERREMOTECALIBRATOR_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperRemoteVariables.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperRemoteVariables.cpp deleted file mode 100644 index cb28a716288..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperRemoteVariables.cpp +++ /dev/null @@ -1,81 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperRemoteVariables.h" - -#include "ControlBoardLogComponent.h" - -using yarp::os::Bottle; - -bool ControlBoardWrapperRemoteVariables::getRemoteVariable(std::string key, yarp::os::Bottle& val) -{ - bool b = true; - - for (size_t i = 0; i < device.subdevices.size(); i++) { - SubDevice* p = device.getSubdevice(i); - - if (!p) { - return false; - } - if (!p->iVar) { - return false; - } - yarp::os::Bottle tmpval; - b &= p->iVar->getRemoteVariable(key, tmpval); - if (b) { - val.append(tmpval); - } - } - - return b; -} - -bool ControlBoardWrapperRemoteVariables::setRemoteVariable(std::string key, const yarp::os::Bottle& val) -{ - size_t bottle_size = val.size(); - size_t device_size = device.subdevices.size(); - if (bottle_size != device_size) { - yCError(CONTROLBOARD, "setRemoteVariable bottle_size != device_size failure"); - return false; - } - - bool b = true; - for (size_t i = 0; i < device_size; i++) { - SubDevice* p = device.getSubdevice(i); - if (!p) { - yCError(CONTROLBOARD, "setRemoteVariable !p failure"); - return false; - } - if (!p->iVar) { - yCError(CONTROLBOARD, "setRemoteVariable !p->iVar failure"); - return false; - } - Bottle* partial_val = val.get(i).asList(); - if (partial_val) { - b &= p->iVar->setRemoteVariable(key, *partial_val); - } else { - yCError(CONTROLBOARD, "setRemoteVariable general failure"); - return false; - } - } - - return b; -} - -bool ControlBoardWrapperRemoteVariables::getRemoteVariablesList(yarp::os::Bottle* listOfKeys) -{ - //int off = device.lut[0].offset; - size_t subIndex = device.lut[0].deviceEntry; - SubDevice* p = device.getSubdevice(subIndex); - - if (!p) { - return false; - } - - if (p->iVar) { - return p->iVar->getRemoteVariablesList(listOfKeys); - } - return false; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperRemoteVariables.h b/src/devices/controlBoardWrapper/ControlBoardWrapperRemoteVariables.h deleted file mode 100644 index 2b8ff5976e9..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperRemoteVariables.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERREMOTEVARIABLES_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERREMOTEVARIABLES_H - -#include - -#include "ControlBoardWrapperCommon.h" - - -class ControlBoardWrapperRemoteVariables : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IRemoteVariables -{ -public: - bool getRemoteVariable(std::string key, yarp::os::Bottle& val) override; - bool setRemoteVariable(std::string key, const yarp::os::Bottle& val) override; - bool getRemoteVariablesList(yarp::os::Bottle* listOfKeys) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERREMOTEVARIABLES_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperTorqueControl.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperTorqueControl.cpp deleted file mode 100644 index 35159a20b53..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperTorqueControl.cpp +++ /dev/null @@ -1,276 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperTorqueControl.h" - -#include "ControlBoardLogComponent.h" - - -bool ControlBoardWrapperTorqueControl::getRefTorques(double* refs) -{ - auto* references = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iTorque) && (ret = p->iTorque->getRefTorques(references))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - refs[juser] = references[jdevice]; - } - } else { - printError("getRefTorques", p->id, ret); - ret = false; - break; - } - } - - delete[] references; - return ret; -} - -bool ControlBoardWrapperTorqueControl::getRefTorque(int j, double* t) -{ - - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iTorque) { - return p->iTorque->getRefTorque(static_cast(off + p->base), t); - } - return false; -} - -bool ControlBoardWrapperTorqueControl::setRefTorques(const double* t) -{ - bool ret = true; - - for (size_t l = 0; l < controlledJoints; l++) { - int off = device.lut[l].offset; - size_t subIndex = device.lut[l].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iTorque) { - ret = ret && p->iTorque->setRefTorque(static_cast(off + p->base), t[l]); - } else { - ret = false; - } - } - return ret; -} - -bool ControlBoardWrapperTorqueControl::setRefTorque(int j, double t) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iTorque) { - return p->iTorque->setRefTorque(static_cast(off + p->base), t); - } - return false; -} - -bool ControlBoardWrapperTorqueControl::setRefTorques(const int n_joints, const int* joints, const double* t) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = t[j]; - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->iTorque) { - ret = ret && rpcData.subdevices_p[subIndex]->iTorque->setRefTorques(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.values[subIndex]); - } else { - ret = false; - } - } - rpcDataMutex.unlock(); - return ret; -} - -bool ControlBoardWrapperTorqueControl::getMotorTorqueParams(int j, yarp::dev::MotorTorqueParameters* params) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iTorque) { - return p->iTorque->getMotorTorqueParams(static_cast(off + p->base), params); - } - return false; -} - -bool ControlBoardWrapperTorqueControl::setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iTorque) { - return p->iTorque->setMotorTorqueParams(static_cast(off + p->base), params); - } - return false; -} - -bool ControlBoardWrapperTorqueControl::getTorque(int j, double* t) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iTorque) { - return p->iTorque->getTorque(static_cast(off + p->base), t); - } - - return false; -} - -bool ControlBoardWrapperTorqueControl::getTorques(double* t) -{ - auto* trqs = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iTorque) && (ret = p->iTorque->getTorques(trqs))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - t[juser] = trqs[jdevice]; - } - } else { - printError("getTorques", p->id, ret); - ret = false; - break; - } - } - - delete[] trqs; - return ret; -} - -bool ControlBoardWrapperTorqueControl::getTorqueRange(int j, double* min, double* max) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - if (!p) { - return false; - } - - if (p->iTorque) { - return p->iTorque->getTorqueRange(static_cast(off + p->base), min, max); - } - - return false; -} - -bool ControlBoardWrapperTorqueControl::getTorqueRanges(double* min, double* max) -{ - auto* t_min = new double[device.maxNumOfJointsInDevices]; - auto* t_max = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->iTorque) && (ret = p->iTorque->getTorqueRanges(t_min, t_max))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - min[juser] = t_min[jdevice]; - max[juser] = t_max[jdevice]; - } - } else { - printError("getTorqueRanges", p->id, ret); - ret = false; - break; - } - } - - delete[] t_min; - delete[] t_max; - return ret; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperTorqueControl.h b/src/devices/controlBoardWrapper/ControlBoardWrapperTorqueControl.h deleted file mode 100644 index 71a2e459320..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperTorqueControl.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERTORQUECONTROL_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERTORQUECONTROL_H - -#include - -#include "ControlBoardWrapperCommon.h" - -class ControlBoardWrapperTorqueControl : - virtual public ControlBoardWrapperCommon, - public yarp::dev::ITorqueControl -{ -public: - inline bool getAxes(int *ax) override { return ControlBoardWrapperCommon::getAxes(ax); } - bool getRefTorques(double* refs) override; - bool getRefTorque(int j, double* t) override; - bool setRefTorques(const double* t) override; - bool setRefTorque(int j, double t) override; - bool setRefTorques(const int n_joint, const int* joints, const double* t) override; - bool getMotorTorqueParams(int j, yarp::dev::MotorTorqueParameters* params) override; - bool setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params) override; - bool getTorque(int j, double* t) override; - bool getTorques(double* t) override; - bool getTorqueRange(int j, double* min, double* max) override; - bool getTorqueRanges(double* min, double* max) override; -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERTORQUECONTROL_H diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperVelocityControl.cpp b/src/devices/controlBoardWrapper/ControlBoardWrapperVelocityControl.cpp deleted file mode 100644 index 5c36dceac41..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperVelocityControl.cpp +++ /dev/null @@ -1,198 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoardWrapperVelocityControl.h" - -#include - -#include "ControlBoardLogComponent.h" - -bool ControlBoardWrapperVelocityControl::velocityMove(int j, double v) -{ - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - - if (!p) { - return false; - } - - if (p->vel) { - return p->vel->velocityMove(static_cast(off + p->base), v); - } - return false; -} - -bool ControlBoardWrapperVelocityControl::velocityMove(const double* v) -{ - bool ret = true; - int j_wrap = 0; // index of the wrapper joint - - for (size_t subDev_idx = 0; subDev_idx < device.subdevices.size(); subDev_idx++) { - SubDevice* p = device.getSubdevice(subDev_idx); - - if (!p) { - return false; - } - - int wrapped_joints = static_cast((p->top - p->base) + 1); - int* joints = new int[wrapped_joints]; - - if (p->vel) { - // verione comandi su subset di giunti - for (int j_dev = 0; j_dev < wrapped_joints; j_dev++) { - joints[j_dev] = static_cast(p->base + j_dev); - } - - ret = ret && p->vel->velocityMove(wrapped_joints, joints, &v[j_wrap]); - j_wrap += wrapped_joints; - } else { - ret = false; - } - - if (joints != nullptr) { - delete[] joints; - joints = nullptr; - } - } - - return ret; -} - -bool ControlBoardWrapperVelocityControl::velocityMove(const int n_joints, const int* joints, const double* spds) -{ - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = spds[j]; - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->vel) { - ret = ret && rpcData.subdevices_p[subIndex]->vel->velocityMove(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.values[subIndex]); - } else { - ret = false; - } - } - rpcDataMutex.unlock(); - return ret; -} - -bool ControlBoardWrapperVelocityControl::getRefVelocity(const int j, double* vel) -{ - yCTrace(CONTROLBOARD); - - size_t off; - try { - off = device.lut.at(j).offset; - } catch (...) { - yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str()); - return false; - } - size_t subIndex = device.lut[j].deviceEntry; - - SubDevice* p = device.getSubdevice(subIndex); - - if (!p) { - return false; - } - - if (p->vel) { - bool ret = p->vel->getRefVelocity(static_cast(off + p->base), vel); - return ret; - } - *vel = 0; - return false; -} - - -bool ControlBoardWrapperVelocityControl::getRefVelocities(double* vels) -{ - auto* references = new double[device.maxNumOfJointsInDevices]; - bool ret = true; - for (size_t d = 0; d < device.subdevices.size(); d++) { - SubDevice* p = device.getSubdevice(d); - if (!p) { - ret = false; - break; - } - - if ((p->vel) && (ret = p->vel->getRefVelocities(references))) { - for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) { - vels[juser] = references[jdevice]; - } - } else { - printError("getRefVelocities", p->id, ret); - ret = false; - break; - } - } - - delete[] references; - return ret; -} - -bool ControlBoardWrapperVelocityControl::getRefVelocities(const int n_joints, const int* joints, double* vels) -{ - yCTrace(CONTROLBOARD); - - bool ret = true; - - rpcDataMutex.lock(); - //Reset subdev_jointsVectorLen vector - memset(rpcData.subdev_jointsVectorLen, 0x00, sizeof(int) * rpcData.deviceNum); - - // Create a map of joints for each subDevice - size_t subIndex = 0; - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - rpcData.jointNumbers[subIndex][rpcData.subdev_jointsVectorLen[subIndex]] = - static_cast(device.lut[joints[j]].offset + rpcData.subdevices_p[subIndex]->base); - rpcData.subdev_jointsVectorLen[subIndex]++; - } - - for (subIndex = 0; subIndex < rpcData.deviceNum; subIndex++) { - if (rpcData.subdevices_p[subIndex]->vel) { - ret = ret && rpcData.subdevices_p[subIndex]->vel->getRefVelocities(rpcData.subdev_jointsVectorLen[subIndex], rpcData.jointNumbers[subIndex], rpcData.values[subIndex]); - } - } - - if (ret) { - // ReMix values by user expectations - for (size_t i = 0; i < rpcData.deviceNum; i++) { - rpcData.subdev_jointsVectorLen[i] = 0; // reset tmp index - } - - // fill the output vector - for (int j = 0; j < n_joints; j++) { - subIndex = device.lut[joints[j]].deviceEntry; - vels[j] = rpcData.values[subIndex][rpcData.subdev_jointsVectorLen[subIndex]]; - rpcData.subdev_jointsVectorLen[subIndex]++; - } - } else { - for (int j = 0; j < n_joints; j++) { - vels[j] = 0; - } - } - rpcDataMutex.unlock(); - return ret; -} diff --git a/src/devices/controlBoardWrapper/ControlBoardWrapperVelocityControl.h b/src/devices/controlBoardWrapper/ControlBoardWrapperVelocityControl.h deleted file mode 100644 index ec32a529e52..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoardWrapperVelocityControl.h +++ /dev/null @@ -1,37 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERVELOCITYCONTROL_H -#define YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERVELOCITYCONTROL_H - -#include - -#include "ControlBoardWrapperCommon.h" - - -class ControlBoardWrapperVelocityControl : - virtual public ControlBoardWrapperCommon, - public yarp::dev::IVelocityControl -{ -public: - inline bool getAxes(int* ax) override { return ControlBoardWrapperCommon::getAxes(ax); } - bool velocityMove(int j, double v) override; - bool velocityMove(const double* v) override; - bool velocityMove(const int n_joints, const int* joints, const double* spds) override; - bool getRefVelocity(const int joint, double* vel) override; - bool getRefVelocities(double* vels) override; - bool getRefVelocities(const int n_joint, const int* joints, double* vels) override; - inline bool setRefAcceleration(int j, double acc) override { return ControlBoardWrapperCommon::setRefAcceleration(j, acc); } - inline bool setRefAccelerations(const double* accs) override { return ControlBoardWrapperCommon::setRefAccelerations(accs); } - inline bool setRefAccelerations(const int n_joints, const int* joints, const double* accs) override { return ControlBoardWrapperCommon::setRefAccelerations(n_joints, joints, accs); } - inline bool getRefAcceleration(int j, double* acc) override { return ControlBoardWrapperCommon::getRefAcceleration(j, acc); } - inline bool getRefAccelerations(double* accs) override { return ControlBoardWrapperCommon::getRefAccelerations(accs); } - inline bool getRefAccelerations(const int n_joints, const int* joints, double* accs) override { return ControlBoardWrapperCommon::getRefAccelerations(n_joints, joints, accs); } - inline bool stop(int j) override { return ControlBoardWrapperCommon::stop(j); } - inline bool stop() override { return ControlBoardWrapperCommon::stop(); } - inline bool stop(const int n_joint, const int* joints) override { return ControlBoardWrapperCommon::stop(n_joint, joints); } -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_CONTROLBOARDWRAPPERVELOCITYCONTROL_H diff --git a/src/devices/controlBoardWrapper/ControlBoard_nws_yarp.cpp b/src/devices/controlBoardWrapper/ControlBoard_nws_yarp.cpp deleted file mode 100644 index 2cbd41db98c..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoard_nws_yarp.cpp +++ /dev/null @@ -1,536 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "ControlBoard_nws_yarp.h" - -#include "ControlBoardLogComponent.h" -#include "RPCMessagesParser.h" -#include "StreamingMessagesParser.h" - -#include -#include - -#include -#include - -using namespace yarp::os; -using namespace yarp::dev; -using namespace yarp::sig; -using yarp::dev::impl::jointData; - -ControlBoard_nws_yarp::ControlBoard_nws_yarp() : - yarp::os::PeriodicThread(default_period) -{ -} - -void ControlBoard_nws_yarp::closePorts() -{ - inputRPCPort.interrupt(); - inputRPCPort.removeCallbackLock(); - inputRPCPort.close(); - - inputStreamingPort.interrupt(); - inputStreamingPort.close(); - - outputPositionStatePort.interrupt(); - outputPositionStatePort.close(); - - extendedOutputStatePort.interrupt(); - extendedOutputStatePort.close(); -} - -bool ControlBoard_nws_yarp::close() -{ - // Ensure that the device is not running - if (isRunning()) { - stop(); - } - - closeDevice(); - closePorts(); - - return true; -} - -bool ControlBoard_nws_yarp::checkPortName(Searchable& params) -{ - // find name as port name (similar both in new and old policy) - if (!params.check("name")) { - yCError(CONTROLBOARD) << - "*************************************************************************************\n" - "* controlBoard_nws_yarp missing mandatory parameter 'name' for port name, usage is: *\n" - "* name: full port prefix name with leading '/', e.g. /robotName/part/ *\n" - "*************************************************************************************"; - return false; - } - - partName = params.find("name").asString(); - if (partName[0] != '/') { - yCWarning(CONTROLBOARD) << - "*************************************************************************************\n" - "* controlBoard_nws_yarp 'name' parameter for port name does not follow convention, *\n" - "* it MUST start with a leading '/' since it is used as the full prefix port name *\n" - "* name: full port prefix name with leading '/', e.g. /robotName/part/ *\n" - "* A temporary automatic fix will be done for you, but please fix your config file *\n" - "*************************************************************************************"; - rootName = "/" + partName; - } else { - rootName = partName; - } - - return true; -} - - -bool ControlBoard_nws_yarp::open(Searchable& config) -{ - Property prop; - prop.fromString(config.toString()); - - if (!checkPortName(config)) { - yCError(CONTROLBOARD) << "'portName' was not correctly set, check you r configuration file"; - return false; - } - - // Check parameter, so if both are present we use the correct one - if (prop.check("period")) { - if (!prop.find("period").isFloat64()) { - yCError(CONTROLBOARD) << "'period' parameter is not a double value"; - return false; - } - period = prop.find("period").asFloat64(); - if (period <= 0) { - yCError(CONTROLBOARD) << "'period' parameter is not valid, read value is" << period; - return false; - } - } else { - yCDebug(CONTROLBOARD) << "'period' parameter missing, using default thread period = 0.02s"; - period = default_period; - } - - // Check if we need to create subdevice or if they are - // passed later on through attachAll() - if (prop.check("subdevice")) { - prop.setMonitor(config.getMonitor()); - if (!openAndAttachSubDevice(prop)) { - yCError(CONTROLBOARD, "Error while opening subdevice"); - return false; - } - subdevice_ready = true; - } - - rootName = prop.check("rootName", Value("/"), "starting '/' if needed.").asString(); - partName = prop.check("name", Value("controlboard"), "prefix for port names").asString(); - rootName += (partName); - if (rootName.find("//") != std::string::npos) { - rootName.replace(rootName.find("//"), 2, "/"); - } - - // Open ports, then attach the readers or callbacks - if (!inputRPCPort.open((rootName + "/rpc:i"))) { - yCError(CONTROLBOARD) << "Error opening port " << rootName + "/rpc:i"; - closePorts(); - return false; - } - inputRPCPort.setReader(RPC_parser); - inputRPC_buffer.attach(inputRPCPort); - RPC_parser.attach(inputRPC_buffer); - - if (!inputStreamingPort.open(rootName + "/command:i")) { - yCError(CONTROLBOARD) << "Error opening port " << rootName + "/rpc:i"; - closePorts(); - return false; - } - inputStreamingPort.setStrict(); - inputStreamingPort.useCallback(streaming_parser); - - if (!outputPositionStatePort.open(rootName + "/state:o")) { - yCError(CONTROLBOARD) << "Error opening port " << rootName + "/state:o"; - closePorts(); - return false; - } - - // Extended output state port - if (!extendedOutputStatePort.open(rootName + "/stateExt:o")) { - yCError(CONTROLBOARD) << "Error opening port " << rootName + "/state:o"; - closePorts(); - return false; - } - extendedOutputState_buffer.attach(extendedOutputStatePort); - - // In case attach is not deferred and the controlboard already owns a valid device - // we can start the thread. Otherwise this will happen when attachAll is called - if (subdevice_ready) { - setPeriod(period); - if (!start()) { - yCError(CONTROLBOARD) << "Error starting thread"; - return false; - } - } - - return true; -} - - -// For the simulator, if a subdevice parameter is given to the wrapper, it will -// open it and attach to immediately. -bool ControlBoard_nws_yarp::openAndAttachSubDevice(Property& prop) -{ - Property p; - auto* subDeviceOwned = new PolyDriver; - p.fromString(prop.toString()); - - std::string subdevice = prop.find("subdevice").asString(); - p.setMonitor(prop.getMonitor(), subdevice.c_str()); // pass on any monitoring - p.unput("device"); - p.put("device", subdevice); // subdevice was already checked before - - // if errors occurred during open, quit here. - yCDebug(CONTROLBOARD, "opening subdevice"); - subDeviceOwned->open(p); - - if (!subDeviceOwned->isValid()) { - yCError(CONTROLBOARD, "opening subdevice... FAILED"); - return false; - } - - return setDevice(subDeviceOwned, true); -} - - -bool ControlBoard_nws_yarp::setDevice(yarp::dev::DeviceDriver* driver, bool owned) -{ - // Save the pointer and subDeviceOwned - subdevice_ptr = driver; - subdevice_owned = owned; - - // yarp::dev::IJointFault* iJointFault{nullptr}; - subdevice_ptr->view(iJointFault); - if (!iJointFault) { - yCWarning(CONTROLBOARD, "Part <%s>: iJointFault interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::IPidControl* iPidControl{nullptr}; - subdevice_ptr->view(iPidControl); - if (!iPidControl) { - yCWarning(CONTROLBOARD, "Part <%s>: IPidControl interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::IPositionControl* iPositionControl{nullptr}; - subdevice_ptr->view(iPositionControl); - if (!iPositionControl) { - yCError(CONTROLBOARD, "Part <%s>: IPositionControl interface was not found in subdevice. Quitting", partName.c_str()); - return false; - } - - // yarp::dev::IPositionDirect* iPositionDirect{nullptr}; - subdevice_ptr->view(iPositionDirect); - if (!iPositionDirect) { - yCWarning(CONTROLBOARD, "Part <%s>: IPositionDirect interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::IVelocityControl* iVelocityControl{nullptr}; - subdevice_ptr->view(iVelocityControl); - if (!iVelocityControl) { - yCError(CONTROLBOARD, "Part <%s>: IVelocityControl interface was not found in subdevice. Quitting", partName.c_str()); - return false; - } - - // yarp::dev::IEncodersTimed* iEncodersTimed{nullptr}; - subdevice_ptr->view(iEncodersTimed); - if (!iEncodersTimed) { - yCError(CONTROLBOARD, "Part <%s>: IEncodersTimed interface was not found in subdevice. Quitting", partName.c_str()); - return false; - } - - // yarp::dev::IMotor* iMotor{nullptr}; - subdevice_ptr->view(iMotor); - if (!iMotor) { - yCWarning(CONTROLBOARD, "Part <%s>: IMotor interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::IMotorEncoders* iMotorEncoders{nullptr}; - subdevice_ptr->view(iMotorEncoders); - if (!iMotorEncoders) { - yCWarning(CONTROLBOARD, "Part <%s>: IMotorEncoders interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::IAmplifierControl* iAmplifierControl{nullptr}; - subdevice_ptr->view(iAmplifierControl); - if (!iAmplifierControl) { - yCWarning(CONTROLBOARD, "Part <%s>: IAmplifierControl interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::IControlLimits* iControlLimits{nullptr}; - subdevice_ptr->view(iControlLimits); - if (!iControlLimits) { - yCWarning(CONTROLBOARD, "Part <%s>: IControlLimits interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::IControlCalibration* iControlCalibration{nullptr}; - subdevice_ptr->view(iControlCalibration); - if (!iControlCalibration) { - yCWarning(CONTROLBOARD, "Part <%s>: IControlCalibration interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::ITorqueControl* iTorqueControl{nullptr}; - subdevice_ptr->view(iTorqueControl); - if (!iTorqueControl) { - yCWarning(CONTROLBOARD, "Part <%s>: ITorqueControl interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::IImpedanceControl* iImpedanceControl{nullptr}; - subdevice_ptr->view(iImpedanceControl); - if (!iImpedanceControl) { - yCWarning(CONTROLBOARD, "Part <%s>: IImpedanceControl interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::IControlMode* iControlMode{nullptr}; - subdevice_ptr->view(iControlMode); - if (!iControlMode) { - yCWarning(CONTROLBOARD, "Part <%s>: IControlMode interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::IAxisInfo* iAxisInfo{nullptr}; - subdevice_ptr->view(iAxisInfo); - if (!iAxisInfo) { - yCWarning(CONTROLBOARD, "Part <%s>: IAxisInfo interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::IPreciselyTimed* iPreciselyTimed{nullptr}; - subdevice_ptr->view(iPreciselyTimed); - if (!iPreciselyTimed) { - yCWarning(CONTROLBOARD, "Part <%s>: IPreciselyTimed interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::IInteractionMode* iInteractionMode{nullptr}; - subdevice_ptr->view(iInteractionMode); - if (!iInteractionMode) { - yCWarning(CONTROLBOARD, "Part <%s>: IInteractionMode interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::IRemoteVariables* iRemoteVariables{nullptr}; - subdevice_ptr->view(iRemoteVariables); - if (!iRemoteVariables) { - yCWarning(CONTROLBOARD, "Part <%s>: IRemoteVariables interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::IPWMControl* iPWMControl{nullptr}; - subdevice_ptr->view(iPWMControl); - if (!iPWMControl) { - yCWarning(CONTROLBOARD, "Part <%s>: IPWMControl interface was not found in subdevice.", partName.c_str()); - } - - // yarp::dev::ICurrentControl* iCurrentControl{nullptr}; - subdevice_ptr->view(iCurrentControl); - if (!iCurrentControl) { - yCWarning(CONTROLBOARD, "Part <%s>: ICurrentControl interface was not found in subdevice.", partName.c_str()); - } - - - // Get the number of controlled joints - int tmp_axes; - if (!iPositionControl->getAxes(&tmp_axes)) { - yCError(CONTROLBOARD) << "Part <%s>: Failed to get axes number for subdevice " << partName.c_str(); - return false; - } - if (tmp_axes <= 0) { - yCError(CONTROLBOARD, "Part <%s>: attached device has an invalid number of joints (%d)", partName.c_str(), tmp_axes); - return false; - } - subdevice_joints = static_cast(tmp_axes); - times.resize(subdevice_joints); - - // Initialization - streaming_parser.init(subdevice_ptr); - streaming_parser.initialize(); - - RPC_parser.init(subdevice_ptr); - RPC_parser.initialize(); - - return true; -} - -void ControlBoard_nws_yarp::closeDevice() -{ - // Reset callbacks - streaming_parser.reset(); - RPC_parser.reset(); - - // If the subdevice is owned, close and delete the device - if (subdevice_owned) { - subdevice_ptr->close(); - delete subdevice_ptr; - } - subdevice_ptr = nullptr; - subdevice_owned = false; - subdevice_joints = 0; - subdevice_ready = false; - - times.clear(); - - // Clear all interfaces - iPidControl = nullptr; - iPositionControl = nullptr; - iPositionDirect = nullptr; - iVelocityControl = nullptr; - iEncodersTimed = nullptr; - iMotor = nullptr; - iMotorEncoders = nullptr; - iAmplifierControl = nullptr; - iControlLimits = nullptr; - iControlCalibration = nullptr; - iTorqueControl = nullptr; - iImpedanceControl = nullptr; - iControlMode = nullptr; - iAxisInfo = nullptr; - iPreciselyTimed = nullptr; - iInteractionMode = nullptr; - iRemoteVariables = nullptr; - iPWMControl = nullptr; - iCurrentControl = nullptr; - iJointFault = nullptr; -} - -bool ControlBoard_nws_yarp::attach(yarp::dev::PolyDriver* poly) -{ - // Check if we already instantiated a subdevice previously - if (subdevice_ready) { - return false; - } - - if (!setDevice(poly, false)) { - return false; - } - - setPeriod(period); - if (!start()) { - yCError(CONTROLBOARD) << "Error starting thread"; - return false; - } - - return true; -} - -bool ControlBoard_nws_yarp::detach() -{ - //check if we already instantiated a subdevice previously - if (subdevice_owned) { - return false; - } - - // Ensure that the device is not running - if (isRunning()) { - stop(); - } - - closeDevice(); - - return true; -} - -void ControlBoard_nws_yarp::run() -{ - // check we are not overflowing with input messages - constexpr int reads_for_warning = 20; - if (inputStreamingPort.getPendingReads() >= reads_for_warning) { - yCWarning(CONTROLBOARD) << "Number of streaming input messages to be read is " << inputStreamingPort.getPendingReads() << " and can overflow"; - } - - // handle stateExt first - jointData& data = extendedOutputState_buffer.get(); - - data.jointPosition.resize(subdevice_joints); - data.jointVelocity.resize(subdevice_joints); - data.jointAcceleration.resize(subdevice_joints); - data.motorPosition.resize(subdevice_joints); - data.motorVelocity.resize(subdevice_joints); - data.motorAcceleration.resize(subdevice_joints); - data.torque.resize(subdevice_joints); - data.pwmDutycycle.resize(subdevice_joints); - data.current.resize(subdevice_joints); - data.controlMode.resize(subdevice_joints); - data.interactionMode.resize(subdevice_joints); - - // Get data from HW - if (iEncodersTimed) { - data.jointPosition_isValid = iEncodersTimed->getEncodersTimed(data.jointPosition.data(), times.data()); - data.jointVelocity_isValid = iEncodersTimed->getEncoderSpeeds(data.jointVelocity.data()); - data.jointAcceleration_isValid = iEncodersTimed->getEncoderAccelerations(data.jointAcceleration.data()); - } else { - data.jointPosition_isValid = false; - data.jointVelocity_isValid = false; - data.jointAcceleration_isValid = false; - } - - if (iMotorEncoders) { - data.motorPosition_isValid = iMotorEncoders->getMotorEncoders(data.motorPosition.data()); - data.motorVelocity_isValid = iMotorEncoders->getMotorEncoderSpeeds(data.motorVelocity.data()); - data.motorAcceleration_isValid = iMotorEncoders->getMotorEncoderAccelerations(data.motorAcceleration.data()); - } else { - data.motorPosition_isValid = false; - data.motorVelocity_isValid = false; - data.motorAcceleration_isValid = false; - } - - if (iTorqueControl) { - data.torque_isValid = iTorqueControl->getTorques(data.torque.data()); - } else { - data.torque_isValid = false; - } - - if (iPWMControl) { - data.pwmDutycycle_isValid = iPWMControl->getDutyCycles(data.pwmDutycycle.data()); - } else { - data.pwmDutycycle_isValid = false; - } - - if (iCurrentControl) { - data.current_isValid = iCurrentControl->getCurrents(data.current.data()); - } else if (iAmplifierControl) { - data.current_isValid = iAmplifierControl->getCurrents(data.current.data()); - } else { - data.current_isValid = false; - } - - if (iControlMode) { - data.controlMode_isValid = iControlMode->getControlModes(data.controlMode.data()); - } else { - data.controlMode_isValid = false; - } - - if (iInteractionMode) { - data.interactionMode_isValid = iInteractionMode->getInteractionModes(reinterpret_cast(data.interactionMode.data())); - } else { - data.interactionMode_isValid = false; - } - - // Check if the encoders timestamps are consistent. - for (double tt : times) - { - if (std::abs(times[0] - tt) > 1.0) - { - yCError(CONTROLBOARD, "Encoder Timestamps are not consistent! Data will not be published."); - yarp::sig::Vector _data(subdevice_joints); - return; - } - } - - // Update the port envelope time by averaging all timestamps - time.update(std::accumulate(times.begin(), times.end(), 0.0) / subdevice_joints); - yarp::os::Stamp averageTime = time; - - extendedOutputStatePort.setEnvelope(averageTime); - extendedOutputState_buffer.write(); - - // handle state:o - yarp::sig::Vector& v = outputPositionStatePort.prepare(); - v.resize(subdevice_joints); - std::copy(data.jointPosition.begin(), data.jointPosition.end(), v.begin()); - - outputPositionStatePort.setEnvelope(averageTime); - outputPositionStatePort.write(); -} diff --git a/src/devices/controlBoardWrapper/ControlBoard_nws_yarp.h b/src/devices/controlBoardWrapper/ControlBoard_nws_yarp.h deleted file mode 100644 index 72b59d0fc73..00000000000 --- a/src/devices/controlBoardWrapper/ControlBoard_nws_yarp.h +++ /dev/null @@ -1,143 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARD_NWS_YARP_H -#define YARP_DEV_CONTROLBOARD_NWS_YARP_H - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include - -#include "StreamingMessagesParser.h" -#include "RPCMessagesParser.h" - - -/** - * @ingroup dev_impl_nws_yarp - * - * \brief `controlBoard_nws_yarp`: A controlBoard network wrapper server for YARP. - * - * \section controlBoard_nws_yarp_device_parameters Description of input parameters - * - * Parameters required by this device are: - * | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes | - * |:--------------:|:--------------:|:-------:|:--------------:|:-------------:|:--------------------------: |:-----------------------------------------------------------------:|:-----:| - * | name | - | string | - | - | Yes | full name of the port opened by the device, like /robotName/part/ | MUST start with a '/' character | - * | period | - | double | s | 0.02 | No | refresh period of the broadcasted values in s | optional, default 0.02s period| - * | subdevice | - | string | - | - | No | name of the subdevice to instantiate | when used, parameters for the subdevice must be provided as well | - */ - -class ControlBoard_nws_yarp : - public yarp::dev::DeviceDriver, - public yarp::os::PeriodicThread, - public yarp::dev::WrapperSingle -{ -private: - std::string rootName; - - bool checkPortName(yarp::os::Searchable ¶ms); - - yarp::os::BufferedPort outputPositionStatePort; // Port /state:o streaming out the encoder positions - yarp::os::BufferedPort inputStreamingPort; // Input streaming port for high frequency commands - yarp::os::Port inputRPCPort; // Input RPC port for set/get remote calls - yarp::os::Port extendedOutputStatePort; // Port /stateExt:o streaming out the struct with the robot data - - yarp::os::PortWriterBuffer extendedOutputState_buffer; // Buffer associated to the extendedOutputStatePort port - yarp::os::PortReaderBuffer inputRPC_buffer; // Buffer associated to the inputRPCPort port - - RPCMessagesParser RPC_parser; // Message parser associated to the inputRPCPort port - StreamingMessagesParser streaming_parser; // Message parser associated to the inputStreamingPort port - - static constexpr double default_period = 0.02; // s - double period {default_period}; - - std::string partName; // to open ports and print more detailed debug messages - yarp::sig::Vector times; // time for each joint - yarp::os::Stamp time; // envelope to attach to the state port - - yarp::dev::DeviceDriver* subdevice_ptr{nullptr}; - bool subdevice_owned = false; - size_t subdevice_joints {0}; - bool subdevice_ready = false; - - yarp::dev::IPidControl* iPidControl{nullptr}; - yarp::dev::IPositionControl* iPositionControl{nullptr}; - yarp::dev::IPositionDirect* iPositionDirect{nullptr}; - yarp::dev::IVelocityControl* iVelocityControl{nullptr}; - yarp::dev::IEncodersTimed* iEncodersTimed{nullptr}; - yarp::dev::IMotor* iMotor{nullptr}; - yarp::dev::IMotorEncoders* iMotorEncoders{nullptr}; - yarp::dev::IAmplifierControl* iAmplifierControl{nullptr}; - yarp::dev::IControlLimits* iControlLimits{nullptr}; - yarp::dev::IControlCalibration* iControlCalibration{nullptr}; - yarp::dev::ITorqueControl* iTorqueControl{nullptr}; - yarp::dev::IImpedanceControl* iImpedanceControl{nullptr}; - yarp::dev::IControlMode* iControlMode{nullptr}; - yarp::dev::IAxisInfo* iAxisInfo{nullptr}; - yarp::dev::IPreciselyTimed* iPreciselyTimed{nullptr}; - yarp::dev::IInteractionMode* iInteractionMode{nullptr}; - yarp::dev::IRemoteVariables* iRemoteVariables{nullptr}; - yarp::dev::IPWMControl* iPWMControl{nullptr}; - yarp::dev::ICurrentControl* iCurrentControl{nullptr}; - yarp::dev::IJointFault* iJointFault{ nullptr }; - - bool setDevice(yarp::dev::DeviceDriver* device, bool owned); - bool openAndAttachSubDevice(yarp::os::Property& prop); - - void closeDevice(); - void closePorts(); - -public: - ControlBoard_nws_yarp(); - ControlBoard_nws_yarp(const ControlBoard_nws_yarp&) = delete; - ControlBoard_nws_yarp(ControlBoard_nws_yarp&&) = delete; - ControlBoard_nws_yarp& operator=(const ControlBoard_nws_yarp&) = delete; - ControlBoard_nws_yarp& operator=(ControlBoard_nws_yarp&&) = delete; - ~ControlBoard_nws_yarp() override = default; - - // yarp::dev::DeviceDriver - bool close() override; - bool open(yarp::os::Searchable& prop) override; - - // yarp::dev::WrapperSingle - bool attach(yarp::dev::PolyDriver* poly) override; - bool detach() override; - - // yarp::os::PeriodicThread - void run() override; -}; - - -#endif // YARP_DEV_CONTROLBOARD_NWS_YARP_H diff --git a/src/devices/controlBoardWrapper/MultiJointData.h b/src/devices/controlBoardWrapper/MultiJointData.h deleted file mode 100644 index 56fee6c72dc..00000000000 --- a/src/devices/controlBoardWrapper/MultiJointData.h +++ /dev/null @@ -1,68 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_MULTIJOINTDATA_H -#define YARP_DEV_CONTROLBOARDWRAPPER_MULTIJOINTDATA_H - -#include "SubDevice.h" - -class MultiJointData -{ -public: - size_t deviceNum {0}; - size_t maxJointsNumForDevice {0}; - - int* subdev_jointsVectorLen {nullptr}; // number of joints belonging to each subdevice - int** jointNumbers {nullptr}; - int** modes {nullptr}; - double** values {nullptr}; - SubDevice** subdevices_p {nullptr}; - - MultiJointData() = default; - - void resize(int _deviceNum, int _maxJointsNumForDevice, WrappedDevice* _device) - { - deviceNum = _deviceNum; - maxJointsNumForDevice = _maxJointsNumForDevice; - subdev_jointsVectorLen = new int[deviceNum]; - jointNumbers = new int*[deviceNum]; // alloc a vector of pointers - jointNumbers[0] = new int[deviceNum * _maxJointsNumForDevice]; // alloc real memory for data - - modes = new int*[deviceNum]; // alloc a vector of pointers - modes[0] = new int[deviceNum * _maxJointsNumForDevice]; // alloc real memory for data - - values = new double*[deviceNum]; // alloc a vector of pointers - values[0] = new double[deviceNum * _maxJointsNumForDevice]; // alloc real memory for data - - subdevices_p = new SubDevice*[deviceNum]; - subdevices_p[0] = _device->getSubdevice(0); - - for (size_t i = 1; i < deviceNum; i++) { - jointNumbers[i] = jointNumbers[i - 1] + _maxJointsNumForDevice; // set pointer to correct location - values[i] = values[i - 1] + _maxJointsNumForDevice; // set pointer to correct location - modes[i] = modes[i - 1] + _maxJointsNumForDevice; // set pointer to correct location - subdevices_p[i] = _device->getSubdevice(i); - } - } - - void destroy() - { - // release matrix memory - delete[] jointNumbers[0]; - delete[] values[0]; - delete[] modes[0]; - - // release vector of pointers - delete[] jointNumbers; - delete[] values; - delete[] modes; - - // delete other vectors - delete[] subdev_jointsVectorLen; - delete[] subdevices_p; - } -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_MULTIJOINTDATA_H diff --git a/src/devices/controlBoardWrapper/RPCMessagesParser.cpp b/src/devices/controlBoardWrapper/RPCMessagesParser.cpp deleted file mode 100644 index b1b9fc398e2..00000000000 --- a/src/devices/controlBoardWrapper/RPCMessagesParser.cpp +++ /dev/null @@ -1,2556 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "RPCMessagesParser.h" - -#include - -#include "ControlBoardWrapperCommon.h" -#include "ControlBoardLogComponent.h" -#include - -using namespace yarp::os; -using namespace yarp::dev; -using namespace yarp::dev::impl; -using namespace yarp::sig; - - -inline void appendTimeStamp(Bottle& bot, Stamp& st) -{ - int count = st.getCount(); - double time = st.getTime(); - bot.addVocab32(VOCAB_TIMESTAMP); - bot.addInt32(count); - bot.addFloat64(time); -} - -void RPCMessagesParser::handleProtocolVersionRequest(const yarp::os::Bottle& cmd, - yarp::os::Bottle& response, - bool* rec, - bool* ok) -{ - if (cmd.get(0).asVocab32() != VOCAB_GET) { - *rec = false; - *ok = false; - return; - } - - response.addVocab32(VOCAB_PROTOCOL_VERSION); - response.addInt32(PROTOCOL_VERSION_MAJOR); - response.addInt32(PROTOCOL_VERSION_MINOR); - response.addInt32(PROTOCOL_VERSION_TWEAK); - - *rec = true; - *ok = true; -} - -void RPCMessagesParser::handleImpedanceMsg(const yarp::os::Bottle& cmd, - yarp::os::Bottle& response, - bool* rec, - bool* ok) -{ - yCTrace(CONTROLBOARD, "Handling IImpedance message"); - if (!rpc_IImpedance) { - yCError(CONTROLBOARD, "I do not have a valid interface"); - *ok = false; - return; - } - - int code = cmd.get(0).asVocab32(); - *ok = false; - switch (code) { - case VOCAB_SET: { - yCTrace(CONTROLBOARD, "handleImpedanceMsg::VOCAB_SET command"); - switch (cmd.get(2).asVocab32()) { - case VOCAB_IMP_PARAM: { - Bottle* b = cmd.get(4).asList(); - if (b != nullptr) { - double stiff = b->get(0).asFloat64(); - double damp = b->get(1).asFloat64(); - *ok = rpc_IImpedance->setImpedance(cmd.get(3).asInt32(), stiff, damp); - *rec = true; - } - } break; - case VOCAB_IMP_OFFSET: { - Bottle* b = cmd.get(4).asList(); - if (b != nullptr) { - double offs = b->get(0).asFloat64(); - *ok = rpc_IImpedance->setImpedanceOffset(cmd.get(3).asInt32(), offs); - *rec = true; - } - } break; - } - } break; - case VOCAB_GET: { - double stiff = 0; - double damp = 0; - double offs = 0; - yCTrace(CONTROLBOARD, "handleImpedanceMsg::VOCAB_GET command"); - - response.addVocab32(VOCAB_IS); - response.add(cmd.get(1)); - switch (cmd.get(2).asVocab32()) { - case VOCAB_IMP_PARAM: { - *ok = rpc_IImpedance->getImpedance(cmd.get(3).asInt32(), &stiff, &damp); - Bottle& b = response.addList(); - b.addFloat64(stiff); - b.addFloat64(damp); - *rec = true; - } break; - case VOCAB_IMP_OFFSET: { - *ok = rpc_IImpedance->getImpedanceOffset(cmd.get(3).asInt32(), &offs); - Bottle& b = response.addList(); - b.addFloat64(offs); - *rec = true; - } break; - case VOCAB_LIMITS: { - double min_stiff = 0; - double max_stiff = 0; - double min_damp = 0; - double max_damp = 0; - *ok = rpc_IImpedance->getCurrentImpedanceLimit(cmd.get(3).asInt32(), &min_stiff, &max_stiff, &min_damp, &max_damp); - Bottle& b = response.addList(); - b.addFloat64(min_stiff); - b.addFloat64(max_stiff); - b.addFloat64(min_damp); - b.addFloat64(max_damp); - *rec = true; - } break; - } - } - lastRpcStamp.update(); - appendTimeStamp(response, lastRpcStamp); - break; // case VOCAB_GET - default: - { - *rec = false; - } break; - } -} - -void RPCMessagesParser::handleJointFaultMsg(const yarp::os::Bottle& cmd, - yarp::os::Bottle& response, - bool* rec, - bool* ok) -{ - //handle here messages about IControlMode interface - int code = cmd.get(0).asVocab32(); - *ok = true; - *rec = true; //or false - - switch (code) - { - case VOCAB_GET: - yCTrace(CONTROLBOARD, "GET command"); - - int method = cmd.get(2).asVocab32(); - - switch (method) - { - case VOCAB_JF_GET_JOINTFAULT: - yCTrace(CONTROLBOARD, "getJointFault"); - int axis = cmd.get(3).asInt32(); - int faultcode=0; - std::string faultmessage; - if (rpc_IJointFault) - { - *ok = rpc_IJointFault->getLastJointFault(axis, faultcode, faultmessage); - *rec = true; - } - response.addVocab32(VOCAB_IS); - response.addInt32(faultcode); - response.addString(faultmessage); - - yCTrace(CONTROLBOARD, "Returning %d %s", faultcode, faultmessage.c_str()); - *rec = true; - break; - } - - break; - } -} - -void RPCMessagesParser::handleControlModeMsg(const yarp::os::Bottle& cmd, - yarp::os::Bottle& response, - bool* rec, - bool* ok) -{ - yCTrace(CONTROLBOARD, "Handling IControlMode message"); - if (!(rpc_iCtrlMode)) { - yCError(CONTROLBOARD, "I do not have a valid iControlMode interface"); - *ok = false; - return; - } - - //handle here messages about IControlMode interface - int code = cmd.get(0).asVocab32(); - *ok = true; - *rec = true; //or false - - switch (code) { - case VOCAB_SET: { - yCTrace(CONTROLBOARD, "handleControlModeMsg::VOCAB_SET command"); - - int method = cmd.get(2).asVocab32(); - - switch (method) { - case VOCAB_CM_CONTROL_MODE: { - int axis = cmd.get(3).asInt32(); - yCTrace(CONTROLBOARD) << "got VOCAB_CM_CONTROL_MODE"; - if (rpc_iCtrlMode) { - *ok = rpc_iCtrlMode->setControlMode(axis, cmd.get(4).asVocab32()); - } else { - yCError(CONTROLBOARD) << "Unable to handle setControlMode request! This should not happen!"; - *rec = false; - } - } break; - - case VOCAB_CM_CONTROL_MODE_GROUP: { - int n_joints = cmd.get(3).asInt32(); - Bottle& jList = *(cmd.get(4).asList()); - Bottle& modeList = *(cmd.get(5).asList()); - - int* js = new int[n_joints]; - int* modes = new int[n_joints]; - - for (int i = 0; i < n_joints; i++) { - js[i] = jList.get(i).asInt32(); - } - - for (int i = 0; i < n_joints; i++) { - modes[i] = modeList.get(i).asVocab32(); - } - if (rpc_iCtrlMode) { - *ok = rpc_iCtrlMode->setControlModes(n_joints, js, modes); - } else { - *rec = false; - *ok = false; - } - delete[] js; - delete[] modes; - } break; - - case VOCAB_CM_CONTROL_MODES: { - yarp::os::Bottle* modeList; - modeList = cmd.get(3).asList(); - - if (modeList->size() != controlledJoints) { - yCError(CONTROLBOARD, "received an invalid setControlMode message. Size of vector doesn´t match the number of controlled joints"); - *ok = false; - break; - } - int* modes = new int[controlledJoints]; - for (size_t i = 0; i < controlledJoints; i++) { - modes[i] = modeList->get(i).asVocab32(); - } - if (rpc_iCtrlMode) { - *ok = rpc_iCtrlMode->setControlModes(modes); - } else { - *rec = false; - *ok = false; - } - delete[] modes; - } break; - - default: - { - // if I´m here, someone is probably sending command using the old interface. - // try to be compatible as much as I can - - yCError(CONTROLBOARD) << " Error, received a set control mode message using a legacy version, trying to be handle the message anyway " - << " but please update your client to be compatible with the IControlMode interface"; - - yCTrace(CONTROLBOARD) << " cmd.get(4).asVocab32() is " << Vocab32::decode(cmd.get(4).asVocab32()); - int axis = cmd.get(3).asInt32(); - - switch (cmd.get(4).asVocab32()) { - case VOCAB_CM_POSITION: - if (rpc_iCtrlMode) { - *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_POSITION); - } - break; - - case VOCAB_CM_POSITION_DIRECT: - if (rpc_iCtrlMode) { - *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_POSITION_DIRECT); - } else { - *rec = false; - *ok = false; - } - break; - - - case VOCAB_CM_VELOCITY: - if (rpc_iCtrlMode) { - *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_VELOCITY); - } - break; - - case VOCAB_CM_TORQUE: - if (rpc_iCtrlMode) { - *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_TORQUE); - } - break; - - case VOCAB_CM_IMPEDANCE_POS: - yCError(CONTROLBOARD) << "The 'impedancePosition' control mode is deprecated. \nUse setInteractionMode(axis, VOCAB_IM_COMPLIANT) + setControlMode(axis, VOCAB_CM_POSITION) instead"; - break; - - case VOCAB_CM_IMPEDANCE_VEL: - yCError(CONTROLBOARD) << "The 'impedanceVelocity' control mode is deprecated. \nUse setInteractionMode(axis, VOCAB_IM_COMPLIANT) + setControlMode(axis, VOCAB_CM_VELOCITY) instead"; - break; - - case VOCAB_CM_PWM: - if (rpc_iCtrlMode) { - *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_PWM); - } else { - *rec = false; - *ok = false; - } - break; - - case VOCAB_CM_CURRENT: - if (rpc_iCtrlMode) { - *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_CURRENT); - } else { - *rec = false; - *ok = false; - } - break; - - case VOCAB_CM_MIXED: - if (rpc_iCtrlMode) { - *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_MIXED); - } else { - *rec = false; - *ok = false; - } - break; - - case VOCAB_CM_FORCE_IDLE: - if (rpc_iCtrlMode) { - *ok = rpc_iCtrlMode->setControlMode(axis, VOCAB_CM_FORCE_IDLE); - } else { - *rec = false; - *ok = false; - } - break; - - default: - yCError(CONTROLBOARD, "SET unknown controlMode : %s ", cmd.toString().c_str()); - *ok = false; - *rec = false; - break; - } - } break; // close default case - } - } break; // close SET case - - case VOCAB_GET: { - yCTrace(CONTROLBOARD, "GET command"); - - int method = cmd.get(2).asVocab32(); - - switch (method) { - - case VOCAB_CM_CONTROL_MODES: { - yCTrace(CONTROLBOARD, "getControlModes"); - int* p = new int[controlledJoints]; - for (size_t i = 0; i < controlledJoints; ++i) { - p[i] = -1; - } - if (rpc_iCtrlMode) { - *ok = rpc_iCtrlMode->getControlModes(p); - } - - response.addVocab32(VOCAB_IS); - response.addVocab32(VOCAB_CM_CONTROL_MODES); - - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addVocab32(p[i]); - } - delete[] p; - - *rec = true; - } break; - - case VOCAB_CM_CONTROL_MODE: { - yCTrace(CONTROLBOARD, "getControlMode"); - - int p = -1; - int axis = cmd.get(3).asInt32(); - if (rpc_iCtrlMode) { - *ok = rpc_iCtrlMode->getControlMode(axis, &p); - } - - response.addVocab32(VOCAB_IS); - response.addInt32(axis); - response.addVocab32(p); - - yCTrace(CONTROLBOARD, "Returning %d", p); - *rec = true; - } break; - - case VOCAB_CM_CONTROL_MODE_GROUP: { - yCTrace(CONTROLBOARD, "getControlMode group"); - - int n_joints = cmd.get(3).asInt32(); - Bottle& lIn = *(cmd.get(4).asList()); - - int* js = new int[n_joints]; - int* modes = new int[n_joints]; - for (int i = 0; i < n_joints; i++) { - js[i] = lIn.get(i).asInt32(); - modes[i] = -1; - } - if (rpc_iCtrlMode) { - *ok = rpc_iCtrlMode->getControlModes(n_joints, js, modes); - } else { - *rec = false; - *ok = false; - } - - response.addVocab32(VOCAB_IS); - response.addVocab32(VOCAB_CM_CONTROL_MODE_GROUP); - Bottle& b = response.addList(); - for (int i = 0; i < n_joints; i++) { - b.addVocab32(modes[i]); - } - - delete[] js; - delete[] modes; - - *rec = true; - } break; - - default: - yCError(CONTROLBOARD, "received a GET ICONTROLMODE command not understood"); - break; - } - } - - lastRpcStamp.update(); - appendTimeStamp(response, lastRpcStamp); - break; // case VOCAB_GET - - default: - { - *rec = false; - } break; - } -} - - -void RPCMessagesParser::handleTorqueMsg(const yarp::os::Bottle& cmd, - yarp::os::Bottle& response, - bool* rec, - bool* ok) -{ - yCTrace(CONTROLBOARD, "Handling ITorqueControl message"); - - if (!rpc_ITorque) { - yCError(CONTROLBOARD, "Error, I do not have a valid ITorque interface"); - *ok = false; - return; - } - - int code = cmd.get(0).asVocab32(); - switch (code) { - case VOCAB_SET: { - *rec = true; - yCTrace(CONTROLBOARD, "set command received"); - - switch (cmd.get(2).asVocab32()) { - case VOCAB_REF: { - *ok = rpc_ITorque->setRefTorque(cmd.get(3).asInt32(), cmd.get(4).asFloat64()); - } break; - - case VOCAB_MOTOR_PARAMS: { - yarp::dev::MotorTorqueParameters params; - int joint = cmd.get(3).asInt32(); - Bottle* b = cmd.get(4).asList(); - - if (b == nullptr) { - break; - } - - if (b->size() != 8) { - yCError(CONTROLBOARD, "received a SET VOCAB_MOTOR_PARAMS command not understood, size != 8"); - break; - } - - params.bemf = b->get(0).asFloat64(); - params.bemf_scale = b->get(1).asFloat64(); - params.ktau = b->get(2).asFloat64(); - params.ktau_scale = b->get(3).asFloat64(); - params.viscousPos = b->get(4).asFloat64(); - params.viscousNeg = b->get(5).asFloat64(); - params.coulombPos = b->get(6).asFloat64(); - params.coulombNeg = b->get(7).asFloat64(); - - *ok = rpc_ITorque->setMotorTorqueParams(joint, params); - } break; - - case VOCAB_REFS: { - Bottle* b = cmd.get(3).asList(); - if (b == nullptr) { - break; - } - - const size_t njs = b->size(); - if (njs == controlledJoints) { - auto* p = new double[njs]; // LATER: optimize to avoid allocation. - for (size_t i = 0; i < njs; i++) { - p[i] = b->get(i).asFloat64(); - } - *ok = rpc_ITorque->setRefTorques(p); - delete[] p; - } - } break; - - case VOCAB_TORQUE_MODE: { - if (rpc_iCtrlMode) { - int* modes = new int[controlledJoints]; - for (size_t i = 0; i < controlledJoints; i++) { - modes[i] = VOCAB_CM_TORQUE; - } - *ok = rpc_iCtrlMode->setControlModes(modes); - delete[] modes; - } else { - *ok = false; - } - } break; - } - } break; - - case VOCAB_GET: { - *rec = true; - yCTrace(CONTROLBOARD, "get command received"); - double dtmp = 0.0; - double dtmp2 = 0.0; - response.addVocab32(VOCAB_IS); - response.add(cmd.get(1)); - - switch (cmd.get(2).asVocab32()) { - case VOCAB_AXES: { - int tmp; - *ok = rpc_ITorque->getAxes(&tmp); - response.addInt32(tmp); - } break; - - case VOCAB_TRQ: { - *ok = rpc_ITorque->getTorque(cmd.get(3).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_MOTOR_PARAMS: { - yarp::dev::MotorTorqueParameters params; - int joint = cmd.get(3).asInt32(); - - // get the data - *ok = rpc_ITorque->getMotorTorqueParams(joint, ¶ms); - - // convert it back to yarp message - Bottle& b = response.addList(); - - b.addFloat64(params.bemf); - b.addFloat64(params.bemf_scale); - b.addFloat64(params.ktau); - b.addFloat64(params.ktau_scale); - b.addFloat64(params.viscousPos); - b.addFloat64(params.viscousNeg); - b.addFloat64(params.coulombPos); - b.addFloat64(params.coulombNeg); - } break; - case VOCAB_RANGE: { - *ok = rpc_ITorque->getTorqueRange(cmd.get(3).asInt32(), &dtmp, &dtmp2); - response.addFloat64(dtmp); - response.addFloat64(dtmp2); - } break; - - case VOCAB_TRQS: { - auto* p = new double[controlledJoints]; - *ok = rpc_ITorque->getTorques(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_RANGES: { - auto* p1 = new double[controlledJoints]; - auto* p2 = new double[controlledJoints]; - *ok = rpc_ITorque->getTorqueRanges(p1, p2); - Bottle& b1 = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b1.addFloat64(p1[i]); - } - Bottle& b2 = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b2.addFloat64(p2[i]); - } - delete[] p1; - delete[] p2; - } break; - - case VOCAB_REFERENCE: { - *ok = rpc_ITorque->getRefTorque(cmd.get(3).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_REFERENCES: { - auto* p = new double[controlledJoints]; - *ok = rpc_ITorque->getRefTorques(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - } - } - lastRpcStamp.update(); - appendTimeStamp(response, lastRpcStamp); - break; // case VOCAB_GET - } -} - -void RPCMessagesParser::handleInteractionModeMsg(const yarp::os::Bottle& cmd, - yarp::os::Bottle& response, - bool* rec, - bool* ok) -{ - yCTrace(CONTROLBOARD, "\nHandling IInteractionMode message"); - if (!rpc_IInteract) { - yCError(CONTROLBOARD, "Error I do not have a valid IInteractionMode interface"); - *ok = false; - return; - } - - yCTrace(CONTROLBOARD) << "received command: " << cmd.toString(); - - int action = cmd.get(0).asVocab32(); - - switch (action) { - case VOCAB_SET: { - switch (cmd.get(2).asVocab32()) { - yarp::os::Bottle* jointList; - yarp::os::Bottle* modeList; - yarp::dev::InteractionModeEnum* modes; - - case VOCAB_INTERACTION_MODE: { - *ok = rpc_IInteract->setInteractionMode(cmd.get(3).asInt32(), static_cast(cmd.get(4).asVocab32())); - } break; - - case VOCAB_INTERACTION_MODE_GROUP: { - yCTrace(CONTROLBOARD) << "CBW.h set interactionMode GROUP"; - - auto n_joints = static_cast(cmd.get(3).asInt32()); - jointList = cmd.get(4).asList(); - modeList = cmd.get(5).asList(); - if ((jointList->size() != n_joints) || (modeList->size() != n_joints)) { - yCWarning(CONTROLBOARD, "Received an invalid setInteractionMode message. Size of vectors doesn´t match"); - *ok = false; - break; - } - int* joints = new int[n_joints]; - modes = new yarp::dev::InteractionModeEnum[n_joints]; - for (size_t i = 0; i < n_joints; i++) { - joints[i] = jointList->get(i).asInt32(); - modes[i] = static_cast(modeList->get(i).asVocab32()); - yCTrace(CONTROLBOARD) << "CBW.cpp received vocab " << yarp::os::Vocab32::decode(modes[i]); - } - *ok = rpc_IInteract->setInteractionModes(n_joints, joints, modes); - delete[] joints; - delete[] modes; - - } break; - - case VOCAB_INTERACTION_MODES: { - yCTrace(CONTROLBOARD) << "CBW.c set interactionMode ALL"; - - modeList = cmd.get(3).asList(); - if (modeList->size() != controlledJoints) { - yCWarning(CONTROLBOARD, "Received an invalid setInteractionMode message. Size of vector doesn´t match the number of controlled joints"); - *ok = false; - break; - } - modes = new yarp::dev::InteractionModeEnum[controlledJoints]; - for (size_t i = 0; i < controlledJoints; i++) { - modes[i] = static_cast(modeList->get(i).asVocab32()); - } - *ok = rpc_IInteract->setInteractionModes(modes); - delete[] modes; - } break; - - default: - { - yCWarning(CONTROLBOARD, "Error while Handling IInteractionMode message, SET command not understood %s", cmd.get(2).asString().c_str()); - *ok = false; - } break; - } - *rec = true; //or false - } break; - - case VOCAB_GET: { - yarp::os::Bottle* jointList; - - switch (cmd.get(2).asVocab32()) { - case VOCAB_INTERACTION_MODE: { - yarp::dev::InteractionModeEnum mode; - *ok = rpc_IInteract->getInteractionMode(cmd.get(3).asInt32(), &mode); - response.addVocab32(mode); - yCTrace(CONTROLBOARD) << " resp is " << response.toString(); - } break; - - case VOCAB_INTERACTION_MODE_GROUP: { - yarp::dev::InteractionModeEnum* modes; - - int n_joints = cmd.get(3).asInt32(); - jointList = cmd.get(4).asList(); - if (jointList->size() != static_cast(n_joints)) { - yCError(CONTROLBOARD, "Received an invalid getInteractionMode message. Size of vectors doesn´t match"); - *ok = false; - break; - } - int* joints = new int[n_joints]; - modes = new yarp::dev::InteractionModeEnum[n_joints]; - for (int i = 0; i < n_joints; i++) { - joints[i] = jointList->get(i).asInt32(); - } - *ok = rpc_IInteract->getInteractionModes(n_joints, joints, modes); - - Bottle& c = response.addList(); - for (int i = 0; i < n_joints; i++) { - c.addVocab32(modes[i]); - } - - yCTrace(CONTROLBOARD, "got response bottle: %s", response.toString().c_str()); - - delete[] joints; - delete[] modes; - } break; - - case VOCAB_INTERACTION_MODES: { - yarp::dev::InteractionModeEnum* modes; - modes = new yarp::dev::InteractionModeEnum[controlledJoints]; - - *ok = rpc_IInteract->getInteractionModes(modes); - - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addVocab32(modes[i]); - } - - yCTrace(CONTROLBOARD, "got response bottle: %s", response.toString().c_str()); - - delete[] modes; - } break; - } - lastRpcStamp.update(); - appendTimeStamp(response, lastRpcStamp); - } break; // case VOCAB_GET - - default: - yCError(CONTROLBOARD, "Error while Handling IInteractionMode message, command was not SET nor GET"); - *ok = false; - break; - } -} - -void RPCMessagesParser::handleCurrentMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool* rec, bool* ok) -{ - yCTrace(CONTROLBOARD, "Handling ICurrentControl message"); - - if (!rpc_ICurrent) { - yCError(CONTROLBOARD, "I do not have a valid ICurrentControl interface"); - *ok = false; - return; - } - - int code = cmd.get(0).asVocab32(); - int action = cmd.get(2).asVocab32(); - - *ok = false; - *rec = true; - switch (code) { - case VOCAB_SET: { - switch (action) { - case VOCAB_CURRENT_REF: { - yCError(CONTROLBOARD, "VOCAB_CURRENT_REF methods is implemented as streaming"); - *ok = false; - } break; - - case VOCAB_CURRENT_REFS: { - yCError(CONTROLBOARD, "VOCAB_CURRENT_REFS methods is implemented as streaming"); - *ok = false; - } break; - - case VOCAB_CURRENT_REF_GROUP: { - yCError(CONTROLBOARD, "VOCAB_CURRENT_REF_GROUP methods is implemented as streaming"); - *ok = false; - } break; - - default: - { - yCError(CONTROLBOARD) << "Unknown handleCurrentMsg message received"; - *rec = false; - *ok = false; - } break; - } - } break; - - case VOCAB_GET: { - *rec = true; - yCTrace(CONTROLBOARD, "get command received"); - double dtmp = 0.0; - double dtmp2 = 0.0; - response.addVocab32(VOCAB_IS); - response.add(cmd.get(1)); - - switch (action) { - case VOCAB_CURRENT_REF: { - *ok = rpc_ICurrent->getRefCurrent(cmd.get(3).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_CURRENT_REFS: { - auto* p = new double[controlledJoints]; - *ok = rpc_ICurrent->getRefCurrents(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_CURRENT_RANGE: { - - *ok = rpc_ICurrent->getCurrentRange(cmd.get(3).asInt32(), &dtmp, &dtmp2); - response.addFloat64(dtmp); - response.addFloat64(dtmp2); - } break; - - case VOCAB_CURRENT_RANGES: { - auto* p1 = new double[controlledJoints]; - auto* p2 = new double[controlledJoints]; - *ok = rpc_ICurrent->getCurrentRanges(p1, p2); - Bottle& b1 = response.addList(); - Bottle& b2 = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b1.addFloat64(p1[i]); - } - for (size_t i = 0; i < controlledJoints; i++) { - b2.addFloat64(p2[i]); - } - delete[] p1; - delete[] p2; - } break; - - default: - { - yCError(CONTROLBOARD) << "Unknown handleCurrentMsg message received"; - *rec = false; - *ok = false; - } break; - } - } break; - - default: - { - yCError(CONTROLBOARD) << "Unknown handleCurrentMsg message received"; - *rec = false; - *ok = false; - } break; - } -} - -void RPCMessagesParser::handlePidMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool* rec, bool* ok) -{ - yCTrace(CONTROLBOARD, "Handling IPidControl message"); - - if (!rpc_IPid) { - yCError(CONTROLBOARD, "I do not have a valid IPidControl interface"); - *ok = false; - return; - } - - int code = cmd.get(0).asVocab32(); - int action = cmd.get(2).asVocab32(); - auto pidtype = static_cast(cmd.get(3).asVocab32()); - - *ok = false; - *rec = true; - switch (code) { - case VOCAB_SET: { - *rec = true; - yCTrace(CONTROLBOARD, "set command received"); - - switch (action) { - case VOCAB_OFFSET: { - double v; - int j = cmd.get(4).asInt32(); - v = cmd.get(5).asFloat64(); - *ok = rpc_IPid->setPidOffset(pidtype, j, v); - } break; - - case VOCAB_PID: { - Pid p; - int j = cmd.get(4).asInt32(); - Bottle* b = cmd.get(5).asList(); - - if (b == nullptr) { - break; - } - - p.kp = b->get(0).asFloat64(); - p.kd = b->get(1).asFloat64(); - p.ki = b->get(2).asFloat64(); - p.max_int = b->get(3).asFloat64(); - p.max_output = b->get(4).asFloat64(); - p.offset = b->get(5).asFloat64(); - p.scale = b->get(6).asFloat64(); - p.stiction_up_val = b->get(7).asFloat64(); - p.stiction_down_val = b->get(8).asFloat64(); - p.kff = b->get(9).asFloat64(); - *ok = rpc_IPid->setPid(pidtype, j, p); - } break; - - case VOCAB_PIDS: { - Bottle* b = cmd.get(4).asList(); - - if (b == nullptr) { - break; - } - - const size_t njs = b->size(); - if (njs == controlledJoints) { - Pid* p = new Pid[njs]; - - bool allOK = true; - - for (size_t i = 0; i < njs; i++) { - Bottle* c = b->get(i).asList(); - if (c != nullptr) { - p[i].kp = c->get(0).asFloat64(); - p[i].kd = c->get(1).asFloat64(); - p[i].ki = c->get(2).asFloat64(); - p[i].max_int = c->get(3).asFloat64(); - p[i].max_output = c->get(4).asFloat64(); - p[i].offset = c->get(5).asFloat64(); - p[i].scale = c->get(6).asFloat64(); - p[i].stiction_up_val = c->get(7).asFloat64(); - p[i].stiction_down_val = c->get(8).asFloat64(); - p[i].kff = c->get(9).asFloat64(); - } else { - allOK = false; - } - } - if (allOK) { - *ok = rpc_IPid->setPids(pidtype, p); - } else { - *ok = false; - } - - delete[] p; - } - } break; - - case VOCAB_REF: { - *ok = rpc_IPid->setPidReference(pidtype, cmd.get(4).asInt32(), cmd.get(5).asFloat64()); - } break; - - case VOCAB_REFS: { - Bottle* b = cmd.get(4).asList(); - - if (b == nullptr) { - break; - } - - const size_t njs = b->size(); - if (njs == controlledJoints) { - auto* p = new double[njs]; // LATER: optimize to avoid allocation. - for (size_t i = 0; i < njs; i++) { - p[i] = b->get(i).asFloat64(); - } - *ok = rpc_IPid->setPidReferences(pidtype, p); - delete[] p; - } - } break; - - case VOCAB_LIM: { - *ok = rpc_IPid->setPidErrorLimit(pidtype, cmd.get(4).asInt32(), cmd.get(5).asFloat64()); - } break; - - case VOCAB_LIMS: { - Bottle* b = cmd.get(4).asList(); - - if (b == nullptr) { - break; - } - - const size_t njs = b->size(); - if (njs == controlledJoints) { - auto* p = new double[njs]; // LATER: optimize to avoid allocation. - for (size_t i = 0; i < njs; i++) { - p[i] = b->get(i).asFloat64(); - } - *ok = rpc_IPid->setPidErrorLimits(pidtype, p); - delete[] p; - } - } break; - - case VOCAB_RESET: { - *ok = rpc_IPid->resetPid(pidtype, cmd.get(4).asInt32()); - } break; - - case VOCAB_DISABLE: { - *ok = rpc_IPid->disablePid(pidtype, cmd.get(4).asInt32()); - } break; - - case VOCAB_ENABLE: { - *ok = rpc_IPid->enablePid(pidtype, cmd.get(4).asInt32()); - } break; - } - } break; - - case VOCAB_GET: { - *rec = true; - yCTrace(CONTROLBOARD, "get command received"); - double dtmp = 0.0; - response.addVocab32(VOCAB_IS); - response.add(cmd.get(1)); - - switch (action) { - case VOCAB_LIMS: { - auto* p = new double[controlledJoints]; - *ok = rpc_IPid->getPidErrorLimits(pidtype, p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_ENABLE: { - bool booltmp = false; - *ok = rpc_IPid->isPidEnabled(pidtype, cmd.get(4).asInt32(), &booltmp); - response.addInt32(booltmp); - } break; - - case VOCAB_ERR: { - *ok = rpc_IPid->getPidError(pidtype, cmd.get(4).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_ERRS: { - auto* p = new double[controlledJoints]; - *ok = rpc_IPid->getPidErrors(pidtype, p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_OUTPUT: { - *ok = rpc_IPid->getPidOutput(pidtype, cmd.get(4).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_OUTPUTS: { - auto* p = new double[controlledJoints]; - *ok = rpc_IPid->getPidOutputs(pidtype, p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_PID: { - Pid p; - *ok = rpc_IPid->getPid(pidtype, cmd.get(4).asInt32(), &p); - Bottle& b = response.addList(); - b.addFloat64(p.kp); - b.addFloat64(p.kd); - b.addFloat64(p.ki); - b.addFloat64(p.max_int); - b.addFloat64(p.max_output); - b.addFloat64(p.offset); - b.addFloat64(p.scale); - b.addFloat64(p.stiction_up_val); - b.addFloat64(p.stiction_down_val); - b.addFloat64(p.kff); - } break; - - case VOCAB_PIDS: { - Pid* p = new Pid[controlledJoints]; - *ok = rpc_IPid->getPids(pidtype, p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - Bottle& c = b.addList(); - c.addFloat64(p[i].kp); - c.addFloat64(p[i].kd); - c.addFloat64(p[i].ki); - c.addFloat64(p[i].max_int); - c.addFloat64(p[i].max_output); - c.addFloat64(p[i].offset); - c.addFloat64(p[i].scale); - c.addFloat64(p[i].stiction_up_val); - c.addFloat64(p[i].stiction_down_val); - c.addFloat64(p[i].kff); - } - delete[] p; - } break; - - case VOCAB_REFERENCE: { - *ok = rpc_IPid->getPidReference(pidtype, cmd.get(4).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_REFERENCES: { - auto* p = new double[controlledJoints]; - *ok = rpc_IPid->getPidReferences(pidtype, p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_LIM: { - *ok = rpc_IPid->getPidErrorLimit(pidtype, cmd.get(4).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - } - } break; - - default: - { - yCError(CONTROLBOARD) << "Unknown handlePWMMsg message received"; - *rec = false; - *ok = false; - } break; - } -} - -void RPCMessagesParser::handlePWMMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool* rec, bool* ok) -{ - yCTrace(CONTROLBOARD, "Handling IPWMControl message"); - - if (!rpc_IPWM) { - yCError(CONTROLBOARD, "I do not have a valid IPWMControl interface"); - *ok = false; - return; - } - - int code = cmd.get(0).asVocab32(); - int action = cmd.get(2).asVocab32(); - - *ok = false; - *rec = true; - switch (code) { - case VOCAB_SET: { - *rec = true; - yCTrace(CONTROLBOARD, "set command received"); - - switch (action) { - case VOCAB_PWMCONTROL_REF_PWM: { - //handled as streaming! - yCError(CONTROLBOARD) << "VOCAB_PWMCONTROL_REF_PWM handled as straming"; - *ok = false; - } break; - - default: - { - yCError(CONTROLBOARD) << "Unknown handlePWMMsg message received"; - *ok = false; - } break; - } - } break; - - case VOCAB_GET: { - yCTrace(CONTROLBOARD, "get command received"); - *rec = true; - double dtmp = 0.0; - response.addVocab32(VOCAB_IS); - response.add(cmd.get(1)); - - switch (action) { - case VOCAB_PWMCONTROL_REF_PWM: { - *ok = rpc_IPWM->getRefDutyCycle(cmd.get(3).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_PWMCONTROL_REF_PWMS: { - auto* p = new double[controlledJoints]; - *ok = rpc_IPWM->getRefDutyCycles(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_PWMCONTROL_PWM_OUTPUT: { - *ok = rpc_IPWM->getDutyCycle(cmd.get(3).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_PWMCONTROL_PWM_OUTPUTS: { - auto* p = new double[controlledJoints]; - *ok = rpc_IPWM->getRefDutyCycles(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - default: - { - yCError(CONTROLBOARD) << "Unknown handlePWMMsg message received"; - *ok = false; - } break; - } - } break; - - default: - { - yCError(CONTROLBOARD) << "Unknown handlePWMMsg message received"; - *rec = false; - *ok = false; - } break; - } -} - -void RPCMessagesParser::handleRemoteVariablesMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool* rec, bool* ok) -{ - yCTrace(CONTROLBOARD, "Handling IRemoteVariables message"); - - if (!rpc_IVar) { - yCError(CONTROLBOARD, "controlBoardWrapper: I do not have a valid IRemoteVariables interface"); - *ok = false; - return; - } - - int code = cmd.get(0).asVocab32(); - int action = cmd.get(2).asVocab32(); - - *ok = false; - *rec = true; - switch (code) { - case VOCAB_SET: { - switch (action) { - case VOCAB_VARIABLE: { - Bottle btail = cmd.tail().tail().tail().tail(); // remove the first four elements - std::string s = btail.toString(); - *ok = rpc_IVar->setRemoteVariable(cmd.get(3).asString(), btail); - } break; - - default: - { - *rec = false; - *ok = false; - } break; - } - } break; - - case VOCAB_GET: { - yCTrace(CONTROLBOARD, "get command received"); - - response.clear(); - response.addVocab32(VOCAB_IS); - response.add(cmd.get(1)); - Bottle btmp; - - switch (action) { - case VOCAB_VARIABLE: { - *ok = rpc_IVar->getRemoteVariable(cmd.get(3).asString(), btmp); - Bottle& b = response.addList(); - b = btmp; - } break; - - case VOCAB_LIST_VARIABLES: { - *ok = rpc_IVar->getRemoteVariablesList(&btmp); - Bottle& b = response.addList(); - b = btmp; - } break; - } - } - } //end get/set switch -} - -void RPCMessagesParser::handleRemoteCalibratorMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool* rec, bool* ok) -{ - yCTrace(CONTROLBOARD, "Handling IRemoteCalibrator message"); - - if (!rpc_IRemoteCalibrator) { - yCError(CONTROLBOARD, "I do not have a valid IRemoteCalibrator interface"); - *ok = false; - return; - } - - int code = cmd.get(0).asVocab32(); - int action = cmd.get(2).asVocab32(); - - *ok = false; - *rec = true; - switch (code) { - case VOCAB_SET: { - switch (action) { - case VOCAB_CALIBRATE_SINGLE_JOINT: { - yCDebug(CONTROLBOARD) << "cmd is " << cmd.toString() << " joint is " << cmd.get(3).asInt32(); - yCTrace(CONTROLBOARD, "Calling calibrate joint with no parameter"); - *ok = rpc_IRemoteCalibrator->calibrateSingleJoint(cmd.get(3).asInt32()); - } break; - - case VOCAB_CALIBRATE_WHOLE_PART: { - yCTrace(CONTROLBOARD, "Calling calibrate whole part"); - *ok = rpc_IRemoteCalibrator->calibrateWholePart(); - } break; - - case VOCAB_HOMING_SINGLE_JOINT: { - yCTrace(CONTROLBOARD, "Calling calibrate joint with no parameter"); - *ok = rpc_IRemoteCalibrator->homingSingleJoint(cmd.get(3).asInt32()); - } break; - - case VOCAB_HOMING_WHOLE_PART: { - yCDebug(CONTROLBOARD) << "Received homing whole part"; - yCTrace(CONTROLBOARD, "Calling calibrate whole part"); - *ok = rpc_IRemoteCalibrator->homingWholePart(); - } break; - - case VOCAB_PARK_SINGLE_JOINT: { - yCTrace(CONTROLBOARD, "Calling calibrate joint with no parameter"); - *ok = rpc_IRemoteCalibrator->parkSingleJoint(cmd.get(3).asInt32()); - } break; - - case VOCAB_PARK_WHOLE_PART: { - yCTrace(CONTROLBOARD, "Calling calibrate whole part"); - *ok = rpc_IRemoteCalibrator->parkWholePart(); - } break; - - case VOCAB_QUIT_CALIBRATE: { - yCTrace(CONTROLBOARD, "Calling quit calibrate"); - *ok = rpc_IRemoteCalibrator->quitCalibrate(); - } break; - - case VOCAB_QUIT_PARK: { - yCTrace(CONTROLBOARD, "Calling quit park"); - *ok = rpc_IRemoteCalibrator->quitPark(); - } break; - - default: - { - *rec = false; - *ok = false; - } break; - } - } break; - - case VOCAB_GET: { - response.clear(); - response.addVocab32(VOCAB_IS); - response.add(cmd.get(1)); - - switch (action) { - case VOCAB_IS_CALIBRATOR_PRESENT: { - bool tmp; - yCTrace(CONTROLBOARD, "Calling VOCAB_IS_CALIBRATOR_PRESENT"); - *ok = rpc_IRemoteCalibrator->isCalibratorDevicePresent(&tmp); - response.addInt32(tmp); - } break; - } - } - } //end get/set switch -} - - -// rpc callback -bool RPCMessagesParser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& response) -{ - bool ok = false; - bool rec = false; // Tells if the command is recognized! - - yCTrace(CONTROLBOARD, "command received: %s", cmd.toString().c_str()); - - int code = cmd.get(0).asVocab32(); - - if (cmd.size() < 2) { - ok = false; - } else { - switch (cmd.get(1).asVocab32()) { - case VOCAB_PID: - handlePidMsg(cmd, response, &rec, &ok); - break; - - case VOCAB_TORQUE: - handleTorqueMsg(cmd, response, &rec, &ok); - break; - - case VOCAB_IJOINTFAULT: - handleJointFaultMsg(cmd, response, &rec, &ok); - break; - - case VOCAB_ICONTROLMODE: - handleControlModeMsg(cmd, response, &rec, &ok); - break; - - case VOCAB_IMPEDANCE: - handleImpedanceMsg(cmd, response, &rec, &ok); - break; - - case VOCAB_INTERFACE_INTERACTION_MODE: - handleInteractionModeMsg(cmd, response, &rec, &ok); - break; - - case VOCAB_PROTOCOL_VERSION: - handleProtocolVersionRequest(cmd, response, &rec, &ok); - break; - - case VOCAB_REMOTE_CALIBRATOR_INTERFACE: - handleRemoteCalibratorMsg(cmd, response, &rec, &ok); - break; - - case VOCAB_REMOTE_VARIABILE_INTERFACE: - handleRemoteVariablesMsg(cmd, response, &rec, &ok); - break; - - case VOCAB_CURRENTCONTROL_INTERFACE: - handleCurrentMsg(cmd, response, &rec, &ok); - break; - - case VOCAB_PWMCONTROL_INTERFACE: - handlePWMMsg(cmd, response, &rec, &ok); - break; - - default: - // fallback for old interfaces with no specific name - switch (code) { - case VOCAB_CALIBRATE_JOINT: { - if (!rpc_Icalib) { ok = false; break; } - rec = true; - yCTrace(CONTROLBOARD, "Calling calibrate joint"); - - int j = cmd.get(1).asInt32(); - int ui = cmd.get(2).asInt32(); - double v1 = cmd.get(3).asFloat64(); - double v2 = cmd.get(4).asFloat64(); - double v3 = cmd.get(5).asFloat64(); - if (rpc_Icalib == nullptr) { - yCError(CONTROLBOARD, "Sorry I don't have a IControlCalibration2 interface"); - } else { - ok = rpc_Icalib->calibrateAxisWithParams(j, ui, v1, v2, v3); - } - } break; - - case VOCAB_CALIBRATE_JOINT_PARAMS: { - if (!rpc_Icalib) { ok = false; break; } - rec = true; - yCTrace(CONTROLBOARD, "Calling calibrate joint"); - - int j = cmd.get(1).asInt32(); - CalibrationParameters params; - params.type = cmd.get(2).asInt32(); - params.param1 = cmd.get(3).asFloat64(); - params.param2 = cmd.get(4).asFloat64(); - params.param3 = cmd.get(5).asFloat64(); - params.param4 = cmd.get(6).asFloat64(); - if (rpc_Icalib == nullptr) { - yCError(CONTROLBOARD, "Sorry I don't have a IControlCalibration2 interface"); - } else { - ok = rpc_Icalib->setCalibrationParameters(j, params); - } - } break; - - case VOCAB_CALIBRATE: { - if (!rpc_Icalib) { ok = false; break; } - rec = true; - yCTrace(CONTROLBOARD, "Calling calibrate"); - ok = rpc_Icalib->calibrateRobot(); - } break; - - case VOCAB_CALIBRATE_DONE: { - if (!rpc_Icalib) { ok = false; break; } - rec = true; - yCTrace(CONTROLBOARD, "Calling calibrate done"); - int j = cmd.get(1).asInt32(); - ok = rpc_Icalib->calibrationDone(j); - } break; - - case VOCAB_PARK: { - if (!rpc_Icalib) { ok = false; break; } - rec = true; - yCTrace(CONTROLBOARD, "Calling park function"); - int flag = cmd.get(1).asInt32(); - ok = rpc_Icalib->park(flag ? true : false); - ok = true; //client would get stuck if returning false - } break; - - case VOCAB_SET: { - rec = true; - yCTrace(CONTROLBOARD, "set command received"); - - switch (cmd.get(1).asVocab32()) { - case VOCAB_POSITION_MOVE: { - if (!rpc_IPosCtrl) { ok = false; break; } - ok = rpc_IPosCtrl->positionMove(cmd.get(2).asInt32(), cmd.get(3).asFloat64()); - } break; - - // this operation is also available on "command" port - case VOCAB_POSITION_MOVES: { - if (!rpc_IPosCtrl) { ok = false; break; } - Bottle* b = cmd.get(2).asList(); - if (b == nullptr) { - break; - } - const size_t njs = b->size(); - if (njs != controlledJoints) { - break; - } - tmpVect.resize(njs); - for (size_t i = 0; i < njs; i++) { - tmpVect[i] = b->get(i).asFloat64(); - } - - if (rpc_IPosCtrl != nullptr) { - ok = rpc_IPosCtrl->positionMove(&tmpVect[0]); - } - } break; - - case VOCAB_POSITION_MOVE_GROUP: { - if (!rpc_IPosCtrl) { ok = false; break; } - auto len = static_cast(cmd.get(2).asInt32()); - Bottle* jlut = cmd.get(3).asList(); - Bottle* pos_val = cmd.get(4).asList(); - - if (rpc_IPosCtrl == nullptr) { - break; - } - - if (jlut == nullptr || pos_val == nullptr) { - break; - } - if (len != jlut->size() || len != pos_val->size()) { - break; - } - - auto* j_tmp = new int[len]; - auto* pos_tmp = new double[len]; - for (size_t i = 0; i < len; i++) { - j_tmp[i] = jlut->get(i).asInt32(); - pos_tmp[i] = pos_val->get(i).asFloat64(); - } - - ok = rpc_IPosCtrl->positionMove(len, j_tmp, pos_tmp); - - delete[] j_tmp; - delete[] pos_tmp; - } break; - - // this operation is also available on "command" port - case VOCAB_VELOCITY_MOVES: { - if (!rpc_IVelCtrl) { ok = false; break; } - Bottle* b = cmd.get(2).asList(); - if (b == nullptr) { - break; - } - const size_t njs = b->size(); - if (njs != controlledJoints) { - break; - } - tmpVect.resize(njs); - for (size_t i = 0; i < njs; i++) { - tmpVect[i] = b->get(i).asFloat64(); - } - if (rpc_IVelCtrl != nullptr) { - ok = rpc_IVelCtrl->velocityMove(&tmpVect[0]); - } - - } break; - - case VOCAB_RELATIVE_MOVE: { - if (!rpc_IPosCtrl) { ok = false; break; } - ok = rpc_IPosCtrl->relativeMove(cmd.get(2).asInt32(), cmd.get(3).asFloat64()); - } break; - - case VOCAB_RELATIVE_MOVE_GROUP: { - if (!rpc_IPosCtrl) { ok = false; break; } - auto len = static_cast(cmd.get(2).asInt32()); - Bottle* jBottle_p = cmd.get(3).asList(); - Bottle* posBottle_p = cmd.get(4).asList(); - - if (rpc_IPosCtrl == nullptr) { - break; - } - - if (jBottle_p == nullptr || posBottle_p == nullptr) { - break; - } - if (len != jBottle_p->size() || len != posBottle_p->size()) { - break; - } - - int* j_tmp = new int[len]; - auto* pos_tmp = new double[len]; - - for (size_t i = 0; i < len; i++) { - j_tmp[i] = jBottle_p->get(i).asInt32(); - } - - for (size_t i = 0; i < len; i++) { - pos_tmp[i] = posBottle_p->get(i).asFloat64(); - } - - ok = rpc_IPosCtrl->relativeMove(len, j_tmp, pos_tmp); - - delete[] j_tmp; - delete[] pos_tmp; - } break; - - case VOCAB_RELATIVE_MOVES: { - if (!rpc_IPosCtrl) { ok = false; break; } - Bottle* b = cmd.get(2).asList(); - - if (b == nullptr) { - break; - } - - const size_t njs = b->size(); - if (njs != controlledJoints) { - break; - } - auto* p = new double[njs]; // LATER: optimize to avoid allocation. - for (size_t i = 0; i < njs; i++) { - p[i] = b->get(i).asFloat64(); - } - ok = rpc_IPosCtrl->relativeMove(p); - delete[] p; - } break; - - case VOCAB_REF_SPEED: { - if (!rpc_IPosCtrl) { ok = false; break; } - ok = rpc_IPosCtrl->setRefSpeed(cmd.get(2).asInt32(), cmd.get(3).asFloat64()); - } break; - - case VOCAB_REF_SPEED_GROUP: { - if (!rpc_IPosCtrl) { ok = false; break; } - auto len = static_cast(cmd.get(2).asInt32()); - Bottle* jBottle_p = cmd.get(3).asList(); - Bottle* velBottle_p = cmd.get(4).asList(); - - if (rpc_IPosCtrl == nullptr) { - break; - } - - if (jBottle_p == nullptr || velBottle_p == nullptr) { - break; - } - if (len != jBottle_p->size() || len != velBottle_p->size()) { - break; - } - - int* j_tmp = new int[len]; - auto* spds_tmp = new double[len]; - - for (size_t i = 0; i < len; i++) { - j_tmp[i] = jBottle_p->get(i).asInt32(); - } - - for (size_t i = 0; i < len; i++) { - spds_tmp[i] = velBottle_p->get(i).asFloat64(); - } - - ok = rpc_IPosCtrl->setRefSpeeds(len, j_tmp, spds_tmp); - delete[] j_tmp; - delete[] spds_tmp; - } break; - - case VOCAB_REF_SPEEDS: { - if (!rpc_IPosCtrl) { ok = false; break; } - Bottle* b = cmd.get(2).asList(); - - if (b == nullptr) { - break; - } - - const size_t njs = b->size(); - if (njs != controlledJoints) { - break; - } - auto* p = new double[njs]; // LATER: optimize to avoid allocation. - for (size_t i = 0; i < njs; i++) { - p[i] = b->get(i).asFloat64(); - } - ok = rpc_IPosCtrl->setRefSpeeds(p); - delete[] p; - } break; - - case VOCAB_REF_ACCELERATION: { - if (!rpc_IPosCtrl) { ok = false; break; } - ok = rpc_IPosCtrl->setRefAcceleration(cmd.get(2).asInt32(), cmd.get(3).asFloat64()); - } break; - - case VOCAB_REF_ACCELERATION_GROUP: { - if (!rpc_IPosCtrl) { ok = false; break; } - auto len = static_cast(cmd.get(2).asInt32()); - Bottle* jBottle_p = cmd.get(3).asList(); - Bottle* accBottle_p = cmd.get(4).asList(); - - if (rpc_IPosCtrl == nullptr) { - break; - } - - if (jBottle_p == nullptr || accBottle_p == nullptr) { - break; - } - if (len != jBottle_p->size() || len != accBottle_p->size()) { - break; - } - - int* j_tmp = new int[len]; - auto* accs_tmp = new double[len]; - - for (size_t i = 0; i < len; i++) { - j_tmp[i] = jBottle_p->get(i).asInt32(); - } - - for (size_t i = 0; i < len; i++) { - accs_tmp[i] = accBottle_p->get(i).asFloat64(); - } - - ok = rpc_IPosCtrl->setRefAccelerations(len, j_tmp, accs_tmp); - delete[] j_tmp; - delete[] accs_tmp; - } break; - - case VOCAB_REF_ACCELERATIONS: { - if (!rpc_IPosCtrl) { ok = false; break; } - Bottle* b = cmd.get(2).asList(); - - if (b == nullptr) { - break; - } - - const size_t njs = b->size(); - if (njs != controlledJoints) { - break; - } - auto* p = new double[njs]; // LATER: optimize to avoid allocation. - for (size_t i = 0; i < njs; i++) { - p[i] = b->get(i).asFloat64(); - } - ok = rpc_IPosCtrl->setRefAccelerations(p); - delete[] p; - } break; - - case VOCAB_STOP: { - if (!rpc_IPosCtrl) { ok = false; break; } - ok = rpc_IPosCtrl->stop(cmd.get(2).asInt32()); - } break; - - case VOCAB_STOP_GROUP: { - if (!rpc_IPosCtrl) { ok = false; break; } - auto len = static_cast(cmd.get(2).asInt32()); - Bottle* jBottle_p = cmd.get(3).asList(); - - if (rpc_IPosCtrl == nullptr) { - break; - } - - if (jBottle_p == nullptr) { - break; - } - if (len != jBottle_p->size()) { - break; - } - - int* j_tmp = new int[len]; - - for (size_t i = 0; i < len; i++) { - j_tmp[i] = jBottle_p->get(i).asInt32(); - } - - ok = rpc_IPosCtrl->stop(len, j_tmp); - delete[] j_tmp; - } break; - - case VOCAB_STOPS: { - if (!rpc_IPosCtrl) { ok = false; break; } - ok = rpc_IPosCtrl->stop(); - } break; - - case VOCAB_E_RESET: { - if (!rpc_IEncTimed) { ok = false; break; } - ok = rpc_IEncTimed->resetEncoder(cmd.get(2).asInt32()); - } break; - - case VOCAB_E_RESETS: { - if (!rpc_IEncTimed) { ok = false; break; } - ok = rpc_IEncTimed->resetEncoders(); - } break; - - case VOCAB_ENCODER: { - if (!rpc_IEncTimed) { ok = false; break; } - ok = rpc_IEncTimed->setEncoder(cmd.get(2).asInt32(), cmd.get(3).asFloat64()); - } break; - - case VOCAB_ENCODERS: { - if (!rpc_IEncTimed) { ok = false; break; } - Bottle* b = cmd.get(2).asList(); - - if (b == nullptr) { - break; - } - - const size_t njs = b->size(); - if (njs != controlledJoints) { - break; - } - auto* p = new double[njs]; // LATER: optimize to avoid allocation. - for (size_t i = 0; i < njs; i++) { - p[i] = b->get(i).asFloat64(); - } - ok = rpc_IEncTimed->setEncoders(p); - delete[] p; - } break; - - case VOCAB_MOTOR_CPR: { - if (!rpc_IMotEnc) { ok = false; break; } - ok = rpc_IMotEnc->setMotorEncoderCountsPerRevolution(cmd.get(2).asInt32(), cmd.get(3).asFloat64()); - } break; - - case VOCAB_MOTOR_E_RESET: { - if (!rpc_IMotEnc) { ok = false; break; } - ok = rpc_IMotEnc->resetMotorEncoder(cmd.get(2).asInt32()); - } break; - - case VOCAB_MOTOR_E_RESETS: { - if (!rpc_IMotEnc) { ok = false; break; } - ok = rpc_IMotEnc->resetMotorEncoders(); - } break; - - case VOCAB_MOTOR_ENCODER: { - if (!rpc_IMotEnc) { ok = false; break; } - ok = rpc_IMotEnc->setMotorEncoder(cmd.get(2).asInt32(), cmd.get(3).asFloat64()); - } break; - - case VOCAB_MOTOR_ENCODERS: { - if (!rpc_IMotEnc) { ok = false; break; } - Bottle* b = cmd.get(2).asList(); - - if (b == nullptr) { - break; - } - - const size_t njs = b->size(); - if (njs != controlledJoints) { - break; - } - auto* p = new double[njs]; // LATER: optimize to avoid allocation. - for (size_t i = 0; i < njs; i++) { - p[i] = b->get(i).asFloat64(); - } - ok = rpc_IMotEnc->setMotorEncoders(p); - delete[] p; - } break; - - case VOCAB_AMP_ENABLE: { - if (!rcp_IAmp) { ok = false; break; } - ok = rcp_IAmp->enableAmp(cmd.get(2).asInt32()); - } break; - - case VOCAB_AMP_DISABLE: { - if (!rcp_IAmp) { ok = false; break; } - ok = rcp_IAmp->disableAmp(cmd.get(2).asInt32()); - } break; - - case VOCAB_AMP_MAXCURRENT: { - if (!rcp_IAmp) { ok = false; break; } - ok = rcp_IAmp->setMaxCurrent(cmd.get(2).asInt32(), cmd.get(3).asFloat64()); - } break; - - case VOCAB_AMP_PEAK_CURRENT: { - if (!rcp_IAmp) { ok = false; break; } - ok = rcp_IAmp->setPeakCurrent(cmd.get(2).asInt32(), cmd.get(3).asFloat64()); - } break; - - case VOCAB_AMP_NOMINAL_CURRENT: { - if (!rcp_IAmp) { ok = false; break; } - ok = rcp_IAmp->setNominalCurrent(cmd.get(2).asInt32(), cmd.get(3).asFloat64()); - } break; - - case VOCAB_AMP_PWM_LIMIT: { - if (!rcp_IAmp) { ok = false; break; } - ok = rcp_IAmp->setPWMLimit(cmd.get(2).asInt32(), cmd.get(3).asFloat64()); - } break; - - case VOCAB_LIMITS: { - if (!rcp_Ilim) {ok= false; break;} - ok = rcp_Ilim->setLimits(cmd.get(2).asInt32(), cmd.get(3).asFloat64(), cmd.get(4).asFloat64()); - } break; - - - case VOCAB_TEMPERATURE_LIMIT: { - if (!rpc_IMotor) { ok = false; break; } - ok = rpc_IMotor->setTemperatureLimit(cmd.get(2).asInt32(), cmd.get(3).asFloat64()); - } break; - - case VOCAB_GEARBOX_RATIO: { - if (!rpc_IMotor) { ok = false; break; } - ok = rpc_IMotor->setGearboxRatio(cmd.get(2).asInt32(), cmd.get(3).asFloat64()); - } break; - - case VOCAB_VEL_LIMITS: { - if (!rcp_Ilim) { ok = false; break; } - ok = rcp_Ilim->setVelLimits(cmd.get(2).asInt32(), cmd.get(3).asFloat64(), cmd.get(4).asFloat64()); - } break; - - default: - { - yCError(CONTROLBOARD, "received an unknown command after a VOCAB_SET (%s)", cmd.toString().c_str()); - } break; - } //switch(cmd.get(1).asVocab32() - break; - } - - case VOCAB_GET: { - rec = true; - yCTrace(CONTROLBOARD, "get command received"); - - double dtmp = 0.0; - Bottle btmp; - response.addVocab32(VOCAB_IS); - response.add(cmd.get(1)); - - switch (cmd.get(1).asVocab32()) { - - case VOCAB_TEMPERATURE_LIMIT: { - if (!rpc_IMotor) { ok = false; break; } - ok = rpc_IMotor->getTemperatureLimit(cmd.get(2).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_TEMPERATURE: { - if (!rpc_IMotor) { ok = false; break; } - ok = rpc_IMotor->getTemperature(cmd.get(2).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_GEARBOX_RATIO: { - if (!rpc_IMotor) { ok = false; break; } - ok = rpc_IMotor->getGearboxRatio(cmd.get(2).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_TEMPERATURES: { - if (!rpc_IMotor) { ok = false; break; } - auto* p = new double[controlledJoints]; - ok = rpc_IMotor->getTemperatures(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_AMP_MAXCURRENT: { - if (!rcp_IAmp) { ok = false; break; } - ok = rcp_IAmp->getMaxCurrent(cmd.get(2).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_POSITION_MOVE: { - if (!rpc_IPosCtrl) { ok = false; break; } - yCTrace(CONTROLBOARD, "getTargetPosition"); - ok = rpc_IPosCtrl->getTargetPosition(cmd.get(2).asInt32(), &dtmp); - - response.addFloat64(dtmp); - rec = true; - } break; - - case VOCAB_POSITION_MOVE_GROUP: { - if (!rpc_IPosCtrl) { ok = false; break; } - int len = cmd.get(2).asInt32(); - Bottle& in = *(cmd.get(3).asList()); - int* jointList = new int[len]; - auto* refs = new double[len]; - - for (int j = 0; j < len; j++) { - jointList[j] = in.get(j).asInt32(); - } - ok = rpc_IPosCtrl->getTargetPositions(len, jointList, refs); - - Bottle& b = response.addList(); - for (int i = 0; i < len; i++) { - b.addFloat64(refs[i]); - } - - delete[] jointList; - delete[] refs; - } break; - - case VOCAB_POSITION_MOVES: { - if (!rpc_IPosCtrl) { ok = false; break; } - auto* refs = new double[controlledJoints]; - ok = rpc_IPosCtrl->getTargetPositions(refs); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(refs[i]); - } - delete[] refs; - } break; - - case VOCAB_POSITION_DIRECT: { - if (!rpc_IPosDirect) { ok = false; break; } - yCTrace(CONTROLBOARD, "getRefPosition"); - ok = rpc_IPosDirect->getRefPosition(cmd.get(2).asInt32(), &dtmp); - - response.addFloat64(dtmp); - rec = true; - } break; - - case VOCAB_POSITION_DIRECT_GROUP: { - if (!rpc_IPosDirect) { ok = false; break; } - int len = cmd.get(2).asInt32(); - Bottle& in = *(cmd.get(3).asList()); - int* jointList = new int[len]; - auto* refs = new double[len]; - - for (int j = 0; j < len; j++) { - jointList[j] = in.get(j).asInt32(); - } - ok = rpc_IPosDirect->getRefPositions(len, jointList, refs); - - Bottle& b = response.addList(); - for (int i = 0; i < len; i++) { - b.addFloat64(refs[i]); - } - - delete[] jointList; - delete[] refs; - } break; - - case VOCAB_POSITION_DIRECTS: { - auto* refs = new double[controlledJoints]; - ok = rpc_IPosDirect->getRefPositions(refs); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(refs[i]); - } - delete[] refs; - } break; - - case VOCAB_VELOCITY_MOVE: { - if (!rpc_IVelCtrl) { ok = false; break; } - yCTrace(CONTROLBOARD, "getVelocityMove - cmd: %s", cmd.toString().c_str()); - ok = rpc_IVelCtrl->getRefVelocity(cmd.get(2).asInt32(), &dtmp); - - response.addFloat64(dtmp); - rec = true; - } break; - - case VOCAB_VELOCITY_MOVE_GROUP: { - if (!rpc_IVelCtrl) { ok = false; break; } - yCTrace(CONTROLBOARD, "getVelocityMove_group - cmd: %s", cmd.toString().c_str()); - - int len = cmd.get(2).asInt32(); - Bottle& in = *(cmd.get(3).asList()); - int* jointList = new int[len]; - auto* refs = new double[len]; - - for (int j = 0; j < len; j++) { - jointList[j] = in.get(j).asInt32(); - } - ok = rpc_IVelCtrl->getRefVelocities(len, jointList, refs); - - Bottle& b = response.addList(); - for (int i = 0; i < len; i++) { - b.addFloat64(refs[i]); - } - - delete[] jointList; - delete[] refs; - } break; - - case VOCAB_VELOCITY_MOVES: { - if (!rpc_IVelCtrl) { ok = false; break; } - yCTrace(CONTROLBOARD, "getVelocityMoves - cmd: %s", cmd.toString().c_str()); - - auto* refs = new double[controlledJoints]; - ok = rpc_IVelCtrl->getRefVelocities(refs); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(refs[i]); - } - delete[] refs; - } break; - - case VOCAB_MOTORS_NUMBER: { - if (!rpc_IMotor) { ok = false; break; } - int tmp; - ok = rpc_IMotor->getNumberOfMotors(&tmp); - response.addInt32(tmp); - } break; - - case VOCAB_AXES: { - if (!rpc_IPosCtrl) { ok = false; break; } - int tmp; - ok = rpc_IPosCtrl->getAxes(&tmp); - response.addInt32(tmp); - } break; - - case VOCAB_MOTION_DONE: { - if (!rpc_IPosCtrl) { ok = false; break; } - bool x = false; - ; - ok = rpc_IPosCtrl->checkMotionDone(cmd.get(2).asInt32(), &x); - response.addInt32(x); - } break; - - case VOCAB_MOTION_DONE_GROUP: { - if (!rpc_IPosCtrl) { ok = false; break; } - bool x = false; - int len = cmd.get(2).asInt32(); - Bottle& in = *(cmd.get(3).asList()); - int* jointList = new int[len]; - for (int j = 0; j < len; j++) { - jointList[j] = in.get(j).asInt32(); - } - if (rpc_IPosCtrl != nullptr) { - ok = rpc_IPosCtrl->checkMotionDone(len, jointList, &x); - } - response.addInt32(x); - - delete[] jointList; - } break; - - case VOCAB_MOTION_DONES: { - if (!rpc_IPosCtrl) { ok = false; break; } - bool x = false; - ok = rpc_IPosCtrl->checkMotionDone(&x); - response.addInt32(x); - } break; - - case VOCAB_REF_SPEED: { - if (!rpc_IPosCtrl) { ok = false; break; } - ok = rpc_IPosCtrl->getRefSpeed(cmd.get(2).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_REF_SPEED_GROUP: { - if (!rpc_IPosCtrl) { ok = false; break; } - int len = cmd.get(2).asInt32(); - Bottle& in = *(cmd.get(3).asList()); - int* jointList = new int[len]; - auto* speeds = new double[len]; - - for (int j = 0; j < len; j++) { - jointList[j] = in.get(j).asInt32(); - } - ok = rpc_IPosCtrl->getRefSpeeds(len, jointList, speeds); - - Bottle& b = response.addList(); - for (int i = 0; i < len; i++) { - b.addFloat64(speeds[i]); - } - - delete[] jointList; - delete[] speeds; - } break; - - case VOCAB_REF_SPEEDS: { - if (!rpc_IPosCtrl) { ok = false; break; } - auto* p = new double[controlledJoints]; - ok = rpc_IPosCtrl->getRefSpeeds(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_REF_ACCELERATION: { - if (!rpc_IPosCtrl) { ok = false; break; } - ok = rpc_IPosCtrl->getRefAcceleration(cmd.get(2).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_REF_ACCELERATION_GROUP: { - if (!rpc_IPosCtrl) { ok = false; break; } - int len = cmd.get(2).asInt32(); - Bottle& in = *(cmd.get(3).asList()); - int* jointList = new int[len]; - auto* accs = new double[len]; - - for (int j = 0; j < len; j++) { - jointList[j] = in.get(j).asInt32(); - } - ok = rpc_IPosCtrl->getRefAccelerations(len, jointList, accs); - - Bottle& b = response.addList(); - for (int i = 0; i < len; i++) { - b.addFloat64(accs[i]); - } - - delete[] jointList; - delete[] accs; - } break; - - case VOCAB_REF_ACCELERATIONS: { - if (!rpc_IPosCtrl) { ok = false; break; } - auto* p = new double[controlledJoints]; - ok = rpc_IPosCtrl->getRefAccelerations(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_ENCODER: { - if (!rpc_IEncTimed) { ok = false; break; } - ok = rpc_IEncTimed->getEncoder(cmd.get(2).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_ENCODERS: { - if (!rpc_IEncTimed) { ok = false; break; } - auto* p = new double[controlledJoints]; - ok = rpc_IEncTimed->getEncoders(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_ENCODER_SPEED: { - if (!rpc_IEncTimed) { ok = false; break; } - ok = rpc_IEncTimed->getEncoderSpeed(cmd.get(2).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_ENCODER_SPEEDS: { - if (!rpc_IEncTimed) { ok = false; break; } - auto* p = new double[controlledJoints]; - ok = rpc_IEncTimed->getEncoderSpeeds(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_ENCODER_ACCELERATION: { - if (!rpc_IEncTimed) { ok = false; break; } - ok = rpc_IEncTimed->getEncoderAcceleration(cmd.get(2).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_ENCODER_ACCELERATIONS: { - if (!rpc_IEncTimed) { ok = false; break; } - auto* p = new double[controlledJoints]; - ok = rpc_IEncTimed->getEncoderAccelerations(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_MOTOR_CPR: { - if (!rpc_IMotEnc) { ok = false; break; } - ok = rpc_IMotEnc->getMotorEncoderCountsPerRevolution(cmd.get(2).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_MOTOR_ENCODER: { - if (!rpc_IMotEnc) { ok = false; break; } - ok = rpc_IMotEnc->getMotorEncoder(cmd.get(2).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_MOTOR_ENCODERS: { - if (!rpc_IMotEnc) { ok = false; break; } - auto* p = new double[controlledJoints]; - ok = rpc_IMotEnc->getMotorEncoders(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_MOTOR_ENCODER_SPEED: { - if (!rpc_IMotEnc) { ok = false; break; } - ok = rpc_IMotEnc->getMotorEncoderSpeed(cmd.get(2).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_MOTOR_ENCODER_SPEEDS: { - if (!rpc_IMotEnc) { ok = false; break; } - auto* p = new double[controlledJoints]; - ok = rpc_IMotEnc->getMotorEncoderSpeeds(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_MOTOR_ENCODER_ACCELERATION: { - if (!rpc_IMotEnc) { ok = false; break; } - ok = rpc_IMotEnc->getMotorEncoderAcceleration(cmd.get(2).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_MOTOR_ENCODER_ACCELERATIONS: { - auto* p = new double[controlledJoints]; - if (!rpc_IMotEnc) { ok = false; break; } - ok = rpc_IMotEnc->getMotorEncoderAccelerations(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_MOTOR_ENCODER_NUMBER: { - int num = 0; - if (!rpc_IMotEnc) { ok = false; break; } - ok = rpc_IMotEnc->getNumberOfMotorEncoders(&num); - response.addInt32(num); - } break; - - case VOCAB_AMP_CURRENT: { - if (!rcp_IAmp) { ok = false; break; } - ok = rcp_IAmp->getCurrent(cmd.get(2).asInt32(), &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_AMP_CURRENTS: { - if (!rcp_IAmp) { ok = false; break; } - auto* p = new double[controlledJoints]; - ok = rcp_IAmp->getCurrents(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addFloat64(p[i]); - } - delete[] p; - } break; - - case VOCAB_AMP_STATUS: { - if (!rcp_IAmp) { ok = false; break; } - int* p = new int[controlledJoints]; - ok = rcp_IAmp->getAmpStatus(p); - Bottle& b = response.addList(); - for (size_t i = 0; i < controlledJoints; i++) { - b.addInt32(p[i]); - } - delete[] p; - } break; - - case VOCAB_AMP_STATUS_SINGLE: { - int j = cmd.get(2).asInt32(); - int itmp; - if (!rcp_IAmp) { ok = false; break; } - ok = rcp_IAmp->getAmpStatus(j, &itmp); - response.addInt32(itmp); - } break; - - case VOCAB_AMP_NOMINAL_CURRENT: { - int m = cmd.get(2).asInt32(); - if (!rcp_IAmp) { ok = false; break; } - ok = rcp_IAmp->getNominalCurrent(m, &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_AMP_PEAK_CURRENT: { - int m = cmd.get(2).asInt32(); - if (!rcp_IAmp) { ok = false; break; } - ok = rcp_IAmp->getPeakCurrent(m, &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_AMP_PWM: { - int m = cmd.get(2).asInt32(); - if (!rcp_IAmp) { ok = false; break; } - ok = rcp_IAmp->getPWM(m, &dtmp); - yCTrace(CONTROLBOARD) << "RPC parser::getPWM: j" << m << " val " << dtmp; - response.addFloat64(dtmp); - } break; - - case VOCAB_AMP_PWM_LIMIT: { - int m = cmd.get(2).asInt32(); - if (!rcp_IAmp) { ok = false; break; } - ok = rcp_IAmp->getPWMLimit(m, &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_AMP_VOLTAGE_SUPPLY: { - int m = cmd.get(2).asInt32(); - if (!rcp_IAmp) { ok = false; break; } - ok = rcp_IAmp->getPowerSupplyVoltage(m, &dtmp); - response.addFloat64(dtmp); - } break; - - case VOCAB_LIMITS: { - double min = 0.0; - double max = 0.0; - if (!rcp_Ilim) { ok = false; break; } - ok = rcp_Ilim->getLimits(cmd.get(2).asInt32(), &min, &max); - response.addFloat64(min); - response.addFloat64(max); - } break; - - case VOCAB_VEL_LIMITS: { - double min = 0.0; - double max = 0.0; - if (!rcp_Ilim) { ok = false; break; } - ok = rcp_Ilim->getVelLimits(cmd.get(2).asInt32(), &min, &max); - response.addFloat64(min); - response.addFloat64(max); - } break; - - case VOCAB_INFO_NAME: { - std::string name = "undocumented"; - if (!rpc_AxisInfo) { ok = false; break; } - ok = rpc_AxisInfo->getAxisName(cmd.get(2).asInt32(), name); - response.addString(name.c_str()); - } break; - - case VOCAB_INFO_TYPE: { - yarp::dev::JointTypeEnum type; - if (!rpc_AxisInfo) { ok = false; break; } - ok = rpc_AxisInfo->getJointType(cmd.get(2).asInt32(), type); - response.addInt32(type); - } break; - - default: - { - yCError(CONTROLBOARD, "received an unknown request after a VOCAB_GET: %s", yarp::os::Vocab32::decode(cmd.get(1).asVocab32()).c_str()); - } break; - } //switch cmd.get(1).asVocab32()) - - lastRpcStamp.update(); - appendTimeStamp(response, lastRpcStamp); - } // case VOCAB_GET - default: - break; - } //switch code - - if (!rec) { - ok = DeviceResponder::respond(cmd, response); - } - } - - if (!ok) { - // failed thus send only a VOCAB back. - response.clear(); - response.addVocab32(VOCAB_FAILED); - } else { - response.addVocab32(VOCAB_OK); - } - } - std::string ss = response.toString(); - - return ok; -} - -bool RPCMessagesParser::initialize() -{ - bool ok = false; - if (rpc_IPosCtrl) { - int tmp_axes; - ok = rpc_IPosCtrl->getAxes(&tmp_axes); - controlledJoints = static_cast(tmp_axes); - } - - DeviceResponder::makeUsage(); - addUsage("[get] [axes]", "get the number of axes"); - addUsage("[get] [name] $iAxisNumber", "get a human-readable name for an axis, if available"); - addUsage("[set] [pos] $iAxisNumber $fPosition", "command the position of an axis"); - addUsage("[set] [rel] $iAxisNumber $fPosition", "command the relative position of an axis"); - addUsage("[set] [vmo] $iAxisNumber $fVelocity", "command the velocity of an axis"); - addUsage("[get] [enc] $iAxisNumber", "get the encoder value for an axis"); - - std::string args; - for (size_t i = 0; i < controlledJoints; i++) { - if (i > 0) { - args += " "; - } - // removed dependency from yarp internals - //args = args + "$f" + yarp::yarp::conf::numeric::to_string(i); - } - addUsage((std::string("[set] [poss] (") + args + ")").c_str(), - "command the position of all axes"); - addUsage((std::string("[set] [rels] (") + args + ")").c_str(), - "command the relative position of all axes"); - addUsage((std::string("[set] [vmos] (") + args + ")").c_str(), - "command the velocity of all axes"); - - addUsage("[set] [aen] $iAxisNumber", "enable (amplifier for) the given axis"); - addUsage("[set] [adi] $iAxisNumber", "disable (amplifier for) the given axis"); - addUsage("[get] [acu] $iAxisNumber", "get current for the given axis"); - addUsage("[get] [acus]", "get current for all axes"); - - return ok; -} - -void RPCMessagesParser::init(yarp::dev::DeviceDriver* x) -{ - x->view(rpc_IPid); - x->view(rpc_IPosCtrl); - x->view(rpc_IPosDirect); - x->view(rpc_IVelCtrl); - x->view(rpc_IEncTimed); - x->view(rpc_IMotEnc); - x->view(rpc_IMotor); - x->view(rpc_IVar); - x->view(rcp_IAmp); - x->view(rcp_Ilim); - x->view(rpc_AxisInfo); - x->view(rpc_IRemoteCalibrator); - x->view(rpc_Icalib); - x->view(rpc_IImpedance); - x->view(rpc_ITorque); - x->view(rpc_iCtrlMode); - x->view(rpc_IInteract); - x->view(rpc_ICurrent); - x->view(rpc_IPWM); - x->view(rpc_IJointFault); - controlledJoints = 0; -} - -void RPCMessagesParser::reset() -{ - rpc_IPid = nullptr; - rpc_IPosCtrl = nullptr; - rpc_IPosDirect = nullptr; - rpc_IVelCtrl = nullptr; - rpc_IEncTimed = nullptr; - rpc_IMotEnc = nullptr; - rpc_IMotor = nullptr; - rpc_IVar = nullptr; - rcp_IAmp = nullptr; - rcp_Ilim = nullptr; - rpc_AxisInfo = nullptr; - rpc_IRemoteCalibrator = nullptr; - rpc_Icalib = nullptr; - rpc_IImpedance = nullptr; - rpc_ITorque = nullptr; - rpc_iCtrlMode = nullptr; - rpc_IInteract = nullptr; - rpc_ICurrent = nullptr; - rpc_IPWM = nullptr; - rpc_IJointFault = nullptr; - controlledJoints = 0; -} diff --git a/src/devices/controlBoardWrapper/RPCMessagesParser.h b/src/devices/controlBoardWrapper/RPCMessagesParser.h deleted file mode 100644 index 6653558d54b..00000000000 --- a/src/devices/controlBoardWrapper/RPCMessagesParser.h +++ /dev/null @@ -1,146 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_RPCMESSAGESPARSER_H -#define YARP_DEV_CONTROLBOARDWRAPPER_RPCMESSAGESPARSER_H - - -// This file contains helper functions for the ControlBoardWrapper - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include - - -#ifdef MSVC -# pragma warning(disable : 4355) -#endif - -/* - * To optimize memory allocation, for group of joints we can have one mem reserver for rpc port - * and on e for streaming. The size could be numOfSubDevices*maxNumOfjointForSubdevice. - * (we could also use the actual joint number for each subdevice using a for loop). TODO - */ - -class ControlBoardWrapperCommon; - -/* the control command message type -* head is a Bottle which contains the specification of the message type -* body is a Vector which move the robot accordingly -*/ -typedef yarp::os::PortablePair CommandMessage; - - -/** -* Helper object for parsing RPC port messages -*/ -class RPCMessagesParser : - public yarp::dev::DeviceResponder -{ -protected: - yarp::dev::IPidControl* rpc_IPid {nullptr}; - yarp::dev::IPositionControl* rpc_IPosCtrl {nullptr}; - yarp::dev::IPositionDirect* rpc_IPosDirect {nullptr}; - yarp::dev::IVelocityControl* rpc_IVelCtrl {nullptr}; - yarp::dev::IEncodersTimed* rpc_IEncTimed {nullptr}; - yarp::dev::IMotorEncoders* rpc_IMotEnc {nullptr}; - yarp::dev::IAmplifierControl* rcp_IAmp {nullptr}; - yarp::dev::IControlLimits* rcp_Ilim {nullptr}; - yarp::dev::ITorqueControl* rpc_ITorque {nullptr}; - yarp::dev::IControlMode* rpc_iCtrlMode {nullptr}; - yarp::dev::IAxisInfo* rpc_AxisInfo {nullptr}; - yarp::dev::IRemoteCalibrator* rpc_IRemoteCalibrator {nullptr}; - yarp::dev::IControlCalibration* rpc_Icalib {nullptr}; - yarp::dev::IImpedanceControl* rpc_IImpedance {nullptr}; - yarp::dev::IInteractionMode* rpc_IInteract {nullptr}; - yarp::dev::IMotor* rpc_IMotor {nullptr}; - yarp::dev::IRemoteVariables* rpc_IVar {nullptr}; - yarp::dev::ICurrentControl* rpc_ICurrent {nullptr}; - yarp::dev::IPWMControl* rpc_IPWM {nullptr}; - yarp::dev::IJointFault* rpc_IJointFault{ nullptr }; - yarp::sig::Vector tmpVect; - yarp::os::Stamp lastRpcStamp; - std::mutex mutex; - size_t controlledJoints {0}; - -public: - /** - * Constructor. - */ - RPCMessagesParser() = default; - - /** - * Initialization. - * @param x is the pointer to the instance of the object that uses the RPCMessagesParser. - * This is required to recover the pointers to the interfaces that implement the responses - * to the commands. - */ - void init(yarp::dev::DeviceDriver* x); - void reset(); - - bool respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& response) override; - - void handleTorqueMsg(const yarp::os::Bottle& cmd, - yarp::os::Bottle& response, - bool* rec, - bool* ok); - - void handleJointFaultMsg(const yarp::os::Bottle& cmd, - yarp::os::Bottle& response, - bool* rec, - bool* ok); - - void handleControlModeMsg(const yarp::os::Bottle& cmd, - yarp::os::Bottle& response, - bool* rec, - bool* ok); - - void handleImpedanceMsg(const yarp::os::Bottle& cmd, - yarp::os::Bottle& response, - bool* rec, - bool* ok); - - void handleInteractionModeMsg(const yarp::os::Bottle& cmd, - yarp::os::Bottle& response, - bool* rec, - bool* ok); - - void handleProtocolVersionRequest(const yarp::os::Bottle& cmd, - yarp::os::Bottle& response, - bool* rec, - bool* ok); - - void handleRemoteCalibratorMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool* rec, bool* ok); - - void handleRemoteVariablesMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool* rec, bool* ok); - - void handleCurrentMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool* rec, bool* ok); - - void handlePWMMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool* rec, bool* ok); - - void handlePidMsg(const yarp::os::Bottle& cmd, yarp::os::Bottle& response, bool* rec, bool* ok); - - /** - * Initialize the internal data. - * @return true/false on success/failure - */ - virtual bool initialize(); -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_RPCMESSAGESPARSER_H diff --git a/src/devices/controlBoardWrapper/StreamingMessagesParser.cpp b/src/devices/controlBoardWrapper/StreamingMessagesParser.cpp deleted file mode 100644 index 8f064af39de..00000000000 --- a/src/devices/controlBoardWrapper/StreamingMessagesParser.cpp +++ /dev/null @@ -1,292 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "StreamingMessagesParser.h" - -#include - -#include "ControlBoardWrapperCommon.h" -#include "ControlBoardLogComponent.h" -#include - -using namespace yarp::os; -using namespace yarp::dev; -using namespace yarp::dev::impl; -using namespace yarp::sig; - - -void StreamingMessagesParser::init(yarp::dev::DeviceDriver* x) -{ - stream_nJoints = 0; - x->view(stream_IPosCtrl); - x->view(stream_IPosDirect); - x->view(stream_IVel); - x->view(stream_ITorque); - x->view(stream_IPWM); - x->view(stream_ICurrent); -} - -void StreamingMessagesParser::reset() -{ - stream_nJoints = 0; - stream_IPosCtrl = nullptr; - stream_IPosDirect = nullptr; - stream_IVel = nullptr; - stream_ITorque = nullptr; - stream_IPWM = nullptr; - stream_ICurrent = nullptr; -} - -bool StreamingMessagesParser::initialize() -{ - stream_nJoints = 0; - if (stream_IPosCtrl) { - stream_IPosCtrl->getAxes(&stream_nJoints); - } - - return true; -} - -// streaming port callback -void StreamingMessagesParser::onRead(CommandMessage& v) -{ - Bottle& b = v.head; - Vector& cmdVector = v.body; - - //Use the following only for debug, since it can heavily slow down the system - yCTrace(CONTROLBOARD, "Received command %s, %s\n", b.toString().c_str(), cmdVector.toString().c_str()); - - // some consistency checks - if (static_cast(cmdVector.size()) > stream_nJoints) { - std::string str = yarp::os::Vocab32::decode(b.get(0).asVocab32()); - yCError(CONTROLBOARD, "Received command vector with number of elements bigger than axis controlled by this wrapper (cmd: %s requested jnts: %d received jnts: %d)\n", str.c_str(), stream_nJoints, (int)cmdVector.size()); - return; - } - if (cmdVector.data() == nullptr) { - yCError(CONTROLBOARD, "Received null command vector"); - return; - } - - switch (b.get(0).asVocab32()) { - // manage commands with interface name as first - case VOCAB_PWMCONTROL_INTERFACE: { - switch (b.get(1).asVocab32()) { - case VOCAB_PWMCONTROL_REF_PWM: { - if (stream_IPWM) { - bool ok = stream_IPWM->setRefDutyCycle(b.get(2).asInt32(), cmdVector[0]); - if (!ok) { - yCError(CONTROLBOARD, "Errors while trying to command an pwm message"); - } - } else { - yCError(CONTROLBOARD, "PWM interface not valid"); - } - } break; - case VOCAB_PWMCONTROL_REF_PWMS: { - if (stream_IPWM) { - bool ok = stream_IPWM->setRefDutyCycles(cmdVector.data()); - if (!ok) { - yCError(CONTROLBOARD, "Errors while trying to command an pwm message"); - } - } else { - yCError(CONTROLBOARD, "PWM interface not valid"); - } - } break; - } - } break; - - case VOCAB_CURRENTCONTROL_INTERFACE: { - switch (b.get(1).asVocab32()) { - case VOCAB_CURRENT_REF: { - if (stream_ICurrent) { - bool ok = stream_ICurrent->setRefCurrent(b.get(2).asInt32(), cmdVector[0]); - if (!ok) { - yCError(CONTROLBOARD, "Errors while trying to command a streaming current message on single joint\n"); - } - } - } break; - case VOCAB_CURRENT_REFS: { - if (stream_ICurrent) { - bool ok = stream_ICurrent->setRefCurrents(cmdVector.data()); - if (!ok) { - yCError(CONTROLBOARD, "Errors while trying to command a streaming current message on all joints\n"); - } - } - } break; - case VOCAB_CURRENT_REF_GROUP: { - if (stream_ICurrent) { - int n_joints = b.get(2).asInt32(); - Bottle* jlut = b.get(3).asList(); - if ((static_cast(jlut->size()) != n_joints) && (static_cast(cmdVector.size()) != n_joints)) { - yCError(CONTROLBOARD, "Received VOCAB_CURRENT_REF_GROUP size of joints vector or currents vector does not match the selected joint number\n"); - } - - int* joint_list = new int[n_joints]; - for (int i = 0; i < n_joints; i++) { - joint_list[i] = jlut->get(i).asInt32(); - } - - - bool ok = stream_ICurrent->setRefCurrents(n_joints, joint_list, cmdVector.data()); - if (!ok) { - yCError(CONTROLBOARD, "Error while trying to command a streaming current message on joint group\n"); - } - - delete[] joint_list; - } - } break; - default: - { - std::string str = yarp::os::Vocab32::decode(b.get(0).asVocab32()); - yCError(CONTROLBOARD, "Unrecognized message while receiving on command port (%s)\n", str.c_str()); - } break; - } - } break; - - // fallback to commands without interface name - case VOCAB_POSITION_MODE: { - yCError(CONTROLBOARD, "Received VOCAB_POSITION_MODE this is an send invalid message on streaming port"); - break; - } - - case VOCAB_POSITION_MOVES: { - if (stream_IPosCtrl) { - bool ok = stream_IPosCtrl->positionMove(cmdVector.data()); - if (!ok) { - yCError(CONTROLBOARD, "Errors while trying to start a position move"); - } - } - - } break; - - case VOCAB_VELOCITY_MODE: { - yCError(CONTROLBOARD, "Received VOCAB_VELOCITY_MODE this is an send invalid message on streaming port"); - break; - } - - case VOCAB_VELOCITY_MOVE: { - stream_IVel->velocityMove(b.get(1).asInt32(), cmdVector[0]); - } break; - - case VOCAB_VELOCITY_MOVES: { - if (stream_IVel) { - bool ok = stream_IVel->velocityMove(cmdVector.data()); - if (!ok) { - yCError(CONTROLBOARD, "Errors while trying to start a velocity move"); - } - } - } break; - - case VOCAB_POSITION_DIRECT: { - if (stream_IPosDirect) { - bool ok = stream_IPosDirect->setPosition(b.get(1).asInt32(), cmdVector[0]); // cmdVector.data()); - if (!ok) { - yCError(CONTROLBOARD, "Errors while trying to command an streaming position direct message on joint %d\n", b.get(1).asInt32()); - } - } - } break; - - case VOCAB_TORQUES_DIRECT: { - if (stream_ITorque) { - bool ok = stream_ITorque->setRefTorque(b.get(1).asInt32(), cmdVector[0]); - if (!ok) { - yCError(CONTROLBOARD, "Errors while trying to command a streaming torque direct message on single joint\n"); - } - } - } break; - - case VOCAB_TORQUES_DIRECTS: { - if (stream_ITorque) { - bool ok = stream_ITorque->setRefTorques(cmdVector.data()); - if (!ok) { - yCError(CONTROLBOARD, "Errors while trying to command a streaming torque direct message on all joints\n"); - } - } - } break; - - case VOCAB_TORQUES_DIRECT_GROUP: { - if (stream_ITorque) { - int n_joints = b.get(1).asInt32(); - Bottle* jlut = b.get(2).asList(); - if ((static_cast(jlut->size()) != n_joints) && (static_cast(cmdVector.size()) != n_joints)) { - yCError(CONTROLBOARD, "Received VOCAB_TORQUES_DIRECT_GROUP size of joints vector or torques vector does not match the selected joint number\n"); - } - - int* joint_list = new int[n_joints]; - for (int i = 0; i < n_joints; i++) { - joint_list[i] = jlut->get(i).asInt32(); - } - - - bool ok = stream_ITorque->setRefTorques(n_joints, joint_list, cmdVector.data()); - if (!ok) { - yCError(CONTROLBOARD, "Error while trying to command a streaming toruqe direct message on joint group\n"); - } - - delete[] joint_list; - } - } break; - - case VOCAB_POSITION_DIRECT_GROUP: { - if (stream_IPosDirect) { - int n_joints = b.get(1).asInt32(); - Bottle* jlut = b.get(2).asList(); - if ((static_cast(jlut->size()) != n_joints) && (static_cast(cmdVector.size()) != n_joints)) { - yCError(CONTROLBOARD, "Received VOCAB_POSITION_DIRECT_GROUP size of joints vector or positions vector does not match the selected joint number\n"); - } - - int* joint_list = new int[n_joints]; - for (int i = 0; i < n_joints; i++) { - joint_list[i] = jlut->get(i).asInt32(); - } - - - bool ok = stream_IPosDirect->setPositions(n_joints, joint_list, cmdVector.data()); - if (!ok) { - yCError(CONTROLBOARD, "Error while trying to command a streaming position direct message on joint group\n"); - } - - delete[] joint_list; - } - } break; - - case VOCAB_POSITION_DIRECTS: { - if (stream_IPosDirect) { - bool ok = stream_IPosDirect->setPositions(cmdVector.data()); - if (!ok) { - yCError(CONTROLBOARD, "Error while trying to command a streaming position direct message on all joints\n"); - } - } - } break; - - case VOCAB_VELOCITY_MOVE_GROUP: { - if (stream_IVel) { - int n_joints = b.get(1).asInt32(); - Bottle* jlut = b.get(2).asList(); - if ((static_cast(jlut->size()) != n_joints) && (static_cast(cmdVector.size()) != n_joints)) { - yCError(CONTROLBOARD, "Received VOCAB_VELOCITY_MOVE_GROUP size of joints vector or positions vector does not match the selected joint number\n"); - } - - int* joint_list = new int[n_joints]; - for (int i = 0; i < n_joints; i++) { - joint_list[i] = jlut->get(i).asInt32(); - } - - bool ok = stream_IVel->velocityMove(n_joints, joint_list, cmdVector.data()); - if (!ok) { - yCError(CONTROLBOARD, "Error while trying to command a velocity move on joint group\n"); - } - - delete[] joint_list; - } - } break; - - default: - { - std::string str = yarp::os::Vocab32::decode(b.get(0).asVocab32()); - yCError(CONTROLBOARD, "Unrecognized message while receiving on command port (%s)\n", str.c_str()); - } break; - } -} diff --git a/src/devices/controlBoardWrapper/StreamingMessagesParser.h b/src/devices/controlBoardWrapper/StreamingMessagesParser.h deleted file mode 100644 index 6ed84a49e49..00000000000 --- a/src/devices/controlBoardWrapper/StreamingMessagesParser.h +++ /dev/null @@ -1,86 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_STREAMINGMESSAGESPARSER_H -#define YARP_DEV_CONTROLBOARDWRAPPER_STREAMINGMESSAGESPARSER_H - - -// This file contains helper functions for the ControlBoardWrapper - - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include -#include - -#ifdef MSVC -# pragma warning(disable : 4355) -#endif - -/* - * To optimize memory allocation, for group of joints we can have one mem reserver for rpc port - * and on e for streaming. The size could be numOfSubDevices*maxNumOfjointForSubdevice. - * (we could also use the actual joint number for each subdevice using a for loop). TODO - */ - - -/* the control command message type -* head is a Bottle which contains the specification of the message type -* body is a Vector which move the robot accordingly -*/ -typedef yarp::os::PortablePair CommandMessage; - -/** -* Callback implementation after buffered input. -*/ -class StreamingMessagesParser : public yarp::os::TypedReaderCallback -{ -protected: - yarp::dev::IPositionControl* stream_IPosCtrl {nullptr}; - yarp::dev::IPositionDirect* stream_IPosDirect {nullptr}; - yarp::dev::IVelocityControl* stream_IVel {nullptr}; - yarp::dev::ITorqueControl* stream_ITorque {nullptr}; - yarp::dev::IPWMControl* stream_IPWM {nullptr}; - yarp::dev::ICurrentControl* stream_ICurrent {nullptr}; - int stream_nJoints {0}; - -public: - /** - * Constructor. - */ - StreamingMessagesParser() = default; - - /** - * Initialization. - * @param x is the instance of the container class using the callback. - */ - void init(yarp::dev::DeviceDriver* x); - - void reset(); - - using yarp::os::TypedReaderCallback::onRead; - /** - * Callback function. - * @param v is the Vector being received. - */ - void onRead(CommandMessage& v) override; - - bool initialize(); -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_STREAMINGMESSAGESPARSER_H diff --git a/src/devices/controlBoardWrapper/SubDevice.cpp b/src/devices/controlBoardWrapper/SubDevice.cpp deleted file mode 100644 index 62c1bf62e13..00000000000 --- a/src/devices/controlBoardWrapper/SubDevice.cpp +++ /dev/null @@ -1,231 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "SubDevice.h" - -#include -#include - -#include "ControlBoardWrapper.h" -#include "ControlBoardLogComponent.h" -#include "RPCMessagesParser.h" -#include "StreamingMessagesParser.h" -#include - -using namespace yarp::os; -using namespace yarp::dev; -using namespace yarp::sig; - - -bool SubDevice::configure(size_t wb, size_t wt, size_t b, size_t t, size_t n, const std::string& key, const std::string& _parentName) -{ - parentName = _parentName; - - configuredF = false; - - wbase = wb; - wtop = wt; - base = b; - top = t; - axes = n; - id = key; - - if (top < base) { - yCError(CONTROLBOARD) << "Check configuration file top: Wrong or unknown device %s. Cannot attach to it.", parentName.c_str(), k.c_str()); - return false; - } - - //configure first - if (!configuredF) { - yCError(CONTROLBOARD, "Part <%s>: You need to call configure before you can attach any device", parentName.c_str()); - return false; - } - - if (d == nullptr) { - yCError(CONTROLBOARD, "Part <%s>: Invalid device (null pointer)", parentName.c_str()); - return false; - } - - subdevice = d; - - if (subdevice->isValid()) { - subdevice->view(pid); - subdevice->view(pos); - subdevice->view(posDir); - subdevice->view(vel); - subdevice->view(amp); - subdevice->view(lim); - subdevice->view(calib); - subdevice->view(info); - subdevice->view(iTimed); - subdevice->view(iTorque); - subdevice->view(iImpedance); - subdevice->view(iMode); - subdevice->view(iJntEnc); - subdevice->view(iMotEnc); - subdevice->view(iInteract); - subdevice->view(imotor); - subdevice->view(iVar); - subdevice->view(iCurr); - subdevice->view(iPWM); - } else { - yCError(CONTROLBOARD, "Part <%s>: Invalid device %s (isValid() returned false).", parentName.c_str(), k.c_str()); - return false; - } - - if (!iMode) { - yCDebug(CONTROLBOARD, "Part <%s>: iMode not valid interface.", parentName.c_str()); - } - - if (!iTorque) { - yCDebug(CONTROLBOARD, "Part <%s>: iTorque not valid interface.", parentName.c_str()); - } - - if (!iCurr) { - yCDebug(CONTROLBOARD, "Part <%s>: iCurr not valid interface.", parentName.c_str()); - } - - if (!iPWM) { - yCDebug(CONTROLBOARD, "Part <%s>: iPWM not valid interface.", parentName.c_str()); - } - - if (!iImpedance) { - yCDebug(CONTROLBOARD, "Part <%s>: iImpedance not valid interface.", parentName.c_str()); - } - - if (!iInteract) { - yCDebug(CONTROLBOARD, "Part <%s>: iInteractionMode not valid interface.", parentName.c_str()); - } - - if (!iMotEnc) { - yCDebug(CONTROLBOARD, "Part <%s>: iMotorEncoder not valid interface.", parentName.c_str()); - } - - if (!imotor) { - yCDebug(CONTROLBOARD, "Part <%s>: iMotor not valid interface.", parentName.c_str()); - } - - if (!iVar) { - yCDebug(CONTROLBOARD, "Part <%s>: iRemoteVariable not valid interface.", parentName.c_str()); - } - - if (!info) { - yCDebug(CONTROLBOARD, "Part <%s>: iAxisInfo not valid interface.", parentName.c_str()); - } - - size_t deviceJoints = 0; - - // checking minimum set of intefaces required - if (!pos) { - yCError(CONTROLBOARD, "Part <%s>: IPositionControl interface was not found in subdevice. Quitting", parentName.c_str()); - return false; - } - - if (!vel) { - yCError(CONTROLBOARD, "Part <%s>: IVelocityControl interface was not found in subdevice. Quitting", parentName.c_str()); - return false; - } - - if (!iJntEnc) { - yCError(CONTROLBOARD, "Part <%s>: IEncoderTimed interface was not found in subdevice.", parentName.c_str()); - return false; - } - - if (pos != nullptr) { - int tmp_axes; - if (!pos->getAxes(&tmp_axes)) { - yCError(CONTROLBOARD) << "Failed to get axes number for subdevice " << k.c_str(); - return false; - } - if (tmp_axes <= 0) { - yCError(CONTROLBOARD, "Part <%s>: attached device has an invalid number of joints (%d)", parentName.c_str(), tmp_axes); - return false; - } - deviceJoints = static_cast(tmp_axes); - } else { - int tmp_axes; - if (!pos->getAxes(&tmp_axes)) { - yCError(CONTROLBOARD, "Part <%s>: failed to get axes number for subdevice %s.", parentName.c_str(), k.c_str()); - return false; - } - if (tmp_axes <= 0) { - yCError(CONTROLBOARD, "Part <%s>: attached device has an invalid number of joints (%d)", parentName.c_str(), tmp_axes); - return false; - } - deviceJoints = static_cast(tmp_axes); - } - - if (deviceJoints < axes) { - yCError(CONTROLBOARD, "Part <%s>: check device configuration, number of joints of attached device '%zu' less \ - than the one specified during configuration '%zu' for %s.", - parentName.c_str(), - deviceJoints, - axes, - k.c_str()); - return false; - } - - int subdevAxes; - if (!pos || !pos->getAxes(&subdevAxes)) { - yCError(CONTROLBOARD) << "Device <" << parentName << "> attached to subdevice " << k.c_str() << " but it was not ready yet. \n" - << "Please check the device has been correctly created and all required initialization actions has been performed."; - return false; - } - - totalAxis = deviceJoints; - attachedF = true; - return true; -} diff --git a/src/devices/controlBoardWrapper/SubDevice.h b/src/devices/controlBoardWrapper/SubDevice.h deleted file mode 100644 index 819c198c1a1..00000000000 --- a/src/devices/controlBoardWrapper/SubDevice.h +++ /dev/null @@ -1,135 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#ifndef YARP_DEV_CONTROLBOARDWRAPPER_SUBDEVICE_H -#define YARP_DEV_CONTROLBOARDWRAPPER_SUBDEVICE_H - - -// ControlBoardWrapper -// A modified version of the remote control board class -// which remaps joints, it can also merge networks into a single part. - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include "RPCMessagesParser.h" -#include "StreamingMessagesParser.h" -#include -#include - -#ifdef MSVC -# pragma warning(disable : 4355) -#endif - -/* - * To optimize memory allocation, for group of joints we can have one mem reserver for rpc port - * and on e for streaming. The size could be numOfSubDevices*maxNumOfjointForSubdevice. - * (we could also use the actual joint number for each subdevice using a for loop). TODO - */ - -class ControlBoardWrapper; - -/* -* An Helper class for the controlBoardWrapper -* It maps only a subpart of the underlying device. -*/ - -class SubDevice -{ -public: - std::string id; - size_t base {static_cast(-1)}; - size_t top {static_cast(-1)}; - size_t wbase; //wrapper base - size_t wtop; //wrapper top - size_t axes {0}; //number of used axis of this subdevice - size_t totalAxis; //Numeber of total axis that the subdevice can control - - bool configuredF {false}; - - std::string parentName; - - yarp::dev::PolyDriver* subdevice {nullptr}; - yarp::dev::IPidControl* pid {nullptr}; - yarp::dev::IPositionControl* pos {nullptr}; - yarp::dev::IVelocityControl* vel {nullptr}; - yarp::dev::IEncodersTimed* iJntEnc {nullptr}; - yarp::dev::IMotorEncoders* iMotEnc {nullptr}; - yarp::dev::IAmplifierControl* amp {nullptr}; - yarp::dev::IControlLimits* lim {nullptr}; - yarp::dev::IControlCalibration* calib {nullptr}; - yarp::dev::IPreciselyTimed* iTimed {nullptr}; - yarp::dev::ITorqueControl* iTorque {nullptr}; - yarp::dev::IImpedanceControl* iImpedance {nullptr}; - yarp::dev::IControlMode* iMode {nullptr}; - yarp::dev::IAxisInfo* info {nullptr}; - yarp::dev::IPositionDirect* posDir {nullptr}; - yarp::dev::IInteractionMode* iInteract {nullptr}; - yarp::dev::IMotor* imotor {nullptr}; - yarp::dev::IRemoteVariables* iVar {nullptr}; - yarp::dev::IPWMControl* iPWM {nullptr}; - yarp::dev::ICurrentControl* iCurr {nullptr}; - - yarp::sig::Vector subDev_joint_encoders; - yarp::sig::Vector jointEncodersTimes; - yarp::sig::Vector subDev_motor_encoders; - yarp::sig::Vector motorEncodersTimes; - - SubDevice() = default; - - bool attach(yarp::dev::PolyDriver* d, const std::string& id); - void detach(); - - bool configure(size_t wbase, size_t wtop, size_t base, size_t top, size_t axes, const std::string& id, const std::string& _parentName); - - bool isAttached() const - { - return attachedF; - } - -private: - bool attachedF {false}; -}; - -typedef std::vector SubDeviceVector; - -struct DevicesLutEntry -{ - size_t offset; //an offset, the device is mapped starting from this joint - size_t deviceEntry; //index to the joint corresponding subdevice in the list - size_t jointIndexInDev; //the index of joint in the numeration inside the device -}; - - -class WrappedDevice -{ -public: - SubDeviceVector subdevices; - std::vector lut; - size_t maxNumOfJointsInDevices; - - inline SubDevice* getSubdevice(size_t i) - { - if (i >= subdevices.size()) { - return nullptr; - } - - return &subdevices[i]; - } -}; - -#endif // YARP_DEV_CONTROLBOARDWRAPPER_SUBDEVICE_H diff --git a/src/devices/controlBoardWrapper/tests/CMakeLists.txt b/src/devices/controlBoardWrapper/tests/CMakeLists.txt deleted file mode 100644 index 7751ec6d118..00000000000 --- a/src/devices/controlBoardWrapper/tests/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -# SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) -# SPDX-License-Identifier: BSD-3-Clause - -controlBoardRemapper (controlBoardWrapper) diff --git a/src/devices/controlBoardWrapper/tests/controlBoardWrapper_test.cpp b/src/devices/controlBoardWrapper/tests/controlBoardWrapper_test.cpp deleted file mode 100644 index e8697632a4e..00000000000 --- a/src/devices/controlBoardWrapper/tests/controlBoardWrapper_test.cpp +++ /dev/null @@ -1,48 +0,0 @@ -/* - * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -using namespace yarp::dev; -using namespace yarp::dev::Nav2D; -using namespace yarp::sig; -using namespace yarp::os; - -TEST_CASE("dev::Map2DnwsTest", "[yarp::dev]") -{ - YARP_REQUIRE_PLUGIN("map2D_nws_yarp", "device"); - - Network::setLocalMode(true); - - SECTION("Checking map2D_nws_yarp device") - { - PolyDriver ddmapserver; - - ////////"Checking opening map2DServer and map2DClient polydrivers" - { - Property pmapserver_cfg; - pmapserver_cfg.put("device", "map2D_nws_yarp"); - REQUIRE(ddmapserver.open(pmapserver_cfg)); - } - - //"Close all polydrivers and check" - { - CHECK(ddmapserver.close()); - } - } - - Network::setLocalMode(false); -}