diff --git a/CMakeLists.txt b/CMakeLists.txt index 64e5712a0e..10e20630bb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,7 +23,7 @@ endif() # Main project project(YARP - VERSION 3.0.1 + VERSION 3.0.102 LANGUAGES C CXX) set(PROJECT_DESCRIPTION "YARP: A thin middleware for humanoid robots and more") diff --git a/bindings/CMakeLists.txt b/bindings/CMakeLists.txt index 163583ebde..2c9735c8eb 100644 --- a/bindings/CMakeLists.txt +++ b/bindings/CMakeLists.txt @@ -88,7 +88,7 @@ if (_nested_build) # bindings for other languages to be compiled from the build material. # Install main CMakeLists and Swig input file - foreach(f CMakeLists.txt yarp.i README.md) + foreach(f CMakeLists.txt yarp.i macrosForMultipleAnalogSensors.i matlab/vectors_fromTo_matlab.i README.md) install(FILES ${CMAKE_SOURCE_DIR}/bindings/${f} COMPONENT development DESTINATION ${CMAKE_INSTALL_DATADIR}/yarp/bindings) diff --git a/bindings/lua/tests/test_vocab.lua b/bindings/lua/tests/test_vocab.lua index 16cb28e64d..ee085084e8 100644 --- a/bindings/lua/tests/test_vocab.lua +++ b/bindings/lua/tests/test_vocab.lua @@ -10,10 +10,13 @@ require("yarp") function test_vocab() - vocab = yarp.Vocab.encode("abcd") + vocab = yarp.encode("abcd") assert("number" == type(vocab)) assert(1684234849 == vocab) - assert("abcd" == yarp.Vocab.decode(vocab)) + assert("abcd" == yarp.decode(vocab)) + vocab2 = yarp.createVocab('a','b','c','d') + assert("number" == type(vocab2)) + assert (vocab == vocab2) end function test_vocab_pixel_types_enum() @@ -27,10 +30,10 @@ function test_vocab_global_scope() assert("number" == type(yarp.VOCAB_CM_POSITION)) assert(7565168 == yarp.VOCAB_CM_POSITION) -- VOCAB3 assert(1685286768 == yarp.VOCAB_CM_POSITION_DIRECT) -- VOCAB4 - assert(845375334 == yarp.VOCAB_FRAMEGRABBER_CONTROL2) -- VOCAB4 with '2' + assert(6514534 == yarp.VOCAB_FRAMEGRABBER_CONTROL) -- VOCAB4 end test_vocab() test_vocab_pixel_types_enum() ---test_vocab_global_scope() -- Requires SWIG_VERSION >= 0x030011 (3.0.11), see yarp.i +test_vocab_global_scope() diff --git a/bindings/macrosForMultipleAnalogSensors.i b/bindings/macrosForMultipleAnalogSensors.i new file mode 100644 index 0000000000..0469cc1201 --- /dev/null +++ b/bindings/macrosForMultipleAnalogSensors.i @@ -0,0 +1,58 @@ +// Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) +// All rights reserved. +// +// This software may be modified and distributed under the terms of the +// BSD-3-Clause license. See the accompanying LICENSE file for details. + +%define RESET_CONSTANTS_IN_EXTENDED_ANALOG_SENSOR_INTERFACE +#undef ThreeAxisGyroscopes_EXTENDED_INTERFACE +#undef ThreeAxisLinearAccelerometers_EXTENDED_INTERFACE +#undef ThreeAxisMagnetometers_EXTENDED_INTERFACE +#undef OrientationSensors_EXTENDED_INTERFACE +#undef TemperatureSensors_EXTENDED_INTERFACE +#undef SixAxisForceTorqueSensors_EXTENDED_INTERFACE +#undef ContactLoadCellArrays_EXTENDED_INTERFACE +#undef EncoderArrays_EXTENDED_INTERFACE +#undef SkinPatches_EXTENDED_INTERFACE +%enddef + +%define EXTENDED_ANALOG_SENSOR_INTERFACE(sensor) + +RESET_CONSTANTS_IN_EXTENDED_ANALOG_SENSOR_INTERFACE + +#define sensor ## _EXTENDED_INTERFACE 1 + + std::string get ## sensor ## Name(int sens_index) const { + std::string name; + bool ok = self->get ## sensor ## Name(sens_index,name); + if (!ok) return "unknown"; + return name; + } + +#if !ContactLoadCellArray_EXTENDED_INTERFACE \ + && !EncoderArray_EXTENDED_INTERFACE \ + && !SkinPatch_EXTENDED_INTERFACE + std::string get ## sensor ## FrameName(int sens_index) const { + std::string frameName; + bool ok = self->get ## sensor ## FrameName(sens_index,frameName); + if (!ok) return "unknown"; + return frameName; + } +#endif + +#if OrientationSensor_EXTENDED_INTERFACE + double get ## sensor ## MeasureAsRollPitchYaw(int sens_index, yarp::sig::Vector& rpy) const { + double timestamp; + bool ok = self->get ## sensor ## MeasureAsRollPitchYaw(sens_index, rpy, timestamp); +#else + double get ## sensor ## Measure(int sens_index, yarp::sig::Vector& out) const { + double timestamp; + bool ok = self->get ## sensor ## Measure(sens_index, out, timestamp); +#endif + if (!ok) return -1; + return timestamp; + } + +#undef sensor ## _EXTENDED_INTERFACE + +%enddef diff --git a/bindings/matlab/IVector_fromTo_matlab.i b/bindings/matlab/IVector_fromTo_matlab.i deleted file mode 100644 index 8ebaffd471..0000000000 --- a/bindings/matlab/IVector_fromTo_matlab.i +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) -// All rights reserved. -// -// This software may be modified and distributed under the terms of the -// BSD-3-Clause license. See the accompanying LICENSE file for details. - -%extend std::vector { - // Convert to a Matlab vector - mxArray * toMatlab() const - { - // create Matlab vector and map it to pointer 'd' - size_t selfDim = self->size(); - mxArray *p = mxCreateDoubleMatrix(selfDim, 1, mxREAL); - double* d = static_cast(mxGetData(p)); - - // copy items from 'self' to 'd' - const int* selfData = self->data(); - for(size_t i=0; i(selfData[i]); - } - return p; - } - - // Convert from a Matlab vector - void fromMatlab(mxArray * in) - { - // check size - const mwSize * dims = mxGetDimensions(in); - size_t selfDim = self->size(); - size_t matlabVecDim = (dims[0] == 1 ? dims[1] : dims[0]); - - if (matlabVecDim == selfDim) - { - // map Matlab vector to pointer 'd' - double* d = static_cast(mxGetData(in)); - - // copy items from 'd' to 'self' - int* selfData = self->data(); - for(size_t i=0; i(d[i]); - } - return; - } else { - mexErrMsgIdAndTxt("yarp:IVector:wrongDimension", - "Wrong vector size. Matlab size: %d. IVector size: %d", matlabVecDim, selfDim); - } - } - - // Reset values - void zero() - { - int* selfData = self->data(); - for(size_t i=0; i < self->size(); i++) - { - selfData[i] = 0; - } - return; - } -} diff --git a/bindings/matlab/vectors_fromTo_matlab.i b/bindings/matlab/vectors_fromTo_matlab.i new file mode 100644 index 0000000000..d2b8d4d131 --- /dev/null +++ b/bindings/matlab/vectors_fromTo_matlab.i @@ -0,0 +1,108 @@ +// Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) +// All rights reserved. +// +// This software may be modified and distributed under the terms of the +// BSD-3-Clause license. See the accompanying LICENSE file for details. + +%define RESET_CONSTANTS_IN_TO_MATLAB +#undef mxCreateDoubleMatrixHasComplexFlagParam +#undef mxCreateLogicalMatrixHasComplexFlagParam +%enddef + +%define TO_MATLAB(matlabType,mxArrayInitializer) + + // Convert to a Matlab vector + mxArray * toMatlab() const + { + // create Matlab vector and map it to pointer 'd' + size_t selfDim = self->size(); + #if mxArrayInitializer ## HasComplexFlagParam + mxArray *p = mxArrayInitializer(selfDim, 1, mxREAL); + #else + mxArray *p = mxArrayInitializer(selfDim, 1); + #endif + matlabType* d = static_cast(mxGetData(p)); + + // copy items from 'self' to 'd' + for(size_t i=0; i(self->operator[](i)); + } + return p; + } + +%enddef + + +%define FROM_MATLAB(matlabType,cppType,mxArrayInitializer,vectorClass) + + // Convert from a Matlab vector + void fromMatlab(mxArray * in) + { + // check size + const mwSize * dims = mxGetDimensions(in); + size_t selfDim = self->size(); + size_t matlabVecDim = (dims[0] == 1 ? dims[1] : dims[0]); + + if (matlabVecDim == selfDim) + { + // map Matlab vector to pointer 'd' + matlabType* d = static_cast(mxGetData(in)); + + // copy items from 'd' to 'self' + for(size_t i=0; ioperator[](i) = static_cast(d[i]); + } + return; + } else { + mexErrMsgIdAndTxt("yarp::vectorClass::wrongDimension", + "Wrong vector size. Matlab size: %d. vectorClass size: %d", matlabVecDim, selfDim); + } + } + +%enddef + + +%define RESET(resetValue) + + // Reset values + void zero() + { + for(size_t i=0; i < self->size(); i++) + { + self->operator[](i) = resetValue; + } + return; + } + +%enddef + +RESET_CONSTANTS_IN_TO_MATLAB +#define mxCreateDoubleMatrixHasComplexFlagParam 1 + +%extend std::vector { + TO_MATLAB(double,mxCreateDoubleMatrix) + FROM_MATLAB(double,double,mxCreateDoubleMatrix,DVector) + RESET(0) +} + +%extend std::vector { + TO_MATLAB(bool,mxCreateLogicalMatrix) + FROM_MATLAB(bool,bool,mxCreateLogicalMatrix,BVector) + RESET(false) +} + +%extend std::vector { + TO_MATLAB(double,mxCreateDoubleMatrix) + FROM_MATLAB(double,int,mxCreateDoubleMatrix,IVector) + RESET(0) +} + +%extend yarp::sig::VectorOf { + TO_MATLAB(double,mxCreateDoubleMatrix) + FROM_MATLAB(double,double,mxCreateDoubleMatrix,yarp::sig::VectorOf) + RESET(0) +} + +RESET_CONSTANTS_IN_TO_MATLAB diff --git a/bindings/yarp.i b/bindings/yarp.i index 08b1750b30..d56a6685ee 100644 --- a/bindings/yarp.i +++ b/bindings/yarp.i @@ -221,7 +221,7 @@ %ignore yarp::sig::Image::getRow(int) const; %ignore yarp::sig::Image::getIplImage() const; %ignore yarp::sig::Image::getReadType() const; -%ignore yarp::sig::Vector::getType() const; +%ignore yarp::sig::VectorOf::getType() const; %ignore yarp::os::Property::put(const char *,Value *); %ignore yarp::os::Bottle::add(Value *); %rename(toString) std::string::operator const char *() const; @@ -301,24 +301,8 @@ void setExternal2(yarp::sig::Image *img, PyObject* mem, int w, int h) { #endif -// We skip macros from Vocab.h via SWIG_PREPROCESSOR_SHOULD_SKIP_THIS directive -// But cannot define YARP_OS_VOCAB_H as we would miss encode/decode -#if (SWIG_VERSION >= 0x030011) && (!defined(SWIGCSHARP)) - %define VOCAB(x1,x2,x3,x4) x4*16777216+x3*65536+x2*256+x1 // Tested on Lua and Python - %enddef -#elif defined(SWIGCSHARP) - // We'd rather no VOCAB vs a defective VOCAB (value 0), but required for CSHARP. - %define VOCAB(x1,x2,x3,x4) 0 // VOCABs in enum should be generated in Lua and Python, not C# - %enddef -#endif // If old SWIG and not CSHARP, no global defined (but enum should be) VOCABs wrappers generated -%define VOCAB4(x1,x2,x3,x4) VOCAB(x1,x2,x3,x4) -%enddef -%define VOCAB3(x1,x2,x3) VOCAB(x1,x2,x3,0) -%enddef -%define VOCAB2(x1,x2) VOCAB(x1,x2,0,0) -%enddef -%define VOCAB1(x1) VOCAB(x1,0,0,0) -%enddef +// Define macros for handling the multiple analog sensors interfaces +%include macrosForMultipleAnalogSensors.i #if defined( SWIGALLEGROCL ) %include "allegro/compat.h" @@ -427,6 +411,10 @@ MAKE_COMMS(Bottle) %include %include %include +%include +%include +%include +%include %include %include %include @@ -438,6 +426,7 @@ MAKE_COMMS(Bottle) #ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 %include #endif +%include %include %include %include @@ -452,17 +441,19 @@ MAKE_COMMS(Bottle) #endif %include %include +%include #if !defined(SWIGCHICKEN) && !defined(SWIGALLEGROCL) %template(DVector) std::vector; %template(BVector) std::vector; %template(SVector) std::vector; -#ifdef SWIGMATLAB - // Extend IVector for handling conversion of vectors from and to Matlab - %include "matlab/IVector_fromTo_matlab.i" -#endif %template(IVector) std::vector; + #ifdef SWIGMATLAB + // Extend IVector for handling conversion of vectors from and to Matlab + %include "matlab/vectors_fromTo_matlab.i" + #endif + #if defined(SWIGCSHARP) SWIG_STD_VECTOR_SPECIALIZE_MINIMUM(Pid,yarp::dev::Pid) #endif @@ -544,9 +535,9 @@ typedef yarp::os::BufferedPort BufferedPortSound; %} %{ -typedef yarp::os::TypedReader TypedReaderVector; -typedef yarp::os::TypedReaderCallback TypedReaderCallbackVector; -typedef yarp::os::BufferedPort BufferedPortVector; +typedef yarp::os::TypedReader> TypedReaderVector; +typedef yarp::os::TypedReaderCallback> TypedReaderCallbackVector; +typedef yarp::os::BufferedPort> BufferedPortVector; %} %feature("notabstract") ImageRgb; @@ -577,6 +568,7 @@ typedef yarp::os::BufferedPort BufferedPortVector; %feature("notabstract") yarp::os::BufferedPort; %feature("notabstract") BufferedPortVector; +%template(Vector) yarp::sig::VectorOf; %template(ImageRgb) yarp::sig::ImageOf; %template(TypedReaderImageRgb) yarp::os::TypedReader >; %template(TypedReaderCallbackImageRgb) yarp::os::TypedReaderCallback >; @@ -610,9 +602,9 @@ typedef yarp::os::BufferedPort BufferedPortVector; %template(TypedReaderCallbackSound) yarp::os::TypedReaderCallback; %template(BufferedPortSound) yarp::os::BufferedPort; -%template(TypedReaderVector) yarp::os::TypedReader; -%template(TypedReaderCallbackVector) yarp::os::TypedReaderCallback; -%template(BufferedPortVector) yarp::os::BufferedPort; +%template(TypedReaderVector) yarp::os::TypedReader >; +%template(TypedReaderCallbackVector) yarp::os::TypedReaderCallback>; +%template(BufferedPortVector) yarp::os::BufferedPort >; // Add getPixel and setPixel methods to access float values %extend yarp::sig::ImageOf { @@ -727,84 +719,30 @@ typedef yarp::os::BufferedPort BufferedPortImageRgbFloat; ////////////////////////////////////////////////////////////////////////// // Deal with PolyDriver idiom that doesn't translate too well -%extend yarp::dev::PolyDriver { - yarp::dev::IFrameGrabberImage *viewFrameGrabberImage() { - yarp::dev::IFrameGrabberImage *result; - self->view(result); - return result; - } - - yarp::dev::IPositionControl *viewIPositionControl() { - yarp::dev::IPositionControl *result; - self->view(result); - return result; - } - - yarp::dev::IVelocityControl *viewIVelocityControl() { - yarp::dev::IVelocityControl *result; - self->view(result); - return result; - } - - yarp::dev::IEncoders *viewIEncoders() { - yarp::dev::IEncoders *result; - self->view(result); - return result; - } - - yarp::dev::IMotorEncoders *viewIMotorEncoders() { - yarp::dev::IMotorEncoders *result; - self->view(result); - return result; - } - - yarp::dev::IPidControl *viewIPidControl() { - yarp::dev::IPidControl *result; - self->view(result); - return result; - } - - yarp::dev::IAmplifierControl *viewIAmplifierControl() { - yarp::dev::IAmplifierControl *result; - self->view(result); - return result; - } - - yarp::dev::IControlLimits *viewIControlLimits() { - yarp::dev::IControlLimits *result; - self->view(result); - return result; - } - - yarp::dev::ICartesianControl *viewICartesianControl() { - yarp::dev::ICartesianControl *result; +%define CAST_POLYDRIVER_TO_INTERFACE(interface) + yarp::dev:: ## interface *view ## interface ## () { + yarp::dev:: ## interface *result; self->view(result); return result; } +%enddef - yarp::dev::IGazeControl *viewIGazeControl() { - yarp::dev::IGazeControl *result; - self->view(result); - return result; - } - - yarp::dev::IImpedanceControl *viewIImpedanceControl() { - yarp::dev::IImpedanceControl *result; - self->view(result); - return result; - } +%extend yarp::dev::PolyDriver { - yarp::dev::ITorqueControl *viewITorqueControl() { - yarp::dev::ITorqueControl *result; - self->view(result); - return result; - } + CAST_POLYDRIVER_TO_INTERFACE(IFrameGrabberImage) + CAST_POLYDRIVER_TO_INTERFACE(IPositionControl) + CAST_POLYDRIVER_TO_INTERFACE(IVelocityControl) + CAST_POLYDRIVER_TO_INTERFACE(IEncoders) + CAST_POLYDRIVER_TO_INTERFACE(IMotorEncoders) + CAST_POLYDRIVER_TO_INTERFACE(IPidControl) + CAST_POLYDRIVER_TO_INTERFACE(IAmplifierControl) + CAST_POLYDRIVER_TO_INTERFACE(IControlLimits) + CAST_POLYDRIVER_TO_INTERFACE(ICartesianControl) + CAST_POLYDRIVER_TO_INTERFACE(IGazeControl) + CAST_POLYDRIVER_TO_INTERFACE(IImpedanceControl) + CAST_POLYDRIVER_TO_INTERFACE(ITorqueControl) + CAST_POLYDRIVER_TO_INTERFACE(IControlMode) - yarp::dev::IControlMode *viewIControlMode() { - yarp::dev::IControlMode *result; - self->view(result); - return result; - } #ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 yarp::dev::IControlMode *viewIControlMode2() { yarp::dev::IControlMode *result; @@ -813,23 +751,11 @@ typedef yarp::os::BufferedPort BufferedPortImageRgbFloat; } #endif - yarp::dev::IPWMControl *viewIPWMControl() { - yarp::dev::IPWMControl *result; - self->view(result); - return result; - } - - yarp::dev::ICurrentControl *viewICurrentControl() { - yarp::dev::ICurrentControl *result; - self->view(result); - return result; - } + CAST_POLYDRIVER_TO_INTERFACE(IInteractionMode) + CAST_POLYDRIVER_TO_INTERFACE(IPWMControl) + CAST_POLYDRIVER_TO_INTERFACE(ICurrentControl) + CAST_POLYDRIVER_TO_INTERFACE(IAnalogSensor) - yarp::dev::IAnalogSensor *viewIAnalogSensor() { - yarp::dev::IAnalogSensor *result; - self->view(result); - return result; - } #ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 yarp::dev::IFrameGrabberControls *viewIFrameGrabberControls2() { yarp::dev::IFrameGrabberControls *result; @@ -837,29 +763,26 @@ typedef yarp::os::BufferedPort BufferedPortImageRgbFloat; return result; } #endif - yarp::dev::IFrameGrabberControls *viewIFrameGrabberControls() { - yarp::dev::IFrameGrabberControls *result; - self->view(result); - return result; - } - - yarp::dev::IPositionDirect *viewIPositionDirect() { - yarp::dev::IPositionDirect *result; - self->view(result); - return result; - } - yarp::dev::IRemoteVariables *viewIRemoteVariables() { - yarp::dev::IRemoteVariables *result; - self->view(result); - return result; - } - - yarp::dev::IAxisInfo *viewIAxisInfo() { - yarp::dev::IAxisInfo *result; - self->view(result); - return result; - } + CAST_POLYDRIVER_TO_INTERFACE(IFrameGrabberControls) + CAST_POLYDRIVER_TO_INTERFACE(IPositionDirect) + CAST_POLYDRIVER_TO_INTERFACE(IRemoteVariables) + CAST_POLYDRIVER_TO_INTERFACE(IAxisInfo) + +// These views are currently disabled in SWIG + java generator since they are +// useless without the EXTENDED_ANALOG_SENSOR_INTERFACE part. +// See also https://github.com/robotology/yarp/issues/1770 +#if !defined(SWIGJAVA) + CAST_POLYDRIVER_TO_INTERFACE(IThreeAxisGyroscopes) + CAST_POLYDRIVER_TO_INTERFACE(IThreeAxisLinearAccelerometers) + CAST_POLYDRIVER_TO_INTERFACE(IThreeAxisMagnetometers) + CAST_POLYDRIVER_TO_INTERFACE(IOrientationSensors) + CAST_POLYDRIVER_TO_INTERFACE(ITemperatureSensors) + CAST_POLYDRIVER_TO_INTERFACE(ISixAxisForceTorqueSensors) + CAST_POLYDRIVER_TO_INTERFACE(IContactLoadCellArrays) + CAST_POLYDRIVER_TO_INTERFACE(IEncoderArrays) + CAST_POLYDRIVER_TO_INTERFACE(ISkinPatches) +#endif // you'll need to add an entry for every interface you wish // to use @@ -869,6 +792,15 @@ typedef yarp::os::BufferedPort BufferedPortImageRgbFloat; ////////////////////////////////////////////////////////////////////////// // Deal with ControlBoardInterfaces pointer arguments that don't translate +%extend yarp::dev::IImpedanceControl { + int getAxes() { + int buffer; + bool ok = self->getAxes(&buffer); + if (!ok) return 0; + return buffer; + } +} + %extend yarp::dev::IPositionControl { int getAxes() { int buffer; @@ -1105,6 +1037,14 @@ typedef yarp::os::BufferedPort BufferedPortImageRgbFloat; } } +%extend yarp::dev::IInteractionMode { + yarp::dev::InteractionModeEnum getInteractionMode(int axis) { + yarp::dev::InteractionModeEnum mode = VOCAB_IM_UNKNOWN; + self->getInteractionMode(axis, &mode); + return mode; + } +} + %extend yarp::dev::IPositionDirect { int getAxes() { int buffer; @@ -1127,7 +1067,22 @@ typedef yarp::os::BufferedPort BufferedPortImageRgbFloat; } } -%extend yarp::sig::Vector { +// This is part is currently broken in SWIG + java generator since SWIG 3.0.3 +// (last swig version tested: 3.0.12) +// See also https://github.com/robotology/yarp/issues/1770 +#if !defined(SWIGJAVA) + %extend yarp::dev::IThreeAxisGyroscopes {EXTENDED_ANALOG_SENSOR_INTERFACE(ThreeAxisGyroscope)} + %extend yarp::dev::IThreeAxisLinearAccelerometers {EXTENDED_ANALOG_SENSOR_INTERFACE(ThreeAxisLinearAccelerometer)} + %extend yarp::dev::IThreeAxisMagnetometers {EXTENDED_ANALOG_SENSOR_INTERFACE(ThreeAxisMagnetometer)} + %extend yarp::dev::IOrientationSensors {EXTENDED_ANALOG_SENSOR_INTERFACE(OrientationSensor)} + %extend yarp::dev::ITemperatureSensors {EXTENDED_ANALOG_SENSOR_INTERFACE(TemperatureSensor)} + %extend yarp::dev::ISixAxisForceTorqueSensors {EXTENDED_ANALOG_SENSOR_INTERFACE(SixAxisForceTorqueSensor)} + %extend yarp::dev::IContactLoadCellArrays {EXTENDED_ANALOG_SENSOR_INTERFACE(ContactLoadCellArray)} + %extend yarp::dev::IEncoderArrays {EXTENDED_ANALOG_SENSOR_INTERFACE(EncoderArray)} + %extend yarp::dev::ISkinPatches {EXTENDED_ANALOG_SENSOR_INTERFACE(SkinPatch)} +#endif + +%extend yarp::sig::VectorOf { double get(int j) { @@ -1379,8 +1334,8 @@ public: return self->cast_as(); } - yarp::sig::Vector* asVector() { - return self->cast_as(); + yarp::sig::VectorOf* asVector() { + return self->cast_as>(); } yarp::sig::Matrix* asMatrix() { @@ -1407,59 +1362,58 @@ public: ////////////////////////////////////////////////////////////////////////// // Deal with IFrameGrabberControls pointer arguments that don't translate - %extend yarp::dev::IFrameGrabberControls { - CameraDescriptor getCameraDescription() { - CameraDescriptor result; - self->getCameraDescription(&result); - return result; - } - - bool hasFeature(int feature) { - bool result; - self->hasFeature(feature, &result); - return result; - } +%extend yarp::dev::IFrameGrabberControls { + CameraDescriptor getCameraDescription() { + CameraDescriptor result; + self->getCameraDescription(&result); + return result; + } - double getFeature(int feature) { - double result; - self->getFeature(feature, &result); - return result; - } + bool hasFeature(int feature) { + bool result; + self->hasFeature(feature, &result); + return result; + } - bool hasOnOff(int feature) { - bool result; - self->hasOnOff(feature, &result); - return result; - } + double getFeature(int feature) { + double result; + self->getFeature(feature, &result); + return result; + } - bool getActive(int feature) { - bool result; - self->getActive(feature, &result); - return result; - } + bool hasOnOff(int feature) { + bool result; + self->hasOnOff(feature, &result); + return result; + } - bool hasAuto(int feature) { - bool result; - self->hasAuto(feature, &result); - return result; - } + bool getActive(int feature) { + bool result; + self->getActive(feature, &result); + return result; + } - bool hasManual(int feature) { - bool result; - self->hasManual(feature, &result); - return result; - } + bool hasAuto(int feature) { + bool result; + self->hasAuto(feature, &result); + return result; + } - bool hasOnePush(int feature) { - bool result; - self->hasOnePush(feature, &result); - return result; - } + bool hasManual(int feature) { + bool result; + self->hasManual(feature, &result); + return result; + } - FeatureMode getMode(int feature) { - FeatureMode result; - self->getMode(feature, &result); - return result; - } - } + bool hasOnePush(int feature) { + bool result; + self->hasOnePush(feature, &result); + return result; + } + FeatureMode getMode(int feature) { + FeatureMode result; + self->getMode(feature, &result); + return result; + } +} diff --git a/cmake/FindI2C.cmake b/cmake/FindI2C.cmake new file mode 100644 index 0000000000..f6ae18c687 --- /dev/null +++ b/cmake/FindI2C.cmake @@ -0,0 +1,71 @@ +#.rst: +# FindI2C +# ------- +# +# Find the I2C device library. +# +# Once done this will define the following variables:: +# +# I2C_INCLUDE_DIRS - I2C include directory +# I2C_LIBRARIES - I2C libraries +# I2C_FOUND - if false, you cannot build anything that requires I2C + +#============================================================================= +# Copyright 2018 Istituto Italiano di Tecnologia (IIT) +# Authors: Nicolò Genesio +# +# Distributed under the OSI-approved BSD License (the "License"); +# see accompanying file Copyright.txt for details. +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#============================================================================= +# (To distribute this file outside of YCM, substitute the full +# License text for the above reference.) + +include(FindPackageHandleStandardArgs) +include(SelectLibraryConfigurations) + +find_path(I2C_smbus_h_INCLUDE_DIR + NAMES i2c/smbus.h) +mark_as_advanced(I2C_smbus_h_INCLUDE_DIR) + +if(I2C_smbus_h_INCLUDE_DIR) + find_library(I2C_i2c_LIBRARY + NAMES i2c) + mark_as_advanced(I2C_i2c_LIBRARY) + + set(I2C_LIBRARIES ${I2C_i2c_LIBRARY}) + set(I2C_INCLUDE_DIRS ${I2C_smbus_h_INCLUDE_DIR}) + set(I2C_DEFINITIONS I2C_HAS_SMBUS_H) + find_package_handle_standard_args(I2C + FOUND_VAR I2C_FOUND + REQUIRED_VARS I2C_INCLUDE_DIRS + I2C_LIBRARIES) +else() + find_path(I2C_i2c_dev_h_INCLUDE_DIR + NAMES linux/i2c-dev.h) + mark_as_advanced(I2C_i2c_dev_h_INCLUDE_DIR) + if(EXISTS "${I2C_i2c_dev_h_INCLUDE_DIR}/linux/i2c-dev.h") + file(READ "${I2C_i2c_dev_h_INCLUDE_DIR}/linux/i2c-dev.h" _i2c_dev_content) + if(NOT "${_i2c_dev_content}" MATCHES "i2c_smbus_access") + set(I2C_i2c_dev_h_INCLUDE_DIR I2C_i2c_dev_h_INCLUDE_DIR-NOTFOUND CACHE STRING "" FORCE) + endif() + + set(I2C_LIBRARIES "") + set(I2C_INCLUDE_DIRS ${I2C_i2c_dev_h_INCLUDE_DIR}) + set(I2C_DEFINITIONS "") + endif() + find_package_handle_standard_args(I2C + FOUND_VAR I2C_FOUND + REQUIRED_VARS I2C_INCLUDE_DIRS) +endif() + + +# Set package properties if FeatureSummary was included +if(COMMAND set_package_properties) + set_package_properties(I2C PROPERTIES DESCRIPTION "Userspace I2C programming library" + URL "https://www.kernel.org/pub/software/utils/i2c-tools/") +endif() + diff --git a/cmake/YarpFindDependencies.cmake b/cmake/YarpFindDependencies.cmake index ac2e60cf93..b10c528928 100644 --- a/cmake/YarpFindDependencies.cmake +++ b/cmake/YarpFindDependencies.cmake @@ -344,7 +344,7 @@ else() endif() endif() -set(RTF_REQUIRED_VERSION 1.4.0) +set(RTF_REQUIRED_VERSION 1.4.61) find_package(RTF ${RTF_REQUIRED_VERSION} QUIET) checkandset_dependency(RTF) @@ -463,6 +463,9 @@ checkandset_dependency(BISON) find_package(FLEX QUIET) checkandset_dependency(FLEX) +find_package(I2C QUIET) +checkandset_dependency(I2C) + # PRINT DEPENDENCIES STATUS: message(STATUS "I have found the following libraries:") @@ -500,6 +503,7 @@ print_dependency(GStreamer) print_dependency(GStreamerPluginsBase) print_dependency(BISON) print_dependency(FLEX) +print_dependency(I2C) # CHECK DEPENDENCIES: diff --git a/doc/device_invocation/grabber_basic.dox b/doc/device_invocation/grabber_basic.dox index 4f357148ef..1caf66ff16 100644 --- a/doc/device_invocation/grabber_basic.dox +++ b/doc/device_invocation/grabber_basic.dox @@ -52,7 +52,7 @@ Here is a list of properties checked when starting up a device based on this con test_grabber.t2Tangential distortion of the lens(fake)12.0 test_grabber.freqrate of test images in Hz test_grabber.periodperiod of test images in seconds -test_grabber.modebouncy [ball], scrolly [line], grid [grid], grid multisize [size], random [rand], none [none]line +test_grabber.modebouncy [ball], scrolly [line], grid [grid], grid multisize [size], random [rand], none [none], time test[time]line test_grabber.src test_grabber.bayershould emit bayer test image? test_grabber.monoshould emit a monochrome image? diff --git a/doc/device_invocation/group_basic.dox b/doc/device_invocation/group_basic.dox index e8d5479226..ba27c6d5f3 100644 --- a/doc/device_invocation/group_basic.dox +++ b/doc/device_invocation/group_basic.dox @@ -67,7 +67,7 @@ Here is a list of properties checked when starting up a device based on this con mycam.t2Tangential distortion of the lens(fake)12.0 mycam.freqrate of test images in Hz mycam.periodperiod of test images in seconds -mycam.modebouncy [ball], scrolly [line], grid [grid], grid multisize [size], random [rand], none [none]line +mycam.modebouncy [ball], scrolly [line], grid [grid], grid multisize [size], random [rand], none [none], time test[time]line mycam.src mycam.bayershould emit bayer test image? mycam.monoshould emit a monochrome image? diff --git a/doc/device_invocation/test_grabber_basic.dox b/doc/device_invocation/test_grabber_basic.dox index de73f94362..ff09951a19 100644 --- a/doc/device_invocation/test_grabber_basic.dox +++ b/doc/device_invocation/test_grabber_basic.dox @@ -47,7 +47,7 @@ Here is a list of properties checked when starting up a device based on this con t2Tangential distortion of the lens(fake)12.0 freqrate of test images in Hz periodperiod of test images in seconds -modebouncy [ball], scrolly [line], grid [grid], grid multisize [size], random [rand], none [none]line +modebouncy [ball], scrolly [line], grid [grid], grid multisize [size], random [rand], none [none], time test[time]line src bayershould emit bayer test image? monoshould emit a monochrome image? diff --git a/doc/release/v3_1_0.md b/doc/release/v3_1_0.md new file mode 100644 index 0000000000..54a3cf76ce --- /dev/null +++ b/doc/release/v3_1_0.md @@ -0,0 +1,126 @@ +YARP 3.1.0 (UNRELEASED) Release Notes {#v3_1_0} +===================================== + + +A (partial) list of bug fixed and issues resolved in this release can be found +[here](https://github.com/robotology/yarp/issues?q=label%3A%22Fixed+in%3A+YARP+v3.1.0%22). + +New Features +------------ + +### Libraries + +#### YARP_OS + +* `Vocab` is now a namespace, and `yarp::os::createVocab` replaces `VOCAB1\2\3\4` + that are now deprecated. + +#### YARP_DEV + +* The following files have been renamed: + * `yarp/dev/ImplementControlLimits.h` + (from `yarp/dev/IControlLimitsImpl.h`) + * `yarp/dev/ImplementControlLimits2.h` + (from `yarp/dev/IControlLimits2Impl.h`) + * `yarp/dev/ImplementInteractionMode.h` + (from `yarp/dev/IInteractionModeImpl.h`) + * `yarp/dev/ImplementPidControl.h` + (from `yarp/dev/IPidControlImpl.h`) + * `yarp/dev/ImplementPositionControl.h` + (from `yarp/dev/IPositionControlImpl.h`) + * `yarp/dev/ImplementPositionControl2.h` + (from `yarp/dev/IPositionControl2Impl.h`) + * `yarp/dev/ImplementPositionDirect.h` + (from `yarp/dev/IPositionDirectImpl.h`) + * `yarp/dev/ImplementVelocityControl.h` + (from `yarp/dev/IVelocityControlImpl.h`) + * `yarp/dev/ImplementVelocityControl2.h` + (from `yarp/dev/IVelocityControl2Impl.h`) + * `yarp/dev/ImplementVirtualAnalogSensor.h` + (from `yarp/dev/IVirtualAnalogSensorImpl.h`) +* The following classes have been refactored. + * These classes are no more template classes: + * ImplementControlCalibration + * ImplementAmplifierControl + * ImplementEncoders + * Class definitions were moved from + `yarp/dev/ImplementControlBoardInterfaces.h` to: + * `yarp/dev/ImplementControlCalibration.h` + * `yarp/dev/ImplementAmplifierControl.h` + * `yarp/dev/ImplementEncoders.h` + * File `yarp/dev/ControlBoardInterfacesImpl-inl.h` has been removed. +* Class `IControlCalibration` refactored. + The following methods were renamed: + * `done(int j)` -> `calibrationDone(int j)` + * `doneRaw(int j)` -> `calibrationDoneRaw(int j)` + * `calibrate()` -> `calibrateRobot()` +* Class `IControlCalibrationRaw` refactored. + The following methods were renamed: + * `calibrateRaw(int, unsigned int, double, double, double)` -> + `calibrateAxisWithParamsRaw(int, unsigned int, double, double, double)` + * `calibrate(int, unsigned int, double, double, double)` -> + `calibrateAxisWithParams(int, unsigned int, double, double, double)` +* The file `yarp/dev/ControlBoardInterfaces.h` has been split into multiple + files: + * `yarp/dev/IAmplifierControl.h` + * `yarp/dev/IAxisInfo.h` + * `yarp/dev/IControlDebug.h` + * `yarp/dev/IControlLimits.h` + + +#### YARP_sig + +* `Vector` made typedef of `VectorOf`(#1598). + +### Devices + +#### imuBosch_BNO055 + +* Added support for i2c. + +#### yarp_test_grabber + +* Added test [time] which draws a text timestamp directly on the image. + +### Tools + +#### yarp + +* Added `yarp name runners` command to get a list of the ports offering + `yarprun` utilities + +### Bindings + +#### IInteractionMode interface bindings + +* Added bindings for the interface IInteractionMode. + The change extends the `yarp::dev::IInteractionMode` and the + `yarp::dev::IImpedanceControl` classes. + +#### Multiple Analog Sensors interfaces bindings + +* Minor refactoring of main header `MultipleAnalogSensorsInterfaces.h` defining + the interfaces: + `IThreeAxisGyroscopes`, `IThreeAxisLinearAccelerometers`, + `IThreeAxisMagnetometers`, `IOrientationSensors`, `ITemperatureSensors`, + `ISixAxisForceTorqueSensors`, `IContactLoadCellArrays`, `IEncoderArrays`, + `ISkinPatches`. +* Extended these interfaces for wrapping the cast functions + `(PolyDriver*)->view()`. +* For this purpose, defined and used a new macro `CAST_POLYDRIVER_TO_INTERFACE`. +* extended the new interfaces for wrapping all other methods which Swig + struggles to handle, using a new Swig macro defined in + `macrosForMultipleAnalogSensors.i` +* refactored the `from/toMatlab()` wrappers and extended them to the classes: + `std::vector`, `std::vector`, `std::vector`, + `yarp::sig::Vector`. + + +Contributors +------------ + +This is a list of people that contributed to this release (generated from the +git history using `git shortlog -ens --no-merges v3.0.0..v3.1.0`): + +``` +``` diff --git a/doc/releases.dox b/doc/releases.dox index 2d16f03fba..c1f43a7c2c 100644 --- a/doc/releases.dox +++ b/doc/releases.dox @@ -4,6 +4,8 @@ This page lists the main changes introduced in YARP at each release. +

YARP 3.1 Series

+\li \subpage v3_1_0

YARP 3.0 Series

\li \subpage v3_0_1 diff --git a/example/dev/simple_motor_client.cpp b/example/dev/simple_motor_client.cpp index aa0ad9e9fd..6f6a592ab3 100644 --- a/example/dev/simple_motor_client.cpp +++ b/example/dev/simple_motor_client.cpp @@ -52,6 +52,7 @@ #include #include +#include #include @@ -59,8 +60,6 @@ using namespace yarp::dev; using namespace yarp::os; using namespace yarp; -#define VOCAB_HELP VOCAB4('h','e','l','p') -#define VOCAB_QUIT VOCAB4('q','u','i','t') // int main(int argc, char *argv[]) diff --git a/example/os/database.cpp b/example/os/database.cpp index 5ff20118fb..0504d20136 100644 --- a/example/os/database.cpp +++ b/example/os/database.cpp @@ -9,14 +9,10 @@ #include #include +#include using namespace yarp::os; -#define VOCAB_SET VOCAB3('s','e','t') -#define VOCAB_GET VOCAB3('g','e','t') -#define VOCAB_NOT VOCAB3('n','o','t') -#define VOCAB_IS VOCAB2('i','s') -#define VOCAB_REMOVE VOCAB2('r','m') int main(int argc, char *argv[]) { diff --git a/example/os/queue_manager.cpp b/example/os/queue_manager.cpp index 3d9571764f..a63adcd903 100644 --- a/example/os/queue_manager.cpp +++ b/example/os/queue_manager.cpp @@ -105,7 +105,7 @@ class QueueManager : public DeviceResponder { yarp::os::Bottle& reply) { mutex.lock(); switch (command.get(0).asVocab()) { - case VOCAB3('a','d','d'): + case yarp::os::createVocab('a','d','d'): { string name = command.get(1).asString().c_str(); if (name!="") { @@ -120,7 +120,7 @@ class QueueManager : public DeviceResponder { } } break; - case VOCAB3('d','e','l'): + case yarp::os::createVocab('d','e','l'): { if (command.get(1).isInt32()) { int idx = command.get(1).asInt32(); @@ -153,7 +153,7 @@ class QueueManager : public DeviceResponder { } } break; - case VOCAB4('l','i','s','t'): + case yarp::os::createVocab('l','i','s','t'): { reply.clear(); addQueue(reply); diff --git a/example/remote_check/main.cpp b/example/remote_check/main.cpp index 077f2de235..2de7fded95 100644 --- a/example/remote_check/main.cpp +++ b/example/remote_check/main.cpp @@ -38,13 +38,13 @@ class FakeFrameGrabber : public Thread, PortReader { printf("command received: %s\n", cmd.toString().c_str()); int code = cmd.get(0).asVocab(); switch (code) { - case VOCAB3('s','e','t'): + case yarp::os::createVocab('s','e','t'): printf("set command received\n"); prop.put(cmd.get(1).asString().c_str(),cmd.get(2)); break; - case VOCAB3('g','e','t'): + case yarp::os::createVocab('g','e','t'): printf("get command received\n"); - response.addVocab(VOCAB2('i','s')); + response.addVocab(yarp::os::createVocab('i','s')); response.add(cmd.get(1)); response.add(prop.find(cmd.get(1).asString().c_str())); break; diff --git a/example/stressTests/testcase-client-server/common/vocabs.h b/example/stressTests/testcase-client-server/common/vocabs.h index b66910f92e..9123b2fe7f 100644 --- a/example/stressTests/testcase-client-server/common/vocabs.h +++ b/example/stressTests/testcase-client-server/common/vocabs.h @@ -12,8 +12,8 @@ #include -#define COLLATZ_VOCAB_REQ_ITEM VOCAB3('r','e','q') -#define COLLATZ_VOCAB_ITEM VOCAB4('i','t','e','m') +#define COLLATZ_VOCAB_REQ_ITEM yarp::os::createVocab('r','e','q') +#define COLLATZ_VOCAB_ITEM yarp::os::createVocab('i','t','e','m') #define COLLATZ_EMPTY_FIELD 0 diff --git a/example/yarpros_examples/src/waggler.cpp b/example/yarpros_examples/src/waggler.cpp index c539fb0f8b..0ef8599dae 100644 --- a/example/yarpros_examples/src/waggler.cpp +++ b/example/yarpros_examples/src/waggler.cpp @@ -16,7 +16,7 @@ #define BOTTLE_TAG_VOCAB (1+8) #define BOTTLE_TAG_FLOAT64 (2+8) #define BOTTLE_TAG_LIST 256 -#define VOCAB(a,b,c,d) ((((int)(d))<<24)+(((int)(c))<<16)+(((int)(b))<<8)+((int)(a))) +#define yarp::os::createVocab(a,b,c,d) ((((int)(d))<<24)+(((int)(c))<<16)+(((int)(b))<<8)+((int)(a))) // YARP defines over int main(int argc, char** argv) { @@ -38,9 +38,9 @@ int main(int argc, char** argv) { msg.list_tag = BOTTLE_TAG_LIST; msg.list_len = 3; msg.vocab1_tag = BOTTLE_TAG_VOCAB; - msg.vocab1_val = VOCAB('s','e','t',0); + msg.vocab1_val = yarp::os::createVocab('s','e','t',0); msg.vocab2_tag = BOTTLE_TAG_VOCAB; - msg.vocab2_val = VOCAB('p','o','s','s'); + msg.vocab2_val = yarp::os::createVocab('p','o','s','s'); msg.setpoints_tag = BOTTLE_TAG_LIST+BOTTLE_TAG_FLOAT64; msg.setpoints.resize(joint_count); for (int i=0; i(this), - ImplementAmplifierControl(this), + ImplementControlCalibration(this), + ImplementAmplifierControl(this), ImplementPidControl(this), ImplementEncodersTimed(this), ImplementPositionControl(this), @@ -559,8 +559,8 @@ bool FakeMotionControl::open(yarp::os::Searchable &config) ControlBoardHelper cb(_njoints, _axisMap, _angleToEncoder, nullptr, _newtonsToSensor, _ampsToSensor, _dutycycleToPWM); ControlBoardHelper cb_copy_test(cb); - ImplementControlCalibration::initialize(_njoints, _axisMap, _angleToEncoder, nullptr); - ImplementAmplifierControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr); + ImplementControlCalibration::initialize(_njoints, _axisMap, _angleToEncoder, nullptr); + ImplementAmplifierControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr); ImplementEncodersTimed::initialize(_njoints, _axisMap, _angleToEncoder, nullptr); ImplementMotorEncoders::initialize(_njoints, _axisMap, _angleToEncoder, nullptr); ImplementPositionControl::initialize(_njoints, _axisMap, _angleToEncoder, nullptr); @@ -1435,8 +1435,8 @@ bool FakeMotionControl::close() ImplementPositionControl::uninitialize(); ImplementVelocityControl::uninitialize(); ImplementPidControl::uninitialize(); - ImplementControlCalibration::uninitialize(); - ImplementAmplifierControl::uninitialize(); + ImplementControlCalibration::uninitialize(); + ImplementAmplifierControl::uninitialize(); ImplementImpedanceControl::uninitialize(); ImplementControlLimits::uninitialize(); ImplementTorqueControl::uninitialize(); @@ -1858,13 +1858,13 @@ bool FakeMotionControl::setCalibrationParametersRaw(int j, const CalibrationPara return true; } -bool FakeMotionControl::calibrateRaw(int j, unsigned int type, double p1, double p2, double p3) +bool FakeMotionControl::calibrateAxisWithParamsRaw(int j, unsigned int type, double p1, double p2, double p3) { yTrace() << "calibrateRaw for joint" << j; return true; } -bool FakeMotionControl::doneRaw(int axis) +bool FakeMotionControl::calibrationDoneRaw(int axis) { bool result = false; diff --git a/src/devices/fakeMotionControl/fakeMotionControl.h b/src/devices/fakeMotionControl/fakeMotionControl.h index 455ead32e9..84318a9c37 100644 --- a/src/devices/fakeMotionControl/fakeMotionControl.h +++ b/src/devices/fakeMotionControl/fakeMotionControl.h @@ -27,9 +27,8 @@ #include #include #include -#include #include -#include +#include namespace yarp { namespace dev { @@ -106,8 +105,8 @@ class yarp::dev::FakeMotionControl : public DeviceDriver, public IInteractionModeRaw, public IAxisInfoRaw, public IVirtualAnalogSensorRaw, //* - public ImplementControlCalibration, - public ImplementAmplifierControl, + public ImplementControlCalibration, + public ImplementAmplifierControl, public ImplementPidControl, public ImplementEncodersTimed, public ImplementPositionControl, @@ -129,7 +128,7 @@ class yarp::dev::FakeMotionControl : public DeviceDriver, enum VerboseLevel { MUTE = 0, // only errors that prevent device from working - QUIET = 1, // adds errors that can cause misfunctioning + QUIET = 1, // adds errors that can cause malfunctioning DEFAULT = 2, // adds warnings // DEFAULT // show noisy messages about back-compatible changes CHATTY = 3, // adds info messages VERBOSE = 4, // adds debug messages @@ -178,7 +177,7 @@ class yarp::dev::FakeMotionControl : public DeviceDriver, std::string *_axisName; /** axis name */ JointTypeEnum *_jointType; /** axis type */ -// ImpedanceLimits *_impedance_limits; /** impedancel imits */ +// ImpedanceLimits *_impedance_limits; /** impedance limits */ double *_limitsMin; /** joint limits, max*/ double *_limitsMax; /** joint limits, min*/ double *_kinematic_mj; /** the kinematic coupling matrix from joints space to motor space */ @@ -323,9 +322,8 @@ class yarp::dev::FakeMotionControl : public DeviceDriver, // calibration2raw virtual bool setCalibrationParametersRaw(int axis, const CalibrationParameters& params) override; - using yarp::dev::IControlCalibrationRaw::calibrateRaw; - virtual bool calibrateRaw(int axis, unsigned int type, double p1, double p2, double p3) override; - virtual bool doneRaw(int j) override; + virtual bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3) override; + virtual bool calibrationDoneRaw(int j) override; /////////////////////////////// END Position Control INTERFACE diff --git a/src/devices/fakebot/FakeBot.h b/src/devices/fakebot/FakeBot.h index b52706c5c1..af52babacf 100644 --- a/src/devices/fakebot/FakeBot.h +++ b/src/devices/fakebot/FakeBot.h @@ -392,13 +392,13 @@ class yarp::dev::FakeBot : public DeviceDriver, return true; } - virtual bool calibrate(int j, unsigned int iv, double v1, double v2, double v3) override + virtual bool calibrateAxisWithParams(int j, unsigned int iv, double v1, double v2, double v3) override { fprintf(stderr, "FakeBot: calibrating joint %d with parameters %u %lf %lf %lf\n", j, iv, v1, v2, v3); return true; } - virtual bool done(int j) override + virtual bool calibrationDone(int j) override { fprintf(stderr , "FakeBot: calibration done on joint %d.\n", j); return true; diff --git a/src/devices/imuBosch_BNO055/CMakeLists.txt b/src/devices/imuBosch_BNO055/CMakeLists.txt index 534d5f4ab6..b0bd008bf9 100644 --- a/src/devices/imuBosch_BNO055/CMakeLists.txt +++ b/src/devices/imuBosch_BNO055/CMakeLists.txt @@ -9,7 +9,7 @@ yarp_prepare_plugin(imuBosch_BNO055 TYPE yarp::dev::BoschIMU INCLUDE imuBosch_BNO055.h EXTRA_CONFIG WRAPPER=inertial - DEPENDS "NOT WIN32;YARP_HAS_MATH_LIB") + DEPENDS "NOT WIN32;YARP_HAS_MATH_LIB;YARP_HAS_I2C") if(ENABLE_imuBosch_BNO055) set(CMAKE_INCLUDE_CURRENT_DIR ON) @@ -24,6 +24,11 @@ if(ENABLE_imuBosch_BNO055) YARP_dev YARP_math) + target_compile_definitions(yarp_imuBosch_BNO055 PRIVATE ${I2C_DEFINITIONS}) + target_include_directories(yarp_imuBosch_BNO055 SYSTEM PRIVATE ${I2C_INCLUDE_DIRS}) + target_link_libraries(yarp_imuBosch_BNO055 PRIVATE ${I2C_LIBRARIES}) +# list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS I2C) (not using targets) + yarp_install(TARGETS yarp_imuBosch_BNO055 EXPORT YARP_${YARP_PLUGIN_MASTER} COMPONENT ${YARP_PLUGIN_MASTER} diff --git a/src/devices/imuBosch_BNO055/imuBosch_BNO055.cpp b/src/devices/imuBosch_BNO055/imuBosch_BNO055.cpp index e0dcb9e367..5698ba8592 100644 --- a/src/devices/imuBosch_BNO055/imuBosch_BNO055.cpp +++ b/src/devices/imuBosch_BNO055/imuBosch_BNO055.cpp @@ -16,21 +16,31 @@ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA */ +#include +#include // Error number definitions +#include #include -#include -#include // terminal io (serial port) interface +#include #include // File control definitions -#include // Error number definitions -#include #include -#include -#include +#include // terminal io (serial port) interface +#include -#include -#include +#include +#ifdef I2C_HAS_SMBUS_H +# include +#endif +#include + +#include +#include +#include + +#include #include +#include #include -#include +#include #include "imuBosch_BNO055.h" @@ -38,16 +48,21 @@ using namespace std; using namespace yarp::os; using namespace yarp::dev; +constexpr uint8_t i2cAddrA = 0x28; +//constexpr uint8_t i2cAddrB = 0x29; + BoschIMU::BoschIMU() : PeriodicThread(0.02), verbose(false), status(0), nChannels(12), timeStamp(0.0), timeLastReport(0.0), + i2c_flag(false), checkError(false), - fd_ser(0), + fd(0), + readFunc(&BoschIMU::sendReadCommandSer), totMessagesRead(0), - errs({0,0,0,0}) + errs(0) { data.resize(12); data.zero(); @@ -65,69 +80,103 @@ bool BoschIMU::open(yarp::os::Searchable& config) //debug yTrace("Parameters are:\n\t%s", config.toString().c_str()); - if(!config.check("comport")) + if(!config.check("comport") && !config.check("i2c")) { - yError() << "Param 'comport' not found"; + yError() << "BoshImu: Params 'comport' and 'i2c' not found"; return false; } - double period = config.check("period",Value(10),"Thread period in ms").asInt32() / 1000.0; - setPeriod(period); + if (config.check("comport") && config.check("i2c")) + { + yError() << "BoshImu: Params 'comport' and 'i2c' both specified"; + return false; + } - nChannels = config.check("channels", Value(12)).asInt32(); + i2c_flag = config.check("i2c"); + + readFunc = i2c_flag ? &BoschIMU::sendReadCommandI2c : &BoschIMU::sendReadCommandSer; + + if (i2c_flag) + { + if (!config.find("i2c").isString()) + { + yError()<<"BoshImu: i2c param malformed, it should be a string, aborting."; + return false; + } + + std::string i2cDevFile = config.find("i2c").asString(); + fd = ::open(i2cDevFile.c_str(), O_RDWR); + + if (fd < 0) + { + yError("BoshImu: can't open %s, %s", i2cDevFile.c_str(), strerror(errno)); + return false; + } + + if (::ioctl(fd, I2C_SLAVE, i2cAddrA) < 0) + { + yError("BoshImu: ioctl failed on %s, %s", i2cDevFile.c_str(), strerror(errno)); + return false; + } - fd_ser = ::open(config.find("comport").toString().c_str(), O_RDWR | O_NOCTTY ); - if (fd_ser < 0) { - yError("can't open %s, %s", config.find("comport").toString().c_str(), strerror(errno)); - return false; } + else + { + fd = ::open(config.find("comport").toString().c_str(), O_RDWR | O_NOCTTY ); + if (fd < 0) { + yError("can't open %s, %s", config.find("comport").toString().c_str(), strerror(errno)); + return false; + } + //Get the current options for the port... + struct termios options; + tcgetattr(fd, &options); - //Get the current options for the port... - struct termios options; - tcgetattr(fd_ser, &options); + cfmakeraw(&options); - cfmakeraw(&options); + //set the baud rate to 115200 + int baudRate = B115200; + cfsetospeed(&options, baudRate); + cfsetispeed(&options, baudRate); - //set the baud rate to 115200 - int baudRate = B115200; - cfsetospeed(&options, baudRate); - cfsetispeed(&options, baudRate); + //set the number of data bits. + options.c_cflag &= ~CSIZE; // Mask the character size bits + options.c_cflag |= CS8; - //set the number of data bits. - options.c_cflag &= ~CSIZE; // Mask the character size bits - options.c_cflag |= CS8; + //set the number of stop bits to 1 + options.c_cflag &= ~CSTOPB; - //set the number of stop bits to 1 - options.c_cflag &= ~CSTOPB; + //Set parity to None + options.c_cflag &=~PARENB; - //Set parity to None - options.c_cflag &=~PARENB; + //set for non-canonical (raw processing, no echo, etc.) + // options.c_iflag = IGNPAR; // ignore parity check + options.c_oflag = 0; // raw output + options.c_lflag = 0; // raw input - //set for non-canonical (raw processing, no echo, etc.) -// options.c_iflag = IGNPAR; // ignore parity check - options.c_oflag = 0; // raw output - options.c_lflag = 0; // raw input + // SET NOT BLOCKING READ + options.c_cc[VMIN] = 0; // block reading until RX x characters. If x = 0, it is non-blocking. + options.c_cc[VTIME] = 2; // Inter-Character Timer -- i.e. timeout= x*.1 s - // SET NOT BLOCKING READ - options.c_cc[VMIN] = 0; // block reading until RX x characters. If x = 0, it is non-blocking. - options.c_cc[VTIME] = 2; // Inter-Character Timer -- i.e. timeout= x*.1 s + //Set local mode and enable the receiver + options.c_cflag |= (CLOCAL | CREAD); - //Set local mode and enable the receiver - options.c_cflag |= (CLOCAL | CREAD); + tcflush(fd, TCIOFLUSH); - tcflush(fd_ser, TCIOFLUSH); + //Set the new options for the port... + if ( tcsetattr(fd, TCSANOW, &options) != 0) + { + yError("Configuring comport failed"); + return false; + } - //Set the new options for the port... - if ( tcsetattr(fd_ser, TCSANOW, &options) != 0) - { - yError("Configuring comport failed"); - return false; } - if(!PeriodicThread::start()) - return false; + nChannels = config.check("channels", Value(12)).asInt32(); - return true; + double period = config.check("period",Value(10),"Thread period in ms").asInt32() / 1000.0; + setPeriod(period); + + return PeriodicThread::start(); } bool BoschIMU::close() @@ -191,7 +240,7 @@ bool BoschIMU::checkWriteResponse(unsigned char* response) return false; } -bool BoschIMU::sendReadCommand(unsigned char register_add, int len, unsigned char* buf, std::string comment) +bool BoschIMU::sendReadCommandSer(unsigned char register_add, int len, unsigned char* buf, std::string comment) { int command_len; int nbytes_w; @@ -213,7 +262,7 @@ bool BoschIMU::sendReadCommand(unsigned char register_add, int len, unsigned cha // printf("\nCommand is:\n"); // printBuffer(command, command_len); - nbytes_w = ::write(fd_ser, (void*)command, command_len); + nbytes_w = ::write(fd, (void*)command, command_len); if(nbytes_w != command_len) { @@ -251,7 +300,7 @@ bool BoschIMU::sendReadCommand(unsigned char register_add, int len, unsigned cha return success; } -bool BoschIMU::sendWriteCommand(unsigned char register_add, int len, unsigned char* cmd, std::string comment) +bool BoschIMU::sendWriteCommandSer(unsigned char register_add, int len, unsigned char* cmd, std::string comment) { int command_len = 4+len; int nbytes_w; @@ -267,7 +316,7 @@ bool BoschIMU::sendWriteCommand(unsigned char register_add, int len, unsigned ch // printf("\nCommand is:\n"); // printBuffer(command, command_len); - nbytes_w = ::write(fd_ser, (void*)command, command_len); + nbytes_w = ::write(fd, (void*)command, command_len); if(nbytes_w != command_len) { yError() << "BoschIMU device cannot correctly send the message: " << comment; @@ -297,7 +346,7 @@ int BoschIMU::readBytes(unsigned char* buffer, int bytes) int bytesRead = 0; do { - r = ::read(fd_ser, (void*)&buffer[bytesRead], 1); + r = ::read(fd, (void*)&buffer[bytesRead], 1); if(r > 0) bytesRead += r; } @@ -309,7 +358,7 @@ int BoschIMU::readBytes(unsigned char* buffer, int bytes) void BoschIMU::dropGarbage() { char byte; - while( (::read(fd_ser, (void*) &byte, 1) > 0 )) + while( (::read(fd, (void*) &byte, 1) > 0 )) { // printf("Dropping byte 0x%02X \n", byte); } @@ -331,12 +380,12 @@ void BoschIMU::readSysError() checkError = true; yarp::os::SystemClock::delaySystem(0.002); - if(!sendReadCommand(REG_SYS_STATUS, 1, response, "Read SYS_STATUS register") ) + if(!sendReadCommandSer(REG_SYS_STATUS, 1, response, "Read SYS_STATUS register") ) { yError() << "@ line " << __LINE__; } - if(!sendReadCommand(REG_SYS_ERR, 1, response, "Read SYS_ERR register") ) + if(!sendReadCommandSer(REG_SYS_ERR, 1, response, "Read SYS_ERR register") ) { yError() << "@ line " << __LINE__; } @@ -344,227 +393,233 @@ void BoschIMU::readSysError() return; } -bool BoschIMU::sendAndVerifyCommand(unsigned char register_add, int len, unsigned char* cmd, std::string comment) +bool BoschIMU::sendAndVerifyCommandSer(unsigned char register_add, int len, unsigned char* cmd, std::string comment) { uint8_t attempts=0; bool ret; do { - ret=sendWriteCommand(register_add, len, cmd, comment); + ret=sendWriteCommandSer(register_add, len, cmd, comment); attempts++; }while((attempts<= ATTEMPTS_NUM_OF_SEND_CONFIG_CMD) && (ret==false)); return(ret); } -bool BoschIMU::threadInit() +bool BoschIMU::sendReadCommandI2c(unsigned char register_add, int len, unsigned char* buf, std::string comment) { - unsigned char msg; - timeLastReport = yarp::os::SystemClock::nowSystem(); - - msg = 0x00; - if(!sendAndVerifyCommand(REG_PAGE_ID, 1, &msg, "PAGE_ID") ) + if (i2c_smbus_read_i2c_block_data(fd, register_add, len, buf) < 0) { - yError() << "BoschIMU: set page id 0 failed"; - return(false); + yError() << "BoschIMU device cannot correctly send the message: " << comment; + return false; } + return true; +} - yarp::os::SystemClock::delaySystem(SWITCHING_TIME); +bool BoschIMU::threadInit() +{ + if (i2c_flag) + { + int trials = 0; + // Make sure we have the right device + while (i2c_smbus_read_byte_data(fd, REG_CHIP_ID) != BNO055_ID) + { + if (trials == 10) + { + yError()<<"BoshImu: wrong device on the bus, it is not BNO055"; + return false; + } + yarp::os::Time::delay(0.1); + trials++; -//Removed because useless - /////////////////////////////////////// - // - // Set power mode - // - /////////////////////////////////////// -// msg = 0x00; -// if(!sendAndVerifyCommand(REG_POWER_MODE, 1, &msg, "Set power mode") ) -// { -// yError() << "BoschIMU: set power mode failed"; -// return(false); -// } -// -// yarp::os::SystemClock::delaySystem(SWITCHING_TIME); - - - /////////////////////////////////////// - // - // Set the device in config mode - // - /////////////////////////////////////// + } - msg = CONFIG_MODE; - if(!sendAndVerifyCommand(REG_OP_MODE, 1, &msg, "Set config mode") ) - { - yError() << "BoschIMU: set config mode failed"; - return(false); - } + yarp::os::SystemClock::delaySystem(SWITCHING_TIME); - yarp::os::SystemClock::delaySystem(SWITCHING_TIME); + // Set the device in config mode + if (i2c_smbus_write_byte_data(fd, REG_OP_MODE, CONFIG_MODE) < 0) + { + yError()<<"BoshImu: Unable to set the Config mode"; + return false; + } + + yarp::os::SystemClock::delaySystem(SWITCHING_TIME); + + if (i2c_smbus_write_byte_data(fd, REG_SYS_TRIGGER, TRIG_EXT_CLK_SEL) < 0) + { + yError()<<"BoshImu: Unable to set external clock"; + return false; + } + + yarp::os::SystemClock::delaySystem(SWITCHING_TIME); + + // Perform any required configuration + + + if (i2c_smbus_write_byte_data(fd, REG_PAGE_ID, 0x00) < 0) + { + yError()<<"BoshImu: Unable to set the page ID"; + return false; + } + + yarp::os::SystemClock::delaySystem(SWITCHING_TIME); + // Set the device into operative mode + + if (i2c_smbus_write_byte_data(fd, REG_OP_MODE, NDOF_MODE) < 0) + { + yError()<<"BoshImu: Unable to set the Operative mode"; + return false; + } + + yarp::os::SystemClock::delaySystem(SWITCHING_TIME); + return true; - /////////////////////////////////////// - // - // Set external clock - // - /////////////////////////////////////// - msg = TRIG_EXT_CLK_SEL; - if(!sendAndVerifyCommand(REG_SYS_TRIGGER, 1, &msg, "Set external clock") ) - { - yError() << "BoschIMU: set external clock failed"; - return(false); } - yarp::os::SystemClock::delaySystem(SWITCHING_TIME); + else + { + unsigned char msg; + timeLastReport = yarp::os::SystemClock::nowSystem(); - /////////////////////////////////////// - // - // Perform any required configuration + msg = 0x00; + if(!sendAndVerifyCommandSer(REG_PAGE_ID, 1, &msg, "PAGE_ID") ) + { + yError() << "BoschIMU: set page id 0 failed"; + return(false); + } + + yarp::os::SystemClock::delaySystem(SWITCHING_TIME); + + //Removed because useless + /////////////////////////////////////// + // + // Set power mode + // + /////////////////////////////////////// + // msg = 0x00; + // if(!sendAndVerifyCommand(REG_POWER_MODE, 1, &msg, "Set power mode") ) + // { + // yError() << "BoschIMU: set power mode failed"; + // return(false); + // } // - /////////////////////////////////////// + // yarp::os::SystemClock::delaySystem(SWITCHING_TIME); + /////////////////////////////////////// + // + // Set the device in config mode + // + /////////////////////////////////////// - /// TODO: meas units, offset and so on ... + msg = CONFIG_MODE; + if(!sendAndVerifyCommandSer(REG_OP_MODE, 1, &msg, "Set config mode") ) + { + yError() << "BoschIMU: set config mode failed"; + return(false); + } - /////////////////////////////////////// - // - // Set the device into operative mode - // - /////////////////////////////////////// + yarp::os::SystemClock::delaySystem(SWITCHING_TIME); - msg = NDOF_MODE; - if(!sendAndVerifyCommand(REG_OP_MODE, 1, &msg, "Set config NDOF_MODE") ) - { - yError() << "BoschIMU: set config NDOF_MODE failed"; - } - yarp::os::SystemClock::delaySystem(SWITCHING_TIME); + /////////////////////////////////////// + // + // Set external clock + // + /////////////////////////////////////// - return true; -} + msg = TRIG_EXT_CLK_SEL; + if(!sendAndVerifyCommandSer(REG_SYS_TRIGGER, 1, &msg, "Set external clock") ) + { + yError() << "BoschIMU: set external clock failed"; + return(false); + } + yarp::os::SystemClock::delaySystem(SWITCHING_TIME); -void BoschIMU::run() -{ - timeStamp = yarp::os::SystemClock::nowSystem(); + /////////////////////////////////////// + // + // Perform any required configuration + // + /////////////////////////////////////// - int16_t raw_data[4]; -// void *tmp = (void*) &response[2]; -// raw_data = static_cast (tmp); - // In order to avoid zeros when a single read from a sensor is missing, - // initialize the new measure to be equal to the previous one - data_tmp = data; - /////////////////////////////////////////// - // - // Read RPY values --> no!! There are bad spikes on yaw values!! - // Read quaternion and convert back to RPY afterwards - // - /////////////////////////////////////////// + /// TODO: meas units, offset and so on ... -// if(!sendReadCommand(REG_RPY_DATA, 6, response, "Read RPY ... ") ) -// { -// yError() << "@ line " << __LINE__; -// } + /////////////////////////////////////// + // + // Set the device into operative mode + // + /////////////////////////////////////// -// data[2] = (double) raw_data[0]/16.0; -// data[0] = (double) raw_data[1]/16.0; -// data[1] = (double) raw_data[2]/16.0; -// yDebug() << "RPY x: " << data[0] << "y: " << data[1] << "z: " << data[2]; + msg = NDOF_MODE; + if(!sendAndVerifyCommandSer(REG_OP_MODE, 1, &msg, "Set config NDOF_MODE") ) + { + yError() << "BoschIMU: set config NDOF_MODE failed"; + return false; + } - /////////////////////////////////////////// - // - // Read accel values - // - /////////////////////////////////////////// + yarp::os::SystemClock::delaySystem(SWITCHING_TIME); - if (sendReadCommand(REG_ACC_DATA, 6, response, "Read accelerations")) { - // Manually compose the data to safely handling endianness - raw_data[0] = response[3] << 8 | response[2]; - raw_data[1] = response[5] << 8 | response[4]; - raw_data[2] = response[7] << 8 | response[6]; - data_tmp[3] = (double)raw_data[0] / 100.0; - data_tmp[4] = (double)raw_data[1] / 100.0; - data_tmp[5] = (double)raw_data[2] / 100.0; - } - else { - errs.acceError++; + return true; } - /////////////////////////////////////////// - // - // Read Gyro values - // - /////////////////////////////////////////// - - if (sendReadCommand(REG_GYRO_DATA, 6, response, "Read Gyros")) { - // Manually compose the data to handle endianness safely - raw_data[0] = response[3] << 8 | response[2]; - raw_data[1] = response[5] << 8 | response[4]; - raw_data[2] = response[7] << 8 | response[6]; - data_tmp[6] = (double)raw_data[0] / 16.0; - data_tmp[7] = (double)raw_data[1] / 16.0; - data_tmp[8] = (double)raw_data[2] / 16.0; - // yDebug() << "Gyro x: " << data[6] << "y: " << data[7] << "z: " << data[8]; - } - else { - errs.gyroError++; - } +} - /////////////////////////////////////////// - // - // Read Magnetometer values - // - /////////////////////////////////////////// - - if (sendReadCommand(REG_MAGN_DATA, 6, response, "Read Magnetometer")) { - // Manually compose the data to safely handling endianness - raw_data[0] = response[3] << 8 | response[2]; - raw_data[1] = response[5] << 8 | response[4]; - raw_data[2] = response[7] << 8 | response[6]; - data_tmp[9] = (double)raw_data[0] / 16.0; - data_tmp[10] = (double)raw_data[1] / 16.0; - data_tmp[11] = (double)raw_data[2] / 16.0; - // yDebug() << "Magn x: " << data[9] << "y: " << data[10] << "z: " << data[11]; - } - else { - errs.magnError++; - } +void BoschIMU::run() +{ + timeStamp = yarp::os::SystemClock::nowSystem(); - /////////////////////////////////////////// - // - // Read Quaternion - // - /////////////////////////////////////////// + int16_t raw_data[16]; - quaternion_tmp = quaternion; - if (sendReadCommand(REG_QUATERN_DATA, 8, response, "Read quaternion")) { - // Manually compose the data to safely handling endianness - raw_data[0] = response[3] << 8 | response[2]; - raw_data[1] = response[5] << 8 | response[4]; - raw_data[2] = response[7] << 8 | response[6]; - raw_data[3] = response[9] << 8 | response[8]; + // In order to avoid zeros when a single read from a sensor is missing, + // initialize the new measure to be equal to the previous one + data_tmp = data; - quaternion_tmp.w() = ((double)raw_data[0]) / (2 << 13); - quaternion_tmp.x() = ((double)raw_data[1]) / (2 << 13); - quaternion_tmp.y() = ((double)raw_data[2]) / (2 << 13); - quaternion_tmp.z() = ((double)raw_data[3]) / (2 << 13); + if (!(this->*readFunc)(REG_ACC_DATA, 32, response, "Read all")) + { + yError()<<"BoshImu: failed to read all the data"; + errs++; + } + else + { + // Correctly construct int16 data + for(int i=0; i<16; i++) + { + raw_data[i] = response[3+ i*2] << 8 | response[2 +i*2]; + } + + // Get quaternion + quaternion_tmp = quaternion; + quaternion_tmp.w() = ((double)raw_data[12]) / (2 << 13); + quaternion_tmp.x() = ((double)raw_data[13]) / (2 << 13); + quaternion_tmp.y() = ((double)raw_data[14]) / (2 << 13); + quaternion_tmp.z() = ((double)raw_data[15]) / (2 << 13); + + // Convert to RPY angles RPY_angle.resize(3); RPY_angle = yarp::math::dcm2rpy(quaternion.toRotationMatrix4x4()); data_tmp[0] = RPY_angle[0] * 180 / M_PI; data_tmp[1] = RPY_angle[1] * 180 / M_PI; data_tmp[2] = RPY_angle[2] * 180 / M_PI; - } - else { - errs.quatError++; - } - // If 100ms have passed since the last received message - if (timeStamp+0.1 < yarp::os::SystemClock::nowSystem()) - { -// status=TIMEOUT; + // Fill in accel values + data_tmp[3] = (double)raw_data[0] / 100.0; + data_tmp[4] = (double)raw_data[1] / 100.0; + data_tmp[5] = (double)raw_data[2] / 100.0; + + + // Fill in Gyro values + data_tmp[6] = (double)raw_data[6] / 100.0; + data_tmp[7] = (double)raw_data[7] / 100.0; + data_tmp[8] = (double)raw_data[8] / 100.0; + + // Fill in Magnetometer values + data_tmp[9] = (double)raw_data[3] / 100.0; + data_tmp[10] = (double)raw_data[4] / 100.0; + data_tmp[11] = (double)raw_data[5] / 100.0; } // Protect only this section in order to avoid slow race conditions when gathering this data @@ -576,19 +631,13 @@ void BoschIMU::run() if (timeStamp > timeLastReport + TIME_REPORT_INTERVAL) { // if almost 1 errors occourred in last interval, then print report - if(errs.acceError + errs.gyroError + errs.magnError + errs.quatError != 0) + if(errs != 0) { yDebug(" IMUBOSCH periodic error report of last %d sec:", TIME_REPORT_INTERVAL); - yDebug("\t errors while reading acceleration: %d", errs.acceError); - yDebug("\t errors while reading gyroscope : %d", errs.gyroError); - yDebug("\t errors while reading magnetometer: %d", errs.magnError); - yDebug("\t errors while reading angles : %d", errs.quatError); + yDebug("\t errors while reading data: %d", errs); } - errs.acceError = 0; - errs.gyroError = 0; - errs.magnError = 0; - errs.quatError = 0; + errs = 0; timeLastReport=timeStamp; } } @@ -609,13 +658,13 @@ bool BoschIMU::read(yarp::sig::Vector &out) } return true; -}; +} bool BoschIMU::getChannels(int *nc) { *nc = nChannels; return true; -}; +} bool BoschIMU::calibrate(int ch, double v) { @@ -624,7 +673,7 @@ bool BoschIMU::calibrate(int ch, double v) // into memory for the next run. // This procedure should be abortable by CTRL+C return false; -}; +} void BoschIMU::threadRelease() { @@ -633,6 +682,6 @@ void BoschIMU::threadRelease() // for(unsigned int i=0; i #include #include +#include using namespace yarp::dev; using namespace yarp::os; diff --git a/src/devices/map2DServer/Map2DServer.cpp b/src/devices/map2DServer/Map2DServer.cpp index 1dead20a9d..3ab9df58e6 100644 --- a/src/devices/map2DServer/Map2DServer.cpp +++ b/src/devices/map2DServer/Map2DServer.cpp @@ -21,6 +21,7 @@ #include "Map2DServer.h" #include #include +#include #include #include #include diff --git a/src/devices/ovrheadset/OVRHeadset.h b/src/devices/ovrheadset/OVRHeadset.h index 21f65ec874..938fd6bde5 100644 --- a/src/devices/ovrheadset/OVRHeadset.h +++ b/src/devices/ovrheadset/OVRHeadset.h @@ -35,6 +35,32 @@ #include #include +/** +* @ingroup dev_impl_other +* +* \section SDLJoypad Description of input parameters +* \brief Device that reads inputs of Joypads compatible with the SDL library. +* +* Parameters accepted in the config argument of the open method: +* | Parameter name | Type | Units | Default Value | Required | Description | Notes | +* |:---------------------:|:------:|:-----:|:-------------:|:---------:|:-----------------------------------------:|:-----:| +* | tfLocal | string | | | yes | local port name receiving and posting tf | | +* | tfRemote | string | | | yes | name of the transformServer port | | +* | tf_left_hand_frame | string | | | Yes | name of the left hand frame | | +* | tf_right_hand_frame | string | | | yes | name of the right hand frame | | +* | tf_root_frame | string | | | yes | name of the root frame | | +* | stick_as_axis | bool | | | yes | if axes shoud be published as sticks | | +* | gui_elements | int | | | yes | number of the gui element to visualize | | + +Gui Groups parameters +* | Parameter name | Type | Units | Default Value | Required | Description | Notes | +* | width | double | pixel | | yes | width of the widget | | +* | height | double | pixel | | yes | height of the widget | | +* | x | double | pixel | | yes | x position of the widget | | +* | y | double | pixel | | yes | y position of the widget | | +* | z | double | pixel | | yes | z position of the widget | | +* | alpha | double | | | yes | alpha value of the widget | | +**/ namespace yarp { namespace os { template class BufferedPort; }} diff --git a/src/devices/test_grabber/TestFrameGrabber.cpp b/src/devices/test_grabber/TestFrameGrabber.cpp index 0c445775b0..a558a43b53 100644 --- a/src/devices/test_grabber/TestFrameGrabber.cpp +++ b/src/devices/test_grabber/TestFrameGrabber.cpp @@ -19,11 +19,13 @@ using namespace yarp::dev; using namespace yarp::sig; using namespace yarp::sig::draw; -#define VOCAB_BALL VOCAB4('b','a','l','l') -#define VOCAB_GRID VOCAB4('g','r','i','d') -#define VOCAB_RAND VOCAB4('r','a','n','d') -#define VOCAB_NONE VOCAB4('n','o','n','e') -#define VOCAB_GRID_MULTISIZE VOCAB4('s','i','z','e') +constexpr yarp::conf::vocab32_t VOCAB_BALL = yarp::os::createVocab('b','a','l','l'); +constexpr yarp::conf::vocab32_t VOCAB_GRID = yarp::os::createVocab('g','r','i','d'); +constexpr yarp::conf::vocab32_t VOCAB_RAND = yarp::os::createVocab('r','a','n','d'); +constexpr yarp::conf::vocab32_t VOCAB_NONE = yarp::os::createVocab('n','o','n','e'); +constexpr yarp::conf::vocab32_t VOCAB_GRID_MULTISIZE = yarp::os::createVocab('s','i','z','e'); +constexpr yarp::conf::vocab32_t VOCAB_TIMETEXT = yarp::os::createVocab('t','i','m','e'); + TestFrameGrabber::TestFrameGrabber() : ct(0), bx(0), @@ -41,7 +43,22 @@ TestFrameGrabber::TestFrameGrabber() : use_bayer(false), use_mono(false), mirror(false) -{} +{ + //the following data are used by [time] test + snprintf(num[0].data, 16, "**** ** ** ****"); + snprintf(num[1].data, 16, " * * * * * "); + snprintf(num[2].data, 16, "*** ***** ***"); + snprintf(num[3].data, 16, "*** **** ****"); + snprintf(num[4].data, 16, "* ** **** * *"); + snprintf(num[5].data, 16, "**** *** ****"); + snprintf(num[6].data, 16, "**** **** ****"); + snprintf(num[7].data, 16, "*** * * * *"); + snprintf(num[8].data, 16, "**** ***** ****"); + snprintf(num[9].data, 16, "**** **** ****"); + snprintf(num[10].data, 16, " "); + snprintf(num[11].data, 16, " ** **"); + start_time = yarp::os::Time::now(); +} bool TestFrameGrabber::close() { @@ -96,7 +113,7 @@ bool TestFrameGrabber::open(yarp::os::Searchable& config) { } mode = config.check("mode", yarp::os::Value(VOCAB_LINE, true), - "bouncy [ball], scrolly [line], grid [grid], grid multisize [size], random [rand], none [none]").asVocab(); + "bouncy [ball], scrolly [line], grid [grid], grid multisize [size], random [rand], none [none], time test[time]").asVocab(); if (config.check("src")) { if (!yarp::sig::file::read(background, @@ -252,6 +269,57 @@ bool TestFrameGrabber::setMode(int feature, FeatureMode mode) { return false; } bool TestFrameGrabber::getMode(int feature, FeatureMode *mode) { return false; } bool TestFrameGrabber::setOnePush(int feature) { return false; } +void TestFrameGrabber::printTime(unsigned char* pixbuf, int pixbuf_w, int pixbuf_h, int x, int y, char* s, int size) +{ + int pixelsize = 5; + for (int i = 0; i= pixbuf_h || + r >= pixbuf_w) + { + //avoid drawing out of the image memory + return; + } + unsigned char *pixel = pixbuf; + size_t offset = c * 3 + r * (pixbuf_w * 3); + pixel = pixel + offset; + pixel[0] = 0; + pixel[1] = 0; + pixel[2] = 255; + } + } + } + } + } +} void TestFrameGrabber::createTestImage(yarp::sig::ImageOf& image) { @@ -267,6 +335,18 @@ void TestFrameGrabber::createTestImage(yarp::sig::ImageOf& image.zero(); } switch (mode) { + case VOCAB_TIMETEXT: + { + char txtbuf[50]; + double time = yarp::os::Time::now() - start_time; + snprintf(txtbuf, 50, "%.3f", time); + int len = strlen(txtbuf); + if (len < 20) + { + printTime((unsigned char*)image.getRawImage(), image.width(), image.height(), 0, 0, txtbuf, len); + } + } + break; case VOCAB_BALL: { addCircle(image,PixelRgb(0,255,0),bx,by,15); diff --git a/src/devices/test_grabber/TestFrameGrabber.h b/src/devices/test_grabber/TestFrameGrabber.h index 54ef5bc8e5..bb529c7981 100644 --- a/src/devices/test_grabber/TestFrameGrabber.h +++ b/src/devices/test_grabber/TestFrameGrabber.h @@ -23,7 +23,7 @@ #include #include -#define VOCAB_LINE VOCAB4('l','i','n','e') +constexpr yarp::conf::vocab32_t VOCAB_LINE = yarp::os::createVocab('l','i','n','e'); namespace yarp { namespace dev { @@ -165,6 +165,13 @@ class yarp::dev::TestFrameGrabber : public DeviceDriver, bool makeSimpleBayer(yarp::sig::ImageOf& src, yarp::sig::ImageOf& bayer); + void printTime(unsigned char* pixbuf, int pixbuf_w, int pixbuf_h, int x, int y, char* s, int size); + struct txtnum_type + { + char data[16]; + }; + txtnum_type num[12]; + double start_time; }; diff --git a/src/idls/CMakeLists.txt b/src/idls/CMakeLists.txt index 972ef5bb4f..8a3e0ebb2d 100644 --- a/src/idls/CMakeLists.txt +++ b/src/idls/CMakeLists.txt @@ -11,5 +11,5 @@ if(NOT CREATE_SHARED_LIBS) include(YarpInstallBasicPackageFiles) yarp_install_basic_package_files(YARP_idl_tools FIRST_TARGET yarpidl_thrift - INCLUDE_CONTENT "include(\${YARP_MODULE_DIR}/YarpIDL.cmake)") + INCLUDE_CONTENT "include(\"\\\${YARP_MODULE_DIR}/YarpIDL.cmake\")") endif() diff --git a/src/idls/thrift/src/t_yarp_generator.cc b/src/idls/thrift/src/t_yarp_generator.cc index 6533d7653f..0cbf9aa3a0 100644 --- a/src/idls/thrift/src/t_yarp_generator.cc +++ b/src/idls/thrift/src/t_yarp_generator.cc @@ -1841,7 +1841,7 @@ void t_yarp_generator::generate_struct(t_struct* tstruct) { indent(out) << "yarp::os::idl::WireWriter writer(reader);" << endl; indent(out) << "if (writer.isNull()) return true;" << endl; indent(out) << "writer.writeListHeader(1);" << endl; - indent(out) << "writer.writeVocab(VOCAB2('o','k'));" << endl; + indent(out) << "writer.writeVocab(yarp::os::createVocab('o','k'));" << endl; indent(out) << "return true;" << endl; scope_down(out); diff --git a/src/libYARP_OS/include/yarp/os/Contact.h b/src/libYARP_OS/include/yarp/os/Contact.h index dbd84535f2..cb1cdf0b37 100644 --- a/src/libYARP_OS/include/yarp/os/Contact.h +++ b/src/libYARP_OS/include/yarp/os/Contact.h @@ -81,7 +81,7 @@ class YARP_OS_API yarp::os::Contact { * * @param rhs the Contact to be moved */ - Contact(Contact&& rhs); + Contact(Contact&& rhs) noexcept; /** * @brief Destructor. @@ -102,7 +102,7 @@ class YARP_OS_API yarp::os::Contact { * @param rhs the Contact to be moved * @return this object */ - Contact& operator=(Contact&& rhs); + Contact& operator=(Contact&& rhs) noexcept; /** @} */ /** @{ */ diff --git a/src/libYARP_OS/include/yarp/os/NestedContact.h b/src/libYARP_OS/include/yarp/os/NestedContact.h index 08138ee119..b2cdbac78c 100644 --- a/src/libYARP_OS/include/yarp/os/NestedContact.h +++ b/src/libYARP_OS/include/yarp/os/NestedContact.h @@ -51,7 +51,7 @@ class YARP_OS_API NestedContact * * @param rhs the NestedContact to be moved */ - NestedContact(NestedContact&& rhs); + NestedContact(NestedContact&& rhs) noexcept; /** * @brief Destructor. @@ -72,7 +72,7 @@ class YARP_OS_API NestedContact * @param rhs the NestedContact to be moved * @return this object */ - NestedContact& operator=(NestedContact&& rhs); + NestedContact& operator=(NestedContact&& rhs) noexcept; /** @} */ /** @{ */ diff --git a/src/libYARP_OS/include/yarp/os/PortInfo.h b/src/libYARP_OS/include/yarp/os/PortInfo.h index f550186da7..c8e9777bc1 100644 --- a/src/libYARP_OS/include/yarp/os/PortInfo.h +++ b/src/libYARP_OS/include/yarp/os/PortInfo.h @@ -40,10 +40,10 @@ class YARP_OS_API yarp::os::PortInfo { PORTINFO_NULL = 0, /// Information about an incoming or outgoing connection. - PORTINFO_CONNECTION = VOCAB4('c', 'o', 'n', 'n'), + PORTINFO_CONNECTION = yarp::os::createVocab('c', 'o', 'n', 'n'), /// Unspecified information. - PORTINFO_MISC = VOCAB4('m', 'i', 's', 'c') + PORTINFO_MISC = yarp::os::createVocab('m', 'i', 's', 'c') }; /// Type of information. PORTINFO_CONNECTION for information diff --git a/src/libYARP_OS/include/yarp/os/Route.h b/src/libYARP_OS/include/yarp/os/Route.h index 56bed707e9..83ed88a1b7 100644 --- a/src/libYARP_OS/include/yarp/os/Route.h +++ b/src/libYARP_OS/include/yarp/os/Route.h @@ -64,7 +64,7 @@ class YARP_OS_API Route { * * @param rhs the Route to be moved */ - Route(Route&& rhs); + Route(Route&& rhs) noexcept; /** * @brief Destructor. @@ -85,7 +85,7 @@ class YARP_OS_API Route { * @param rhs the Route to be moved * @return this object */ - Route& operator=(Route&& rhs); + Route& operator=(Route&& rhs) noexcept; /** @} */ /** @{ */ diff --git a/src/libYARP_OS/include/yarp/os/SharedLibraryClassApi.h b/src/libYARP_OS/include/yarp/os/SharedLibraryClassApi.h index 1f95662250..850c6fd380 100644 --- a/src/libYARP_OS/include/yarp/os/SharedLibraryClassApi.h +++ b/src/libYARP_OS/include/yarp/os/SharedLibraryClassApi.h @@ -88,7 +88,7 @@ extern "C" { YARP_SHARED_CLASS_FN int factoryname(void *api, int len) { \ struct yarp::os::SharedLibraryClassApi *sapi = (struct yarp::os::SharedLibraryClassApi *) api; \ if (len<(int)sizeof(yarp::os::SharedLibraryClassApi)) return -1; \ - sapi->startCheck = VOCAB4('Y', 'A', 'R', 'P'); \ + sapi->startCheck = yarp::os::createVocab('Y', 'A', 'R', 'P'); \ sapi->structureSize = sizeof(yarp::os::SharedLibraryClassApi); \ sapi->systemVersion = 5; \ sapi->create = factoryname ## _create; \ @@ -98,7 +98,7 @@ extern "C" { sapi->getClassName = factoryname ## _getClassName; \ sapi->getBaseClassName = factoryname ## _getBaseClassName; \ for (int i=0; iroomToGrow[i] = 0; } \ - sapi->endCheck = VOCAB4('P', 'L', 'U', 'G'); \ + sapi->endCheck = yarp::os::createVocab('P', 'L', 'U', 'G'); \ return sapi->startCheck; \ } // The double cast in the _create() and _destroy() functions are diff --git a/src/libYARP_OS/include/yarp/os/SharedLibraryFactory.h b/src/libYARP_OS/include/yarp/os/SharedLibraryFactory.h index 78936f83e1..e0a351446d 100644 --- a/src/libYARP_OS/include/yarp/os/SharedLibraryFactory.h +++ b/src/libYARP_OS/include/yarp/os/SharedLibraryFactory.h @@ -41,11 +41,11 @@ class YARP_OS_API yarp::os::SharedLibraryFactory { */ enum { STATUS_NONE, //!< Not configured yet. - STATUS_OK = VOCAB2('o', 'k'), //!< Present and sane. - STATUS_LIBRARY_NOT_FOUND = VOCAB4('f', 'o', 'u', 'n'), //!< Named shared library was not found. - STATUS_LIBRARY_NOT_LOADED = VOCAB4('l', 'o', 'a', 'd'), //!< Named shared library failed to load. - STATUS_FACTORY_NOT_FOUND = VOCAB4('f', 'a', 'c', 't'), //!< Named method wasn't present in library. - STATUS_FACTORY_NOT_FUNCTIONAL = VOCAB3('r', 'u', 'n') //!< Named method is not working right. + STATUS_OK = yarp::os::createVocab('o', 'k'), //!< Present and sane. + STATUS_LIBRARY_NOT_FOUND = yarp::os::createVocab('f', 'o', 'u', 'n'), //!< Named shared library was not found. + STATUS_LIBRARY_NOT_LOADED = yarp::os::createVocab('l', 'o', 'a', 'd'), //!< Named shared library failed to load. + STATUS_FACTORY_NOT_FOUND = yarp::os::createVocab('f', 'a', 'c', 't'), //!< Named method wasn't present in library. + STATUS_FACTORY_NOT_FUNCTIONAL = yarp::os::createVocab('r', 'u', 'n') //!< Named method is not working right. }; /** diff --git a/src/libYARP_OS/include/yarp/os/Type.h b/src/libYARP_OS/include/yarp/os/Type.h index ff72aa4b49..757582611b 100644 --- a/src/libYARP_OS/include/yarp/os/Type.h +++ b/src/libYARP_OS/include/yarp/os/Type.h @@ -38,6 +38,13 @@ class YARP_OS_API Type */ Type(const Type& rhs); + /** + * @brief Move constructor. + * + * @param rhs the Type to be moved + */ + Type(Type&& rhs) noexcept; + /** * @brief Destructor. */ @@ -51,6 +58,14 @@ class YARP_OS_API Type */ Type& operator=(const Type& rhs); + /** + * @brief Move assignment operator. + * + * @param rhs the Type to be moved + * @return this object + */ + Type& operator=(Type&& rhs) noexcept; + /** @} */ /** @{ */ diff --git a/src/libYARP_OS/include/yarp/os/Vocab.h b/src/libYARP_OS/include/yarp/os/Vocab.h index 7cb0092123..be1ac68d56 100644 --- a/src/libYARP_OS/include/yarp/os/Vocab.h +++ b/src/libYARP_OS/include/yarp/os/Vocab.h @@ -13,21 +13,6 @@ #include #include -namespace yarp { - namespace os { - class Vocab; - } -} - -// We need a macro for efficient switching. -// Use as, for example, VOCAB3('s','e','t') -#ifndef SWIG_PREPROCESSOR_SHOULD_SKIP_THIS -#define VOCAB(a,b,c,d) ((((int)(d))<<24)+(((int)(c))<<16)+(((int)(b))<<8)+((int)(a))) -#define VOCAB4(a,b,c,d) VOCAB((a),(b),(c),(d)) -#define VOCAB3(a,b,c) VOCAB((a),(b),(c),(0)) -#define VOCAB2(a,b) VOCAB((a),(b),(0),(0)) -#define VOCAB1(a) VOCAB((a),(0),(0),(0)) -#endif // SWIG_PREPROCESSOR_SHOULD_SKIP_THIS /** * Short readable codes. They are integers, for efficient switching, @@ -37,14 +22,12 @@ namespace yarp { * This is a compromise to allow the creation of messages that * can be parsed very efficiently by machine but nevertheless are * human readable and writable. - * When switching on a Vocab code, we suggest you use the - * VOCABn macro defined in yarp/os/Vocab.h. * \code * switch(code) { - * case VOCAB3('s','e','t'): // switch on "set" + * case createVocab('s','e','t'): // switch on "set" * ... * break; - * case VOCAB4('s','t','o','p'): // switch on "stop" + * case createVocab('s','t','o','p'): // switch on "stop" * ... * break; * } @@ -53,8 +36,18 @@ namespace yarp { * traverse the network will be human readable/writable. * */ -class YARP_OS_API yarp::os::Vocab { -public: +namespace yarp { +namespace os { +// We need a constexpr for efficient switching. +// Use as, for example, createVocab('s','e','t') +constexpr yarp::conf::vocab32_t createVocab(char a, char b = 0, char c = 0, char d = 0) { + return ((yarp::conf::vocab32_t)a) + + ((yarp::conf::vocab32_t)b<<8) + + ((yarp::conf::vocab32_t)c<<16)+ + ((yarp::conf::vocab32_t)d<<24); +} + +namespace Vocab { /** * Convert a string into a vocabulary identifier. If the string * is longer than four characters, only the first four characters @@ -62,14 +55,29 @@ class YARP_OS_API yarp::os::Vocab { * @param str the string to convert * @result the integer equivalent of the string form of the identifier */ - static NetInt32 encode(const std::string& str); + YARP_OS_API NetInt32 encode(const std::string& str); /** * Convert a vocabulary identifier into a string. * @param code the vocabulary identifier to convert * @result the string equivalent of the integer form of the identifier */ - static std::string decode(NetInt32 code); -}; + YARP_OS_API std::string decode(NetInt32 code); +} // Vocab +} // os +} // yarp + +#ifndef YARP_NO_DEPRECATED // since YARP 3.1.0 +YARP_DEPRECATED_MSG("Use yarp::os::createVocab() instead") +constexpr int32_t VOCAB(char a, char b, char c, char d) { return yarp::os::createVocab(a,b,c,d); } +YARP_DEPRECATED_MSG("Use yarp::os::createVocab() instead") +constexpr int32_t VOCAB4(char a, char b, char c, char d) { return yarp::os::createVocab(a,b,c,d); } +YARP_DEPRECATED_MSG("Use yarp::os::createVocab() instead") +constexpr int32_t VOCAB3(char a, char b, char c) { return yarp::os::createVocab(a,b,c,0); } +YARP_DEPRECATED_MSG("Use yarp::os::createVocab() instead") +constexpr int32_t VOCAB2(char a, char b) { return yarp::os::createVocab(a,b,0,0); } +YARP_DEPRECATED_MSG("Use yarp::os::createVocab() instead") +constexpr int32_t VOCAB1(char a) { return yarp::os::createVocab(a,0,0,0); } +#endif // YARP_NO_DEPRECATED #endif // YARP_OS_VOCAB_H diff --git a/src/libYARP_OS/include/yarp/os/impl/NameClient.h b/src/libYARP_OS/include/yarp/os/impl/NameClient.h index 6e229d733e..97b0865ccb 100644 --- a/src/libYARP_OS/include/yarp/os/impl/NameClient.h +++ b/src/libYARP_OS/include/yarp/os/impl/NameClient.h @@ -11,6 +11,7 @@ #define YARP_OS_IMPL_NAMECLIENT_H #include +#include #include namespace yarp { @@ -118,7 +119,7 @@ class YARP_OS_impl_API NameClient * * @return the reply from the name server. */ - std::string send(const std::string& cmd, bool multi = true); + std::string send(const std::string& cmd, bool multi = true, const ContactStyle& style = ContactStyle()); /** * Send a message to the nameserver in Bottle format, and return the diff --git a/src/libYARP_OS/src/BottleImpl.cpp b/src/libYARP_OS/src/BottleImpl.cpp index cd4346400f..a619a08596 100644 --- a/src/libYARP_OS/src/BottleImpl.cpp +++ b/src/libYARP_OS/src/BottleImpl.cpp @@ -222,7 +222,7 @@ void BottleImpl::fromString(const std::string& line) (nestedAlt == 0) && (nested == 0)) { if (arg != "") { if (arg == "null") { - add(new StoreVocab(VOCAB4('n', 'u', 'l', 'l'))); + add(new StoreVocab(yarp::os::createVocab('n', 'u', 'l', 'l'))); } else { smartAdd(arg); } diff --git a/src/libYARP_OS/src/ConnectionRecorder.cpp b/src/libYARP_OS/src/ConnectionRecorder.cpp index f7dceffbf2..43d0e71298 100644 --- a/src/libYARP_OS/src/ConnectionRecorder.cpp +++ b/src/libYARP_OS/src/ConnectionRecorder.cpp @@ -268,7 +268,7 @@ bool yarp::os::impl::ConnectionRecorder::write(yarp::os::ConnectionWriter& conne connection.appendInt32(BOTTLE_TAG_LIST); // nested structure connection.appendInt32(3); // with three elements connection.appendInt32(BOTTLE_TAG_VOCAB); - connection.appendInt32(VOCAB3('r', 'p', 'c')); + connection.appendInt32(yarp::os::createVocab('r', 'p', 'c')); bool ok = readerStore.write(connection); if (ok) { writerStore.write(connection); diff --git a/src/libYARP_OS/src/Contact.cpp b/src/libYARP_OS/src/Contact.cpp index c7ba0105a6..d41759349f 100644 --- a/src/libYARP_OS/src/Contact.cpp +++ b/src/libYARP_OS/src/Contact.cpp @@ -97,7 +97,7 @@ Contact::Contact(const Contact& rhs) : { } -Contact::Contact(Contact&& rhs) : +Contact::Contact(Contact&& rhs) noexcept : mPriv(rhs.mPriv) { rhs.mPriv = nullptr; @@ -116,7 +116,7 @@ Contact& Contact::operator=(const Contact& rhs) return *this; } -Contact& Contact::operator=(Contact&& rhs) +Contact& Contact::operator=(Contact&& rhs) noexcept { if (&rhs != this) { std::swap(mPriv, rhs.mPriv); diff --git a/src/libYARP_OS/src/NameClient.cpp b/src/libYARP_OS/src/NameClient.cpp index 77c0708a72..4b1a260329 100644 --- a/src/libYARP_OS/src/NameClient.cpp +++ b/src/libYARP_OS/src/NameClient.cpp @@ -299,8 +299,7 @@ Contact NameClient::extractAddress(const Bottle& bot) return Contact(); } -std::string NameClient::send(const std::string& cmd, bool multi) -{ +std::string NameClient::send(const std::string& cmd, bool multi, const ContactStyle& style) { //printf("*** OLD YARP command %s\n", cmd.c_str()); setup(); @@ -321,6 +320,10 @@ std::string NameClient::send(const std::string& cmd, bool multi) std::string result; Contact server = getAddress(); float timeout = 10; + if (style.timeout > 0) + { + timeout = style.timeout; + } server.setTimeout(timeout); do { diff --git a/src/libYARP_OS/src/NameConfig.cpp b/src/libYARP_OS/src/NameConfig.cpp index 46dd63a511..5a01de1a2d 100644 --- a/src/libYARP_OS/src/NameConfig.cpp +++ b/src/libYARP_OS/src/NameConfig.cpp @@ -351,7 +351,7 @@ bool NameConfig::isLocalName(const std::string& name) { if (strcmp(hostname, name.c_str()) == 0) result = true; if (!result) { Bottle lst = getIpsAsBottle(); - for (int i=0; i %s", @@ -2236,7 +2236,7 @@ bool PortCore::adminBlock(ConnectionReader& reader, reader.requestDrop(); // ROS likes to close down. } break; - case VOCAB3('p', 'i', 'd'): + case yarp::os::createVocab('p', 'i', 'd'): { // ROS-style query for PID. result.addInt32(1); @@ -2245,7 +2245,7 @@ bool PortCore::adminBlock(ConnectionReader& reader, reader.requestDrop(); // ROS likes to close down. } break; - case VOCAB3('b', 'u', 's'): + case yarp::os::createVocab('b', 'u', 's'): { // ROS-style query for bus information - we support this // in yarp::os::Node but not otherwise. @@ -2255,11 +2255,11 @@ bool PortCore::adminBlock(ConnectionReader& reader, reader.requestDrop(); // ROS likes to close down. } break; - case VOCAB4('p', 'r', 'o', 'p'): + case yarp::os::createVocab('p', 'r', 'o', 'p'): { // Set/get arbitrary properties on a port. switch (cmd.get(1).asVocab()) { - case VOCAB3('g', 'e', 't'): + case yarp::os::createVocab('g', 'e', 't'): { Property *p = acquireProperties(false); if (p) { @@ -2353,7 +2353,7 @@ bool PortCore::adminBlock(ConnectionReader& reader, releaseProperties(p); } break; - case VOCAB3('s', 'e', 't'): + case yarp::os::createVocab('s', 'e', 't'): { Property *p = acquireProperties(false); bool bOk = true; @@ -2446,10 +2446,10 @@ bool PortCore::adminBlock(ConnectionReader& reader, // the expected levels are: LOW, NORM, HIGH, CRIT int dscp; switch(priority) { - case VOCAB3('L', 'O', 'W') : dscp = 10; break; - case VOCAB4('N', 'O', 'R', 'M'): dscp = 0; break; - case VOCAB4('H', 'I', 'G', 'H'): dscp = 36; break; - case VOCAB4('C', 'R', 'I', 'T'): dscp = 44; break; + case yarp::os::createVocab('L', 'O', 'W') : dscp = 10; break; + case yarp::os::createVocab('N', 'O', 'R', 'M'): dscp = 0; break; + case yarp::os::createVocab('H', 'I', 'G', 'H'): dscp = 36; break; + case yarp::os::createVocab('C', 'R', 'I', 'T'): dscp = 44; break; default: dscp = -1; }; if (dscp >= 0) { diff --git a/src/libYARP_OS/src/QosStyle.cpp b/src/libYARP_OS/src/QosStyle.cpp index a183c14ad6..91fe9d06cd 100644 --- a/src/libYARP_OS/src/QosStyle.cpp +++ b/src/libYARP_OS/src/QosStyle.cpp @@ -123,28 +123,28 @@ yarp::os::QosStyle::PacketPriorityLevel yarp::os::QosStyle::getPacketPriorityAsL // High Drop | AF13 (DSCP 14) AF23 (DSCP 22) AF33 (DSCP 30) AF43 (DSCP 38) yarp::os::QosStyle::PacketPriorityDSCP yarp::os::QosStyle::getDSCPByVocab(int vocab) { switch(vocab) { - case VOCAB3('C', 'S', '0') : return DSCP_CS0; - case VOCAB3('C', 'S', '1') : return DSCP_CS1; - case VOCAB3('C', 'S', '2') : return DSCP_CS2; - case VOCAB3('C', 'S', '3') : return DSCP_CS3; - case VOCAB3('C', 'S', '4') : return DSCP_CS4; - case VOCAB3('C', 'S', '5') : return DSCP_CS5; - case VOCAB3('C', 'S', '6') : return DSCP_CS6; - case VOCAB3('C', 'S', '7') : return DSCP_CS7; - case VOCAB4('A', 'F', '1', '1'): return DSCP_AF11; - case VOCAB4('A', 'F', '1', '2'): return DSCP_AF12; - case VOCAB4('A', 'F', '1', '3'): return DSCP_AF13; - case VOCAB4('A', 'F', '2', '1'): return DSCP_AF21; - case VOCAB4('A', 'F', '2', '2'): return DSCP_AF22; - case VOCAB4('A', 'F', '2', '3'): return DSCP_AF23; - case VOCAB4('A', 'F', '3', '1'): return DSCP_AF31; - case VOCAB4('A', 'F', '3', '2'): return DSCP_AF32; - case VOCAB4('A', 'F', '3', '3'): return DSCP_AF33; - case VOCAB4('A', 'F', '4', '1'): return DSCP_AF41; - case VOCAB4('A', 'F', '4', '2'): return DSCP_AF42; - case VOCAB4('A', 'F', '4', '3'): return DSCP_AF43; - case VOCAB2('V', 'A') : return DSCP_VA; - case VOCAB2('E', 'F') : return DSCP_EF; + case yarp::os::createVocab('C', 'S', '0') : return DSCP_CS0; + case yarp::os::createVocab('C', 'S', '1') : return DSCP_CS1; + case yarp::os::createVocab('C', 'S', '2') : return DSCP_CS2; + case yarp::os::createVocab('C', 'S', '3') : return DSCP_CS3; + case yarp::os::createVocab('C', 'S', '4') : return DSCP_CS4; + case yarp::os::createVocab('C', 'S', '5') : return DSCP_CS5; + case yarp::os::createVocab('C', 'S', '6') : return DSCP_CS6; + case yarp::os::createVocab('C', 'S', '7') : return DSCP_CS7; + case yarp::os::createVocab('A', 'F', '1', '1'): return DSCP_AF11; + case yarp::os::createVocab('A', 'F', '1', '2'): return DSCP_AF12; + case yarp::os::createVocab('A', 'F', '1', '3'): return DSCP_AF13; + case yarp::os::createVocab('A', 'F', '2', '1'): return DSCP_AF21; + case yarp::os::createVocab('A', 'F', '2', '2'): return DSCP_AF22; + case yarp::os::createVocab('A', 'F', '2', '3'): return DSCP_AF23; + case yarp::os::createVocab('A', 'F', '3', '1'): return DSCP_AF31; + case yarp::os::createVocab('A', 'F', '3', '2'): return DSCP_AF32; + case yarp::os::createVocab('A', 'F', '3', '3'): return DSCP_AF33; + case yarp::os::createVocab('A', 'F', '4', '1'): return DSCP_AF41; + case yarp::os::createVocab('A', 'F', '4', '2'): return DSCP_AF42; + case yarp::os::createVocab('A', 'F', '4', '3'): return DSCP_AF43; + case yarp::os::createVocab('V', 'A') : return DSCP_VA; + case yarp::os::createVocab('E', 'F') : return DSCP_EF; default : return DSCP_Invalid; }; } @@ -152,10 +152,10 @@ yarp::os::QosStyle::PacketPriorityDSCP yarp::os::QosStyle::getDSCPByVocab(int vo yarp::os::QosStyle::PacketPriorityLevel yarp::os::QosStyle::getLevelByVocab(int vocab) { switch(vocab) { - case VOCAB4('N', 'O', 'R', 'M') : return PacketPriorityNormal; - case VOCAB3('L', 'O', 'W') : return PacketPriorityLow; - case VOCAB4('H', 'I', 'G', 'H') : return PacketPriorityHigh; - case VOCAB4('C', 'R', 'I', 'T') : return PacketPriorityCritical; + case yarp::os::createVocab('N', 'O', 'R', 'M') : return PacketPriorityNormal; + case yarp::os::createVocab('L', 'O', 'W') : return PacketPriorityLow; + case yarp::os::createVocab('H', 'I', 'G', 'H') : return PacketPriorityHigh; + case yarp::os::createVocab('C', 'R', 'I', 'T') : return PacketPriorityCritical; default : return PacketPriorityInvalid; } } diff --git a/src/libYARP_OS/src/RFModule.cpp b/src/libYARP_OS/src/RFModule.cpp index 55b7f98580..4b469fbb77 100644 --- a/src/libYARP_OS/src/RFModule.cpp +++ b/src/libYARP_OS/src/RFModule.cpp @@ -524,9 +524,9 @@ bool RFModule::safeRespond(const Bottle& command, Bottle& reply) { bool RFModule::basicRespond(const Bottle& command, Bottle& reply) { switch (command.get(0).asVocab()) { - case VOCAB4('q', 'u', 'i', 't'): - case VOCAB4('e', 'x', 'i', 't'): - case VOCAB3('b', 'y', 'e'): + case yarp::os::createVocab('q', 'u', 'i', 't'): + case yarp::os::createVocab('e', 'x', 'i', 't'): + case yarp::os::createVocab('b', 'y', 'e'): reply.addVocab(Vocab::encode("bye")); stopModule(false); //calls interruptModule() // interruptModule(); diff --git a/src/libYARP_OS/src/Route.cpp b/src/libYARP_OS/src/Route.cpp index 4d415b8991..55bec5e0f8 100644 --- a/src/libYARP_OS/src/Route.cpp +++ b/src/libYARP_OS/src/Route.cpp @@ -68,7 +68,7 @@ Route::Route(const Route& rhs) : { } -Route::Route(Route&& rhs) : +Route::Route(Route&& rhs) noexcept : mPriv(rhs.mPriv) { rhs.mPriv = nullptr; @@ -87,7 +87,7 @@ Route& Route::operator=(const Route& rhs) return *this; } -Route& Route::operator=(Route&& rhs) +Route& Route::operator=(Route&& rhs) noexcept { if (&rhs != this) { std::swap(mPriv, rhs.mPriv); diff --git a/src/libYARP_OS/src/SharedLibraryFactory.cpp b/src/libYARP_OS/src/SharedLibraryFactory.cpp index 9c06d5a21c..81bdb21812 100644 --- a/src/libYARP_OS/src/SharedLibraryFactory.cpp +++ b/src/libYARP_OS/src/SharedLibraryFactory.cpp @@ -75,10 +75,10 @@ bool yarp::os::SharedLibraryFactory::open(const char *dll_name, const char *fn_n bool yarp::os::SharedLibraryFactory::isValid() const { - if (returnValue != VOCAB4('Y', 'A', 'R', 'P')) { + if (returnValue != yarp::os::createVocab('Y', 'A', 'R', 'P')) { return false; } - if (api.startCheck != VOCAB4('Y', 'A', 'R', 'P')) { + if (api.startCheck != yarp::os::createVocab('Y', 'A', 'R', 'P')) { return false; } if (api.structureSize != sizeof(SharedLibraryClassApi)) { @@ -87,7 +87,7 @@ bool yarp::os::SharedLibraryFactory::isValid() const if (api.systemVersion != 5) { return false; } - if (api.endCheck != VOCAB4('P', 'L', 'U', 'G')) { + if (api.endCheck != yarp::os::createVocab('P', 'L', 'U', 'G')) { return false; } return true; diff --git a/src/libYARP_OS/src/Terminator.cpp b/src/libYARP_OS/src/Terminator.cpp index 64d86cfe3d..38a39ce4d8 100644 --- a/src/libYARP_OS/src/Terminator.cpp +++ b/src/libYARP_OS/src/Terminator.cpp @@ -118,9 +118,9 @@ void Terminee::run() { } if (cmd.get(0).asString()=="quit") { quit = true; - reply.addVocab(VOCAB2('o', 'k')); + reply.addVocab(yarp::os::createVocab('o', 'k')); } else { - reply.addVocab(VOCAB4('h', 'u', 'h', '?')); + reply.addVocab(yarp::os::createVocab('h', 'u', 'h', '?')); } helper.reply(reply); } diff --git a/src/libYARP_OS/src/Type.cpp b/src/libYARP_OS/src/Type.cpp index e78b4eac4c..374068e676 100644 --- a/src/libYARP_OS/src/Type.cpp +++ b/src/libYARP_OS/src/Type.cpp @@ -101,6 +101,12 @@ Type::Type(const Type& rhs) : { } +Type::Type(Type&& rhs) noexcept : + mPriv(rhs.mPriv) +{ + rhs.mPriv = nullptr; +} + Type::~Type() { delete mPriv; @@ -114,6 +120,14 @@ Type& Type::operator=(const Type& rhs) return *this; } +Type& Type::operator=(Type&& rhs) noexcept +{ + if (&rhs != this) { + std::swap(mPriv, rhs.mPriv); + } + return *this; +} + const Searchable& Type::readProperties() const { return mPriv->readProperties(); diff --git a/src/libYARP_OS/src/Vocab.cpp b/src/libYARP_OS/src/Vocab.cpp index f79b4c1417..0717283a91 100644 --- a/src/libYARP_OS/src/Vocab.cpp +++ b/src/libYARP_OS/src/Vocab.cpp @@ -28,7 +28,7 @@ NetInt32 Vocab::encode(const std::string& str) { } } } - return VOCAB(a, b, c, d); + return createVocab(a, b, c, d); } diff --git a/src/libYARP_OS/src/WireReader.cpp b/src/libYARP_OS/src/WireReader.cpp index cb2ba94e12..17fcd391fa 100644 --- a/src/libYARP_OS/src/WireReader.cpp +++ b/src/libYARP_OS/src/WireReader.cpp @@ -107,7 +107,7 @@ bool WireReader::readBool(bool& x) return false; } std::int32_t v = reader.expectInt32(); - x = (v!=0) && (v!=VOCAB4('f', 'a', 'i', 'l')); + x = (v!=0) && (v!=yarp::os::createVocab('f', 'a', 'i', 'l')); state->len--; return !reader.isError(); } @@ -508,7 +508,7 @@ bool WireReader::readListReturn() if (!readVocab(v)) { return false; } - if (v!=VOCAB2('i', 's')) { + if (v!=yarp::os::createVocab('i', 's')) { return false; } std::string dummy; diff --git a/src/libYARP_OS/src/WireWriter.cpp b/src/libYARP_OS/src/WireWriter.cpp index e61ac02f09..2051c66861 100644 --- a/src/libYARP_OS/src/WireWriter.cpp +++ b/src/libYARP_OS/src/WireWriter.cpp @@ -57,7 +57,7 @@ bool WireWriter::writeNested(const yarp::os::PortWriter& obj) const { bool WireWriter::writeBool(bool x) const { writer.appendInt32(BOTTLE_TAG_VOCAB); - writer.appendInt32(x?VOCAB2('o', 'k'):VOCAB4('f', 'a', 'i', 'l')); + writer.appendInt32(x?yarp::os::createVocab('o', 'k'):yarp::os::createVocab('f', 'a', 'i', 'l')); return !writer.isError(); } @@ -156,7 +156,7 @@ bool WireWriter::writeListHeader(int len) const { if (get_mode) { writer.appendInt32(len+3); writer.appendInt32(BOTTLE_TAG_VOCAB); - writer.appendInt32(VOCAB2('i', 's')); + writer.appendInt32(yarp::os::createVocab('i', 's')); if (get_is_vocab) { writer.appendInt32(BOTTLE_TAG_VOCAB); writer.appendInt32(Vocab::encode(get_string.c_str())); @@ -206,6 +206,6 @@ bool WireWriter::writeMapEnd() const { bool WireWriter::writeOnewayResponse() const { if (!writeListHeader(1)) return false; writer.appendInt32(BOTTLE_TAG_VOCAB); - writer.appendInt32(VOCAB4('d', 'o', 'n', 'e')); + writer.appendInt32(yarp::os::createVocab('d', 'o', 'n', 'e')); return true; } diff --git a/src/libYARP_OS/src/YarpNameSpace.cpp b/src/libYARP_OS/src/YarpNameSpace.cpp index f19d385d0d..fa02ebb343 100644 --- a/src/libYARP_OS/src/YarpNameSpace.cpp +++ b/src/libYARP_OS/src/YarpNameSpace.cpp @@ -154,7 +154,6 @@ Contact YarpNameSpace::detectNameServer(bool useDetectedServer, bool YarpNameSpace::writeToNameServer(PortWriter& cmd, PortReader& reply, const ContactStyle& style) { - YARP_UNUSED(style); Contact srv = getNameServerContact(); std::string cmd0 = "NAME_SERVER"; @@ -167,7 +166,7 @@ bool YarpNameSpace::writeToNameServer(PortWriter& cmd, cmd0 += in.get(i).toString().c_str(); } NameClient& nic = HELPER(this); - std::string result = nic.send(cmd0); + std::string result = nic.send(cmd0, true, style); Bottle reply2; reply2.addString(result.c_str()); DummyConnector con; diff --git a/src/libYARP_companion/src/Companion.cpp b/src/libYARP_companion/src/Companion.cpp index c701645e7a..5bc9b3535c 100644 --- a/src/libYARP_companion/src/Companion.cpp +++ b/src/libYARP_companion/src/Companion.cpp @@ -99,7 +99,7 @@ static char* command_generator (const char* text, int state) if (helpOk) { yarp::os::Bottle* cmdList = nullptr; - if (helpBottle.get(0).isVocab() && helpBottle.get(0).asVocab()==VOCAB4('m', 'a', 'n', 'y') ) + if (helpBottle.get(0).isVocab() && helpBottle.get(0).asVocab()==yarp::os::createVocab('m', 'a', 'n', 'y') ) { cmdList=helpBottle.get(1).asList(); } @@ -641,7 +641,7 @@ int Companion::cmdName(int argc, char *argv[]) { } if (reply.size()==1&&reply.get(0).isString()) { printf("%s", reply.get(0).asString().c_str()); - } else if (reply.get(0).isVocab() && reply.get(0).asVocab()==VOCAB4('m', 'a', 'n', 'y')) { + } else if (reply.get(0).isVocab() && reply.get(0).asVocab()==yarp::os::createVocab('m', 'a', 'n', 'y')) { for (size_t i=1; i +#include namespace yarp { namespace dev { @@ -134,16 +135,15 @@ class YARP_dev_API yarp::dev::IRemoteCalibrator virtual bool quitPark()=0; }; -#define VOCAB_REMOTE_CALIBRATOR_INTERFACE VOCAB4('r','e','c','a') - -#define VOCAB_IS_CALIBRATOR_PRESENT VOCAB4('i','s','c','a') -#define VOCAB_CALIBRATE_SINGLE_JOINT VOCAB3('c','a','l') -#define VOCAB_CALIBRATE_WHOLE_PART VOCAB4('c','a','l','s') -#define VOCAB_HOMING_SINGLE_JOINT VOCAB3('h','o','m') -#define VOCAB_HOMING_WHOLE_PART VOCAB4('h','o','m','s') -#define VOCAB_PARK_SINGLE_JOINT VOCAB3('p','a','r') -#define VOCAB_PARK_WHOLE_PART VOCAB4('p','a','r','s') -#define VOCAB_QUIT_CALIBRATE VOCAB4('q','u','c','a') -#define VOCAB_QUIT_PARK VOCAB4('q','u','p','a') +constexpr yarp::conf::vocab32_t VOCAB_REMOTE_CALIBRATOR_INTERFACE = yarp::os::createVocab('r','e','c','a'); +constexpr yarp::conf::vocab32_t VOCAB_IS_CALIBRATOR_PRESENT = yarp::os::createVocab('i','s','c','a'); +constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_SINGLE_JOINT = yarp::os::createVocab('c','a','l'); +constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_WHOLE_PART = yarp::os::createVocab('c','a','l','s'); +constexpr yarp::conf::vocab32_t VOCAB_HOMING_SINGLE_JOINT = yarp::os::createVocab('h','o','m'); +constexpr yarp::conf::vocab32_t VOCAB_HOMING_WHOLE_PART = yarp::os::createVocab('h','o','m','s'); +constexpr yarp::conf::vocab32_t VOCAB_PARK_SINGLE_JOINT = yarp::os::createVocab('p','a','r'); +constexpr yarp::conf::vocab32_t VOCAB_PARK_WHOLE_PART = yarp::os::createVocab('p','a','r','s'); +constexpr yarp::conf::vocab32_t VOCAB_QUIT_CALIBRATE = yarp::os::createVocab('q','u','c','a'); +constexpr yarp::conf::vocab32_t VOCAB_QUIT_PARK = yarp::os::createVocab('q','u','p','a'); #endif // YARP_DEV_CALIBRATORINTERFACES_H diff --git a/src/libYARP_dev/include/yarp/dev/ControlBoardInterfaces.h b/src/libYARP_dev/include/yarp/dev/ControlBoardInterfaces.h index e9d1bae760..a73022ffc8 100644 --- a/src/libYARP_dev/include/yarp/dev/ControlBoardInterfaces.h +++ b/src/libYARP_dev/include/yarp/dev/ControlBoardInterfaces.h @@ -33,777 +33,37 @@ #include #include #include - -namespace yarp { - namespace dev { - class IPidControlRaw; - class IPidControl; - class IEncodersRaw; - class IEncoders; - class IMotor; - class IMotorRaw; - class IMotorEncodersRaw; - class IMotorEncoders; - class IAmplifierControlRaw; - class IAmplifierControl; - class IControlDebug; - class IControlLimitsRaw; - class IControlLimits; - class IControlCalibrationRaw; -#ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 -YARP_DEPRECATED_TYPEDEF_MSG("Use yarp::dev::IControlCalibrationRaw instead") IControlCalibrationRaw IControlCalibration2Raw; -#endif - class IControlCalibration; -#ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 -YARP_DEPRECATED_TYPEDEF_MSG("Use yarp::dev::IControlCalibration instead") IControlCalibration IControlCalibration2; -#endif - class IAxisInfo; - class IAxisInfoRaw; - struct CalibrationParameters; - - enum JointTypeEnum - { - VOCAB_JOINTTYPE_REVOLUTE = VOCAB4('a', 't', 'r', 'v'), - VOCAB_JOINTTYPE_PRISMATIC = VOCAB4('a', 't', 'p', 'r'), - VOCAB_JOINTTYPE_UNKNOWN = VOCAB4('u', 'n', 'k', 'n') - }; - } -} - -struct YARP_dev_API yarp::dev::CalibrationParameters -{ - unsigned int type; - double param1; - double param2; - double param3; - double param4; - double param5; - double paramZero; - CalibrationParameters() { type = 0; param1 = 0; param2 = 0; param3 = 0; param4 = 0; param5 = 0; paramZero = 0; } -}; - -/** - * @ingroup dev_iface_motor - * - * Interface for control devices, amplifier commands. - */ -class YARP_dev_API yarp::dev::IAmplifierControl -{ -public: - /** - * Destructor. - */ - virtual ~IAmplifierControl() {} - - /** Enable the amplifier on a specific joint. Be careful, check that the output - * of the controller is appropriate (usually zero), to avoid - * generating abrupt movements. - * @return true/false on success/failure - */ - virtual bool enableAmp(int j)=0; - - /** Disable the amplifier on a specific joint. All computations within the board - * will be carried out normally, but the output will be disabled. - * @return true/false on success/failure - */ - virtual bool disableAmp(int j)=0; - - /* Get the status of the amplifiers, coded in a 32 bits integer for - * each amplifier (at the moment contains only the fault, it will be - * expanded in the future). - * @param st pointer to storage - * @return true in good luck, false otherwise. - */ - virtual bool getAmpStatus(int *st)=0; - - /* Get the status of a single amplifier, coded in a 32 bits integer - * @param j joint number - * @param st storage for return value - * @return true/false success failure. - */ - virtual bool getAmpStatus(int j, int *v)=0; - - /* Read the electric current going to all motors. - * @param vals pointer to storage for the output values - * @return hopefully true, false in bad luck. - */ - virtual bool getCurrents(double *vals)=0; - - /* Read the electric current going to a given motor. - * @param j motor number - * @param val pointer to storage for the output value - * @return probably true, might return false in bad times - */ - virtual bool getCurrent(int j, double *val)=0; - - /** - * Returns the maximum electric current allowed for a given motor. - * Exceeding this value will trigger instantaneous hardware fault. - * @param j motor number - * @param v the return value - * @return probably true, might return false in bad times - */ - virtual bool getMaxCurrent(int j, double *v)=0; - - /* Set the maximum electric current going to a given motor. - * Exceeding this value will trigger instantaneous hardware fault. - * @param j motor number - * @param v the new value - * @return probably true, might return false in bad times - */ - virtual bool setMaxCurrent(int j, double v)=0; - - /* Get the the nominal current which can be kept for an indefinite amount of time - * without harming the motor. This value is specific for each motor and it is typically - * found in its datasheet. The units are Ampere. - * This value and the peak current may be used by the firmware to configure - * an I2T filter. - * @param j joint number - * @param val storage for return value. [Ampere] - * @return true/false success failure. - */ - virtual bool getNominalCurrent(int m, double *val) {return false;}; - - /* Set the the nominal current which can be kept for an indefinite amount of time - * without harming the motor. This value is specific for each motor and it is typically - * found in its datasheet. The units are Ampere. - * This value and the peak current may be used by the firmware to configure - * an I2T filter. - * @param j joint number - * @param val storage for return value. [Ampere] - * @return true/false success failure. - */ - virtual bool setNominalCurrent(int m, const double val) { return false; }; - - /* Get the the peak current which causes damage to the motor if maintained - * for a long amount of time. - * The value is often found in the motor datasheet, units are Ampere. - * This value and the nominal current may be used by the firmware to configure - * an I2T filter. - * @param j joint number - * @param val storage for return value. [Ampere] - * @return true/false success failure. - */ - virtual bool getPeakCurrent(int m, double *val) {return false;}; - - /* Set the the peak current. This value which causes damage to the motor if maintained - * for a long amount of time. - * The value is often found in the motor datasheet, units are Ampere. - * This value and the nominal current may be used by the firmware to configure - * an I2T filter. - * @param j joint number - * @param val storage for return value. [Ampere] - * @return true/false success failure. - */ - virtual bool setPeakCurrent(int m, const double val) {return false;}; - - /* Get the the current PWM value used to control the motor. - * The units are firmware dependent, either machine units or percentage. - * @param j joint number - * @param val filled with PWM value. - * @return true/false success failure. - */ - virtual bool getPWM(int j, double* val) {return false;}; - - /* Get the PWM limit for the given motor. - * The units are firmware dependent, either machine units or percentage. - * @param j joint number - * @param val filled with PWM limit value. - * @return true/false success failure. - */ - virtual bool getPWMLimit(int j, double* val) {return false;}; - - /* Set the PWM limit for the given motor. - * The units are firmware dependent, either machine units or percentage. - * @param j joint number - * @param val new value for the PWM limit. - * @return true/false success failure. - */ - virtual bool setPWMLimit(int j, const double val) {return false;}; - - /* Get the power source voltage for the given motor in Volt. - * @param j joint number - * @param val filled with return value. - * @return true/false success failure. - */ - virtual bool getPowerSupplyVoltage(int j, double* val) {return false;}; -}; - -/** - * - * Interface for control devices, amplifier commands. - */ -class YARP_dev_API yarp::dev::IAmplifierControlRaw -{ -public: - /** - * Destructor. - */ - virtual ~IAmplifierControlRaw() {} - - /** Enable the amplifier on a specific joint. Be careful, check that the output - * of the controller is appropriate (usually zero), to avoid - * generating abrupt movements. - * @return true/false on success/failure - */ - virtual bool enableAmpRaw(int j)=0; - - /** Disable the amplifier on a specific joint. All computations within the board - * will be carried out normally, but the output will be disabled. - * @return true/false on success/failure - */ - virtual bool disableAmpRaw(int j)=0; - - /* Get the status of the amplifiers, coded in a 32 bits integer for - * each amplifier (at the moment contains only the fault, it will be - * expanded in the future). - * @param st pointer to storage - * @return true/false success failure. - */ - virtual bool getAmpStatusRaw(int *st)=0; - - /* Get the status of a single amplifier, coded in a 32 bits integer - * @param j joint number - * @param st storage for return value - * @return true/false success failure. - */ - virtual bool getAmpStatusRaw(int j, int *st)=0; - - /* Read the electric current going to all motors. - * @param vals pointer to storage for the output values - * @return hopefully true, false in bad luck. - */ - virtual bool getCurrentsRaw(double *vals)=0; - - /* Read the electric current going to a given motor. - * @param j motor number - * @param val pointer to storage for the output value - * @return probably true, might return false in bad times - */ - virtual bool getCurrentRaw(int j, double *val)=0; - - /* Set the maximum electric current going to a given motor. - * Exceeding this value will trigger instantaneous hardware fault. - * @param j motor number - * @param v the new value - * @return probably true, might return false in bad times - */ - virtual bool setMaxCurrentRaw(int j, double v)=0; - - /** - * Returns the maximum electric current allowed for a given motor. - * Exceeding this value will trigger instantaneous hardware fault. - * @param j motor number - * @param v the return value - * @return probably true, might return false in bad times - */ - virtual bool getMaxCurrentRaw(int j, double *v)=0; - - /* Get the the nominal current which can be kept for an indefinite amount of time - * without harming the motor. This value is specific for each motor and it is typically - * found in its datasheet. The units are Ampere. - * This value and the peak current may be used by the firmware to configure - * an I2T filter. - * @param j joint number - * @param val storage for return value. [Ampere] - * @return true/false success failure. - */ - virtual bool getNominalCurrentRaw(int m, double *val) {return false;}; - - /* Set the the nominal current which can be kept for an indefinite amount of time - * without harming the motor. This value is specific for each motor and it is typically - * found in its datasheet. The units are Ampere. - * This value and the peak current may be used by the firmware to configure - * an I2T filter. - * @param j joint number - * @param val storage for return value. [Ampere] - * @return true/false success failure. - */ - virtual bool setNominalCurrentRaw(int m, const double val) { return false; }; - - /* Get the the peak current which causes damage to the motor if maintained - * for a long amount of time. - * The value is often found in the motor datasheet, units are Ampere. - * This value and the nominal current may be used by the firmware to configure - * an I2T filter. - * @param j joint number - * @param val storage for return value. [Ampere] - * @return true/false success failure. - */ - virtual bool getPeakCurrentRaw(int m, double *val) {return false;}; - - /* Set the the peak current. This value which causes damage to the motor if maintained - * for a long amount of time. - * The value is often found in the motor datasheet, units are Ampere. - * This value and the nominal current may be used by the firmware to configure - * an I2T filter. - * @param j joint number - * @param val storage for return value. [Ampere] - * @return true/false success failure. - */ - virtual bool setPeakCurrentRaw(int m, const double val) {return false;}; - - /* Get the the current PWM value used to control the motor. - * The units are firmware dependent, either machine units or percentage. - * @param j joint number - * @param val filled with PWM value. - * @return true/false success failure. - */ - virtual bool getPWMRaw(int j, double* val) {return false;}; - - /* Get the PWM limit for the given motor. - * The units are firmware dependent, either machine units or percentage. - * @param j joint number - * @param val filled with PWM limit value. - * @return true/false success failure. - */ - virtual bool getPWMLimitRaw(int j, double* val) {return false;}; - - /* Set the PWM limit for the given motor. - * The units are firmware dependent, either machine units or percentage. - * @param j joint number - * @param val new value for the PWM limit. - * @return true/false success failure. - */ - virtual bool setPWMLimitRaw(int j, const double val) {return false;}; - - /* Get the power source voltage for the given motor in Volt. - * @param j joint number - * @param val filled with return value. [Volt] - * @return true/false success failure. - */ - virtual bool getPowerSupplyVoltageRaw(int j, double* val) {return false;}; -}; - -/** - * - * Interface for control devices, calibration commands. - */ -class YARP_dev_API yarp::dev::IControlCalibrationRaw -{ -public: - IControlCalibrationRaw(){} - /** - * Destructor. - */ - virtual ~IControlCalibrationRaw() {} - -#ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 - /** - * Start calibration, this method is very often platform - * specific. - * @deprecated Since YARP 3.0.0 - * @return true/false on success failure - */ - virtual bool calibrateRaw(int j, double p) { return false; } - - /** - * Start calibration, this method is very often platform - * specific. - * @deprecated Since YARP 3.0.0 - * @return true/false on success failure - */ - virtual bool calibrate2Raw(int axis, unsigned int type, double p1, double p2, double p3) { return false;} -#endif - - /** - * Check if the calibration is terminated, on a particular joint. - * Non blocking. - * @return true/false - */ - virtual bool doneRaw(int j)=0; - - /** - * Start calibration, this method is very often platform - * specific. - * @return true/false on success failure - */ - virtual bool calibrateRaw(int axis, unsigned int type, double p1, double p2, double p3)=0; - - /** - * Start calibration, this method is very often platform - * specific. - * @return true/false on success failure - */ - virtual bool setCalibrationParametersRaw(int axis, const CalibrationParameters& params) { return false; } - -}; - -/** - * @ingroup dev_iface_motor - * - * Interface for control devices, calibration commands. - */ -class YARP_dev_API yarp::dev::IControlCalibration -{ -private: - ICalibrator *calibrator; - -public: - IControlCalibration(); - /** - * Destructor. - */ - virtual ~IControlCalibration() {} - -#ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 - /** - * Start calibration, this method is very often platform - * specific. - * @return true/false on success failure - * @deprecated Since YARP 3.0.0 - */ - YARP_DEPRECATED - virtual bool calibrate(int j, double p) { return false; } - - /** - * Start calibration, this method is very often platform - * specific. - * @return true/false on success failure - * @deprecated Since YARP 3.0.0 - */ - YARP_DEPRECATED_MSG("Use calibrate instead") - virtual bool calibrate2(int axis, unsigned int type, double p1, double p2, double p3) { return calibrate(axis, type, p1, p2, p3); } -#endif - - /** - * Start calibration, this method is very often platform - * specific. - * @return true/false on success failure - */ - virtual bool calibrate(int axis, unsigned int type, double p1, double p2, double p3)=0; - - /** - * Start calibration, this method is very often platform - * specific. - * @return true/false on success failure - */ - virtual bool setCalibrationParameters(int axis, const CalibrationParameters& params) { return false; } - - /** - * Check if the calibration is terminated, on a particular joint. - * Non blocking. - * @return true/false - */ - virtual bool done(int j)=0; - - /** - * Set the calibrator object to be used to calibrate the robot. - * @param c pointer to the calibrator object - * @return true/false on success failure - */ - virtual bool setCalibrator(ICalibrator *c); - - /** - * Calibrate robot by using an external calibrator. The external - * calibrator must be previously set by calling the setCalibration() - * method. - * @return true/false on success failure - */ - virtual bool calibrate(); - - virtual bool park(bool wait=true); - - /* Abort calibration, force the function calibrate() to return.*/ - virtual bool abortCalibration(); - - /* Abort parking, force the function park() to return.*/ - virtual bool abortPark(); - - - -}; - -/** - * @ingroup dev_iface_motor - * - * Interface for control devices, debug commands. - */ -class YARP_dev_API yarp::dev::IControlDebug -{ -public: - /** - * Destructor. - */ - virtual ~IControlDebug() {} - - /* Set the print function, pass here a pointer to your own function - * to print. This function should implement "printf" like parameters. - * @param a pointer to the print function - * @return I don't see good reasons why it should return false. - */ - virtual bool setPrintFunction(int (*f) (const char *fmt, ...))=0; - - /* Read the content of the board internal memory, this is usually done - * at boot time, but can be forced by calling this method. - * @return true/false on success failure - */ - virtual bool loadBootMemory()=0; - - /* Save the current board configuration to the internal memory, - * this values are read at boot time or if loadBootMemory() is called. - * @return true/false on success/failure - */ - virtual bool saveBootMemory()=0; -}; - -/** - * @ingroup dev_iface_motor - * - * Interface for control devices, commands to get/set position and veloity limits. - */ -class YARP_dev_API yarp::dev::IControlLimits -{ -public: - /** - * Destructor. - */ - virtual ~IControlLimits() {} - - /** - * Set the software limits for a particular axis, the behavior of the - * control card when these limits are exceeded, depends on the implementation. - * @param axis joint number (why am I telling you this) - * @param min the value of the lower limit - * @param max the value of the upper limit - * @return true or false on success or failure - */ - virtual bool setLimits(int axis, double min, double max)=0; - - /** - * Get the software limits for a particular axis. - * @param axis joint number - * @param pointer to store the value of the lower limit - * @param pointer to store the value of the upper limit - * @return true if everything goes fine, false otherwise. - */ - virtual bool getLimits(int axis, double *min, double *max)=0; - - /** - * Set the software speed limits for a particular axis, the behavior of the - * control card when these limits are exceeded, depends on the implementation. - * @param axis joint number - * @param min the value of the lower limit - * @param max the value of the upper limit - * @return true or false on success or failure - */ - virtual bool setVelLimits(int axis, double min, double max)=0; - - /** - * Get the software speed limits for a particular axis. - * @param axis joint number - * @param min pointer to store the value of the lower limit - * @param max pointer to store the value of the upper limit - * @return true if everything goes fine, false otherwise. - */ - virtual bool getVelLimits(int axis, double *min, double *max)=0; -}; - -/** - * Interface for control devices. Limits commands. - */ -class YARP_dev_API yarp::dev::IControlLimitsRaw -{ -public: - /** - * Destructor. - */ - virtual ~IControlLimitsRaw() {} - - /** - * Set the software limits for a particular axis, the behavior of the - * control card when these limits are exceeded, depends on the implementation. - * @param axis joint number (why am I telling you this) - * @param min the value of the lower limit - * @param max the value of the upper limit - * @return true or false on success or failure - */ - virtual bool setLimitsRaw(int axis, double min, double max)=0; - - /** - * Get the software limits for a particular axis. - * @param axis joint number - * @param pointer to store the value of the lower limit - * @param pointer to store the value of the upper limit - * @return true if everything goes fine, false otherwise. - */ - virtual bool getLimitsRaw(int axis, double *min, double *max)=0; - - /** - * Set the software speed limits for a particular axis, the behavior of the - * control card when these limits are exceeded, depends on the implementation. - * @param axis joint number - * @param min the value of the lower limit - * @param max the value of the upper limit - * @return true or false on success or failure - */ - virtual bool setVelLimitsRaw(int axis, double min, double max)=0; - - /** - * Get the software speed limits for a particular axis. - * @param axis joint number - * @param min pointer to store the value of the lower limit - * @param max pointer to store the value of the upper limit - * @return true if everything goes fine, false otherwise. - */ - virtual bool getVelLimitsRaw(int axis, double *min, double *max)=0; -}; - -/** - * @ingroup dev_iface_motor - * - * Interface for getting information about specific axes, if available. - */ -class YARP_dev_API yarp::dev::IAxisInfo -{ -public: - /** - * Destructor. - */ - virtual ~IAxisInfo() {} - - /* Get the name for a particular axis. - * @param axis joint number - * @param name the axis name - * @return true if everything goes fine, false otherwise. - */ - virtual bool getAxisName(int axis, std::string& name) = 0; - - /* Get the joint type (e.g. revolute/prismatic) for a particular axis. - * @param axis joint number - * @param type the joint type - * @return true if everything goes fine, false otherwise. - */ - virtual bool getJointType(int axis, yarp::dev::JointTypeEnum& type) { yFatal("getJointType() not implemented on your device, cannot proceed further. Please report the problem on yarp issue tracker"); return false; } -}; - -/** -* Interface for getting information about specific axes, if available. -*/ -class YARP_dev_API yarp::dev::IAxisInfoRaw -{ -public: - /** - * Destructor. - */ - virtual ~IAxisInfoRaw() {} - - /* Get the name for a particular axis. - * @param axis joint number - * @param name the axis name - * @return true if everything goes fine, false otherwise. - */ - virtual bool getAxisNameRaw(int axis, std::string& name) = 0; - - /* Get the joint type (e.g. revolute/prismatic) for a particular axis. - * @param axis joint number - * @param type the joint type - * @return true if everything goes fine, false otherwise. - */ - virtual bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum& type) { yFatal("getJointType() not implemented on your device, cannot proceed further. Please report the problem on yarp issue tracker"); return false; }; -}; +#include +#include +#include +#include +#include /* Vocabs representing the above interfaces */ - -#define VOCAB_CALIBRATE_JOINT VOCAB4('c','a','l','j') -#define VOCAB_CALIBRATE_JOINT_PARAMS VOCAB4('c','l','j','p') -#define VOCAB_CALIBRATE VOCAB3('c','a','l') -#define VOCAB_ABORTCALIB VOCAB4('a','b','c','a') -#define VOCAB_ABORTPARK VOCAB4('a','b','p','a') -#define VOCAB_CALIBRATE_DONE VOCAB4('c','a','l','d') -#define VOCAB_PARK VOCAB4('p','a','r','k') -#define VOCAB_SET VOCAB3('s','e','t') -#define VOCAB_GET VOCAB3('g','e','t') -#define VOCAB_IS VOCAB2('i','s') -#define VOCAB_FAILED VOCAB4('f','a','i','l') -#define VOCAB_OK VOCAB2('o','k') - -#define VOCAB_OFFSET VOCAB3('o', 'f', 'f') - -// interface IPidControl sets. -#define VOCAB_PID VOCAB3('p','i','d') -#define VOCAB_PIDS VOCAB4('p','i','d','s') -#define VOCAB_REF VOCAB3('r','e','f') -#define VOCAB_REFS VOCAB4('r','e','f','s') -#define VOCAB_REFG VOCAB4('r','e','f','g') -#define VOCAB_LIM VOCAB3('l','i','m') -#define VOCAB_LIMS VOCAB4('l','i','m','s') -#define VOCAB_RESET VOCAB3('r','e','s') -#define VOCAB_DISABLE VOCAB3('d','i','s') -#define VOCAB_ENABLE VOCAB3('e','n','a') - -// interface IPidControl gets. -#define VOCAB_ERR VOCAB3('e','r','r') -#define VOCAB_ERRS VOCAB4('e','r','r','s') -#define VOCAB_OUTPUT VOCAB3('o','u','t') -#define VOCAB_OUTPUTS VOCAB4('o','u','t','s') -#define VOCAB_REFERENCE VOCAB3('r','e','f') -#define VOCAB_REFERENCES VOCAB4('r','e','f','s') - -// interface IPositionControl gets -#define VOCAB_AXES VOCAB4('a','x','e','s') -#define VOCAB_MOTION_DONE VOCAB3('d','o','n') -#define VOCAB_MOTION_DONES VOCAB4('d','o','n','s') +constexpr yarp::conf::vocab32_t VOCAB_MOTION_DONE = yarp::os::createVocab('d','o','n'); +constexpr yarp::conf::vocab32_t VOCAB_MOTION_DONES = yarp::os::createVocab('d','o','n','s'); +constexpr yarp::conf::vocab32_t VOCAB_TIMESTAMP = yarp::os::createVocab('t', 's', 't', 'a'); // interface IPositionControl sets -#define VOCAB_POSITION_MODE VOCAB4('p','o','s','d') -#define VOCAB_POSITION_MOVE VOCAB3('p','o','s') -#define VOCAB_POSITION_MOVES VOCAB4('p','o','s','s') -#define VOCAB_RELATIVE_MOVE VOCAB3('r','e','l') - -#define VOCAB_RELATIVE_MOVES VOCAB4('r','e','l','s') -#define VOCAB_REF_SPEED VOCAB3('v','e','l') -#define VOCAB_REF_SPEEDS VOCAB4('v','e','l','s') -#define VOCAB_REF_ACCELERATION VOCAB3('a','c','c') -#define VOCAB_REF_ACCELERATIONS VOCAB4('a','c','c','s') -#define VOCAB_STOP VOCAB3('s','t','o') -#define VOCAB_STOPS VOCAB4('s','t','o','s') +constexpr yarp::conf::vocab32_t VOCAB_POSITION_MODE = yarp::os::createVocab('p','o','s','d'); +constexpr yarp::conf::vocab32_t VOCAB_POSITION_MOVE = yarp::os::createVocab('p','o','s'); +constexpr yarp::conf::vocab32_t VOCAB_POSITION_MOVES = yarp::os::createVocab('p','o','s','s'); +constexpr yarp::conf::vocab32_t VOCAB_RELATIVE_MOVE = yarp::os::createVocab('r','e','l'); + +constexpr yarp::conf::vocab32_t VOCAB_RELATIVE_MOVES = yarp::os::createVocab('r','e','l','s'); +constexpr yarp::conf::vocab32_t VOCAB_REF_SPEED = yarp::os::createVocab('v','e','l'); +constexpr yarp::conf::vocab32_t VOCAB_REF_SPEEDS = yarp::os::createVocab('v','e','l','s'); +constexpr yarp::conf::vocab32_t VOCAB_REF_ACCELERATION = yarp::os::createVocab('a','c','c'); +constexpr yarp::conf::vocab32_t VOCAB_REF_ACCELERATIONS = yarp::os::createVocab('a','c','c','s'); +constexpr yarp::conf::vocab32_t VOCAB_STOP = yarp::os::createVocab('s','t','o'); +constexpr yarp::conf::vocab32_t VOCAB_STOPS = yarp::os::createVocab('s','t','o','s'); // interface IVelocityControl sets -#define VOCAB_VELOCITY_MODE VOCAB4('v','e','l','d') -#define VOCAB_VELOCITY_MOVE VOCAB3('v','m','o') -#define VOCAB_VELOCITY_MOVES VOCAB4('v','m','o','s') - -// interface IAmplifierControl sets/gets -#define VOCAB_AMP_ENABLE VOCAB3('a','e','n') -#define VOCAB_AMP_DISABLE VOCAB3('a','d','i') -#define VOCAB_AMP_STATUS VOCAB4('a','s','t','a') -#define VOCAB_AMP_STATUS_SINGLE VOCAB4('a','s','t','s') -#define VOCAB_AMP_CURRENT VOCAB3('a','c','u') -#define VOCAB_AMP_CURRENTS VOCAB4('a','c','u','s') -#define VOCAB_AMP_MAXCURRENT VOCAB4('m','a','x','c') -#define VOCAB_AMP_NOMINAL_CURRENT VOCAB4('a','c','n','o') -#define VOCAB_AMP_PEAK_CURRENT VOCAB4('a','c','p','k') -#define VOCAB_AMP_PWM VOCAB3('p','w','m') -#define VOCAB_AMP_PWM_LIMIT VOCAB4('p','w','m','l') -#define VOCAB_AMP_VOLTAGE_SUPPLY VOCAB4('a','v','s','u') - -// interface IControlLimits sets/gets -#define VOCAB_LIMITS VOCAB4('l','l','i','m') -#define VOCAB_VEL_LIMITS VOCAB4('v','l','i','m') - - -// interface IAxisInfo -#define VOCAB_INFO_NAME VOCAB4('n','a','m','e') -#define VOCAB_INFO_TYPE VOCAB4('t','y','p','e') - -#define VOCAB_TIMESTAMP VOCAB4('t','s','t','a') -#define VOCAB_TORQUE VOCAB4('t','o','r','q') -#define VOCAB_TORQUE_MODE VOCAB4('t','r','q','d') -#define VOCAB_TRQS VOCAB4('t','r','q','s') -#define VOCAB_TRQ VOCAB3('t','r','q') -#define VOCAB_BEMF VOCAB3('b','m','f') -#define VOCAB_MOTOR_PARAMS VOCAB4('m','t','p','s') -#define VOCAB_RANGES VOCAB4('r','n','g','s') -#define VOCAB_RANGE VOCAB3('r','n','g') -#define VOCAB_IMP_PARAM VOCAB3('i','p','r') -#define VOCAB_IMP_OFFSET VOCAB3('i','o','f') - -#define VOCAB_TORQUES_DIRECTS VOCAB4('d','t','q','s') //This implements the setRefTorques for the whole part -#define VOCAB_TORQUES_DIRECT VOCAB3('d','t','q') //This implements the setRefTorque for a single joint -#define VOCAB_TORQUES_DIRECT_GROUP VOCAB4('d','t','q','g') //This implements the setRefTorques with joint list +constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MODE = yarp::os::createVocab('v','e','l','d'); +constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVE = yarp::os::createVocab('v','m','o'); +constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVES = yarp::os::createVocab('v','m','o','s'); // protocol version -#define VOCAB_PROTOCOL_VERSION VOCAB('p', 'r', 'o', 't') +constexpr yarp::conf::vocab32_t VOCAB_PROTOCOL_VERSION = yarp::os::createVocab('p', 'r', 'o', 't'); #endif // YARP_DEV_CONTROLBOARDINTERFACES_H diff --git a/src/libYARP_dev/include/yarp/dev/ControlBoardInterfacesImpl-inl.h b/src/libYARP_dev/include/yarp/dev/ControlBoardInterfacesImpl-inl.h deleted file mode 100644 index 972a21cbb9..0000000000 --- a/src/libYARP_dev/include/yarp/dev/ControlBoardInterfacesImpl-inl.h +++ /dev/null @@ -1,471 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * Copyright (C) 2006-2010 RobotCub Consortium - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#include -#include - -#include - -// Be careful: this file contains template implementations and is included by translation -// units that use the template (e.g. .cpp files). Avoid putting here non-template functions to -// avoid multiple definitions. - -using namespace yarp::dev; - -//////////////////////// -// Encoder Interface Implementation -template -ImplementEncoders::ImplementEncoders(DERIVED *y) -{ - iEncoders= dynamic_cast (y); - helper = 0; - temp=0; -} - -template -ImplementEncoders::~ImplementEncoders() -{ - uninitialize(); -} - -template -bool ImplementEncoders:: initialize (int size, const int *amap, const double *enc, const double *zos) -{ - if (helper!=0) - return false; - - helper=(void *)(new ControlBoardHelper(size, amap, enc, zos)); - yAssert (helper != 0); - temp=new double [size]; - yAssert (temp != 0); - return true; -} - -/** -* Clean up internal data and memory. -* @return true if uninitialization is executed, false otherwise. -*/ -template -bool ImplementEncoders::uninitialize () -{ - if (helper!=0) - { - delete castToMapper(helper); - helper=0; - } - - checkAndDestroy(temp); - - return true; -} - -template -bool ImplementEncoders::getAxes(int *ax) -{ - (*ax)=castToMapper(helper)->axes(); - return true; -} - -template -bool ImplementEncoders::resetEncoder(int j) -{ - int k; - k=castToMapper(helper)->toHw(j); - - return iEncoders->resetEncoderRaw(k); -} - - -template -bool ImplementEncoders::resetEncoders() -{ - return iEncoders->resetEncodersRaw(); -} - -template -bool ImplementEncoders::setEncoder(int j, double val) -{ - int k; - double enc; - - castToMapper(helper)->posA2E(val, j, enc, k); - - return iEncoders->setEncoderRaw(k, enc); -} - -template -bool ImplementEncoders::setEncoders(const double *val) -{ - castToMapper(helper)->posA2E(val, temp); - - return iEncoders->setEncodersRaw(temp); -} - -template -bool ImplementEncoders::getEncoder(int j, double *v) -{ - int k; - double enc; - bool ret; - - k=castToMapper(helper)->toHw(j); - - ret=iEncoders->getEncoderRaw(k, &enc); - - *v=castToMapper(helper)->posE2A(enc, k); - - return ret; -} - -template -bool ImplementEncoders::getEncoders(double *v) -{ - bool ret; - castToMapper(helper)->axes(); - - ret=iEncoders->getEncodersRaw(temp); - - castToMapper(helper)->posE2A(temp, v); - - return ret; -} - -template -bool ImplementEncoders::getEncoderSpeed(int j, double *v) -{ - int k; - double enc; - bool ret; - - k=castToMapper(helper)->toHw(j); - - ret=iEncoders->getEncoderSpeedRaw(k, &enc); - - *v=castToMapper(helper)->velE2A(enc, k); - - return ret; -} - -template -bool ImplementEncoders::getEncoderSpeeds(double *v) -{ - bool ret; - ret=iEncoders->getEncoderSpeedsRaw(temp); - - castToMapper(helper)->velE2A(temp, v); - - return ret; -} - -template -bool ImplementEncoders::getEncoderAcceleration(int j, double *v) -{ - int k; - double enc; - bool ret; - - k=castToMapper(helper)->toHw(j); - - ret=iEncoders->getEncoderAccelerationRaw(k, &enc); - - *v=castToMapper(helper)->accE2A(enc, k); - - return ret; -} - -template -bool ImplementEncoders::getEncoderAccelerations(double *v) -{ - bool ret; - ret=iEncoders->getEncoderAccelerationsRaw(temp); - - castToMapper(helper)->accE2A(temp, v); - - return ret; -} - -//////////////////////// -// ControlCalibration Interface Implementation -template -ImplementControlCalibration::ImplementControlCalibration(DERIVED *y) -{ - iCalibrate= dynamic_cast (y); - helper = 0; - temp=0; -} - -template -ImplementControlCalibration::~ImplementControlCalibration() -{ - uninitialize(); -} - -template -bool ImplementControlCalibration:: initialize (int size, const int *amap, const double *enc, const double *zos) -{ - if (helper!=0) - return false; - - helper=(void *)(new ControlBoardHelper(size, amap, enc, zos)); - yAssert (helper != 0); - temp=new double [size]; - yAssert (temp != 0); - return true; -} - -/** -* Clean up internal data and memory. -* @return true if uninitialization is executed, false otherwise. -*/ -template -bool ImplementControlCalibration::uninitialize () -{ - if (helper!=0) - { - delete castToMapper(helper); - helper=0; - } - - checkAndDestroy(temp); - - return true; -} - -#ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 -template -bool ImplementControlCalibration::calibrate(int j, double p) -{ - int k=castToMapper(helper)->toHw(j); - - return iCalibrate->calibrateRaw(k, p); -} -#endif - -template -bool ImplementControlCalibration::done(int j) -{ - int k=castToMapper(helper)->toHw(j); - - return iCalibrate->doneRaw(k); -} - -template -bool ImplementControlCalibration::calibrate(int axis, unsigned int type, double p1, double p2, double p3) -{ - int k=castToMapper(helper)->toHw(axis); - - return iCalibrate->calibrateRaw(k, type, p1, p2, p3); -} - -template -bool ImplementControlCalibration::setCalibrationParameters(int axis, const CalibrationParameters& params) -{ - int k = castToMapper(helper)->toHw(axis); - - return iCalibrate->setCalibrationParametersRaw(k, params); -} - -////////////////////////////// -///////////////// Implement -template -ImplementAmplifierControl::ImplementAmplifierControl(DERIVED *y) -{ - iAmplifier= dynamic_cast (y); - helper = 0; - dTemp=0; - iTemp=0; -} - -template -ImplementAmplifierControl::~ImplementAmplifierControl() -{ - uninitialize(); -} - -template -bool ImplementAmplifierControl:: initialize (int size, const int *amap, const double *enc, const double *zos, const double *ampereFactor, const double *voltFactor) -{ - if (helper!=0) - return false; - - helper=(void *)(new ControlBoardHelper(size, amap, enc, zos,nullptr, ampereFactor, voltFactor)); - yAssert (helper != 0); - dTemp=new double[size]; - yAssert (dTemp != 0); - iTemp=new int[size]; - yAssert (iTemp != 0); - - return true; -} - -/** -* Clean up internal data and memory. -* @return true if uninitialization is executed, false otherwise. -*/ -template -bool ImplementAmplifierControl::uninitialize () -{ - if (helper!=0) - delete castToMapper(helper); - - delete [] dTemp; - delete [] iTemp; - - helper=0; - dTemp=0; - iTemp=0; - return true; -} - -template -bool ImplementAmplifierControl::enableAmp(int j) -{ - int k=castToMapper(helper)->toHw(j); - - return iAmplifier->enableAmpRaw(k); -} - -template -bool ImplementAmplifierControl::disableAmp(int j) -{ - int k=castToMapper(helper)->toHw(j); - - return iAmplifier->disableAmpRaw(k); -} - -template -bool ImplementAmplifierControl::getCurrents(double *currs) -{ - bool ret=iAmplifier->getCurrentsRaw(dTemp); - castToMapper(helper)->ampereS2A(dTemp, currs); - return ret; -} - -template -bool ImplementAmplifierControl::getCurrent(int j, double *c) -{ - double temp = 0; - int k = castToMapper(helper)->toHw(j); - bool ret = iAmplifier->getCurrentRaw(k, &temp); - castToMapper(helper)->ampereS2A(temp, k, *c, j); - return ret; -} - -template -bool ImplementAmplifierControl::getAmpStatus(int *st) -{ - bool ret=iAmplifier->getAmpStatusRaw(iTemp); - castToMapper(helper)->toUser(iTemp, st); - - return ret; -} - -template -bool ImplementAmplifierControl::getAmpStatus(int k, int *st) -{ - int j=castToMapper(helper)->toHw(k); - bool ret=iAmplifier->getAmpStatusRaw(j, st); - - return ret; -} - -template -bool ImplementAmplifierControl::setMaxCurrent(int m, double v) -{ - int k; - double curr; - castToMapper(helper)->ampereA2S(v, m, curr, k); - return iAmplifier->setMaxCurrentRaw(k, curr); -} - -template -bool ImplementAmplifierControl::getMaxCurrent(int j, double* v) -{ - double val; - int k=castToMapper(helper)->toHw(j); - bool ret = iAmplifier->getMaxCurrentRaw(k, &val); - *v = castToMapper(helper)->ampereS2A(val, k); - return ret; -} - -template -bool ImplementAmplifierControl::getNominalCurrent(int m, double *curr) -{ - int k; - bool ret; - double tmp; - - k=castToMapper(helper)->toHw(m); - ret=iAmplifier->getNominalCurrentRaw(k, &tmp); - *curr=castToMapper(helper)->ampereS2A(tmp, k); - return ret; -} - -template -bool ImplementAmplifierControl::getPeakCurrent(int m, double *curr) -{ - int k; - bool ret; - double tmp; - - k=castToMapper(helper)->toHw(m); - ret=iAmplifier->getPeakCurrentRaw(k, &tmp); - *curr=castToMapper(helper)->ampereS2A(tmp, k); - return ret; -} - -template -bool ImplementAmplifierControl::setPeakCurrent(int m, const double curr) -{ - int k; - double val; - castToMapper(helper)->ampereA2S(curr, m, val, k); - return iAmplifier->setPeakCurrentRaw(k, val); -} - -template -bool ImplementAmplifierControl::setNominalCurrent(int m, const double curr) -{ - int k; - double val; - castToMapper(helper)->ampereA2S(curr, m, val, k); - return iAmplifier->setNominalCurrentRaw(k, val); -} - -template -bool ImplementAmplifierControl::getPWM(int m, double* pwm) -{ - int k; - k=castToMapper(helper)->toHw(m); - return iAmplifier->getPWMRaw(k, pwm); -} - -template -bool ImplementAmplifierControl::getPWMLimit(int m, double* limit) -{ - int k; - k=castToMapper(helper)->toHw(m); - return iAmplifier->getPWMLimitRaw(k, limit); -} - -template -bool ImplementAmplifierControl::setPWMLimit(int m, const double limit) -{ - int k; - k=castToMapper(helper)->toHw(m); - return iAmplifier->setPWMLimitRaw(k, limit); -} - -template -bool ImplementAmplifierControl::getPowerSupplyVoltage(int m, double *voltage) -{ - int k; - k=castToMapper(helper)->toHw(m); - return iAmplifier->getPowerSupplyVoltageRaw(k, voltage); -} diff --git a/src/libYARP_dev/include/yarp/dev/ControlBoardInterfacesImpl.h b/src/libYARP_dev/include/yarp/dev/ControlBoardInterfacesImpl.h index d5705e927c..60031cb811 100644 --- a/src/libYARP_dev/include/yarp/dev/ControlBoardInterfacesImpl.h +++ b/src/libYARP_dev/include/yarp/dev/ControlBoardInterfacesImpl.h @@ -23,12 +23,15 @@ #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // old interface implementation file #include diff --git a/src/libYARP_dev/include/yarp/dev/FrameGrabberInterfaces.h b/src/libYARP_dev/include/yarp/dev/FrameGrabberInterfaces.h index 95ee2da8c3..f2550807ab 100644 --- a/src/libYARP_dev/include/yarp/dev/FrameGrabberInterfaces.h +++ b/src/libYARP_dev/include/yarp/dev/FrameGrabberInterfaces.h @@ -37,46 +37,38 @@ typedef struct { /* * Vocab for interfaces */ -#define VOCAB_FRAMEGRABBER_IMAGE VOCAB3('f','g','i') -#define VOCAB_FRAMEGRABBER_IMAGERAW VOCAB4('f','g','i','r') +constexpr yarp::conf::vocab32_t VOCAB_FRAMEGRABBER_IMAGE = yarp::os::createVocab('f','g','i'); +constexpr yarp::conf::vocab32_t VOCAB_FRAMEGRABBER_IMAGERAW = yarp::os::createVocab('f','g','i','r'); /* * Generic capabilities defines */ -#define VOCAB_BRIGHTNESS VOCAB3('b','r','i') -#define VOCAB_EXPOSURE VOCAB4('e','x','p','o') -#define VOCAB_SHARPNESS VOCAB4('s','h','a','r') -#define VOCAB_WHITE VOCAB4('w','h','i','t') -#define VOCAB_HUE VOCAB3('h','u','e') -#define VOCAB_SATURATION VOCAB4('s','a','t','u') -#define VOCAB_GAMMA VOCAB4('g','a','m','m') -#define VOCAB_SHUTTER VOCAB4('s','h','u','t') -#define VOCAB_GAIN VOCAB4('g','a','i','n') -#define VOCAB_IRIS VOCAB4('i','r','i','s') +constexpr yarp::conf::vocab32_t VOCAB_BRIGHTNESS = yarp::os::createVocab('b','r','i'); +constexpr yarp::conf::vocab32_t VOCAB_EXPOSURE = yarp::os::createVocab('e','x','p','o'); +constexpr yarp::conf::vocab32_t VOCAB_SHARPNESS = yarp::os::createVocab('s','h','a','r'); +constexpr yarp::conf::vocab32_t VOCAB_WHITE = yarp::os::createVocab('w','h','i','t'); +constexpr yarp::conf::vocab32_t VOCAB_HUE = yarp::os::createVocab('h','u','e'); +constexpr yarp::conf::vocab32_t VOCAB_SATURATION = yarp::os::createVocab('s','a','t','u'); +constexpr yarp::conf::vocab32_t VOCAB_GAMMA = yarp::os::createVocab('g','a','m','m'); +constexpr yarp::conf::vocab32_t VOCAB_SHUTTER = yarp::os::createVocab('s','h','u','t'); +constexpr yarp::conf::vocab32_t VOCAB_GAIN = yarp::os::createVocab('g','a','i','n'); +constexpr yarp::conf::vocab32_t VOCAB_IRIS = yarp::os::createVocab('i','r','i','s'); // General usage vocabs -#define VOCAB_SET VOCAB3('s','e','t') -#define VOCAB_GET VOCAB3('g','e','t') -#define VOCAB_IS VOCAB2('i','s') -#define VOCAB_WIDTH VOCAB1('w') -#define VOCAB_HEIGHT VOCAB1('h') - -#define VOCAB_CROP VOCAB4('c','r','o','p') - - -#define VOCAB_FRAMEGRABBER_CONTROL VOCAB3('f','g','c') -#define VOCAB_FRAMEGRABBER_CONTROL_DC1394 VOCAB4('f','g','f','w') -#define VOCAB_CAMERA_DESCRIPTION VOCAB4('c','a','m','d') -#define VOCAB_HAS VOCAB3('h','a','s') -#define VOCAB_FEATURE VOCAB4('f','e','a','t') -#define VOCAB_FEATURE2 VOCAB4('f','e','a','2') -#define VOCAB_ONOFF VOCAB4('o','n','o','f') -#define VOCAB_AUTO VOCAB4('a','u','t','o') -#define VOCAB_MANUAL VOCAB3('m','a','n') -#define VOCAB_ONEPUSH VOCAB4('o','n','e','p') -#define VOCAB_ACTIVE VOCAB4('a','c','t','v') -#define VOCAB_MODE VOCAB4('m','o','d','e') +constexpr yarp::conf::vocab32_t VOCAB_CROP = yarp::os::createVocab('c','r','o','p'); +constexpr yarp::conf::vocab32_t VOCAB_FRAMEGRABBER_CONTROL = yarp::os::createVocab('f','g','c'); +constexpr yarp::conf::vocab32_t VOCAB_FRAMEGRABBER_CONTROL_DC1394 = yarp::os::createVocab('f','g','f','w'); +constexpr yarp::conf::vocab32_t VOCAB_CAMERA_DESCRIPTION = yarp::os::createVocab('c','a','m','d'); +constexpr yarp::conf::vocab32_t VOCAB_HAS = yarp::os::createVocab('h','a','s'); +constexpr yarp::conf::vocab32_t VOCAB_FEATURE = yarp::os::createVocab('f','e','a','t'); +constexpr yarp::conf::vocab32_t VOCAB_FEATURE2 = yarp::os::createVocab('f','e','a','2'); +constexpr yarp::conf::vocab32_t VOCAB_ONOFF = yarp::os::createVocab('o','n','o','f'); +constexpr yarp::conf::vocab32_t VOCAB_AUTO = yarp::os::createVocab('a','u','t','o'); +constexpr yarp::conf::vocab32_t VOCAB_MANUAL = yarp::os::createVocab('m','a','n'); +constexpr yarp::conf::vocab32_t VOCAB_ONEPUSH = yarp::os::createVocab('o','n','e','p'); +constexpr yarp::conf::vocab32_t VOCAB_ACTIVE = yarp::os::createVocab('a','c','t','v'); +constexpr yarp::conf::vocab32_t VOCAB_MODE = yarp::os::createVocab('m','o','d','e'); typedef enum { @@ -118,59 +110,49 @@ typedef enum { * For usage with IFrameGrabberControlsDC1394 interface */ -#define VOCAB_DRHASFEA VOCAB4('D','R','2','a') // 00 -#define VOCAB_DRSETVAL VOCAB4('D','R','2','b') // 01 -#define VOCAB_DRGETVAL VOCAB4('D','R','2','c') // 02 - -#define VOCAB_DRHASACT VOCAB4('D','R','2','d') // 03 -#define VOCAB_DRSETACT VOCAB4('D','R','2','e') // 04 -#define VOCAB_DRGETACT VOCAB4('D','R','2','f') // 05 - -#define VOCAB_DRHASMAN VOCAB4('D','R','2','g') // 06 -#define VOCAB_DRHASAUT VOCAB4('D','R','2','h') // 07 -#define VOCAB_DRHASONP VOCAB4('D','R','2','i') // 08 -#define VOCAB_DRSETMOD VOCAB4('D','R','2','j') // 09 -#define VOCAB_DRGETMOD VOCAB4('D','R','2','k') // 10 -#define VOCAB_DRSETONP VOCAB4('D','R','2','l') // 11 +constexpr yarp::conf::vocab32_t VOCAB_DRHASFEA = yarp::os::createVocab('D','R','2','a');// 00 +constexpr yarp::conf::vocab32_t VOCAB_DRSETVAL = yarp::os::createVocab('D','R','2','b');// 01 +constexpr yarp::conf::vocab32_t VOCAB_DRGETVAL = yarp::os::createVocab('D','R','2','c');// 02 +constexpr yarp::conf::vocab32_t VOCAB_DRHASACT = yarp::os::createVocab('D','R','2','d');// 03 +constexpr yarp::conf::vocab32_t VOCAB_DRSETACT = yarp::os::createVocab('D','R','2','e');// 04 +constexpr yarp::conf::vocab32_t VOCAB_DRGETACT = yarp::os::createVocab('D','R','2','f');// 05 +constexpr yarp::conf::vocab32_t VOCAB_DRHASMAN = yarp::os::createVocab('D','R','2','g');// 06 +constexpr yarp::conf::vocab32_t VOCAB_DRHASAUT = yarp::os::createVocab('D','R','2','h');// 07 +constexpr yarp::conf::vocab32_t VOCAB_DRHASONP = yarp::os::createVocab('D','R','2','i');// 08 +constexpr yarp::conf::vocab32_t VOCAB_DRSETMOD = yarp::os::createVocab('D','R','2','j');// 09 +constexpr yarp::conf::vocab32_t VOCAB_DRGETMOD = yarp::os::createVocab('D','R','2','k');// 10 +constexpr yarp::conf::vocab32_t VOCAB_DRSETONP = yarp::os::createVocab('D','R','2','l');// 11 // masks -#define VOCAB_DRGETMSK VOCAB4('D','R','2','m') // 12 -#define VOCAB_DRGETVMD VOCAB4('D','R','2','n') // 13 -#define VOCAB_DRSETVMD VOCAB4('D','R','2','o') // 14 - -#define VOCAB_DRGETFPM VOCAB4('D','R','2','p') // 15 -#define VOCAB_DRGETFPS VOCAB4('D','R','2','q') // 16 -#define VOCAB_DRSETFPS VOCAB4('D','R','2','r') // 17 - -#define VOCAB_DRGETISO VOCAB4('D','R','2','s') // 18 -#define VOCAB_DRSETISO VOCAB4('D','R','2','t') // 19 - -#define VOCAB_DRGETCCM VOCAB4('D','R','2','u') // 20 -#define VOCAB_DRGETCOD VOCAB4('D','R','2','v') // 21 -#define VOCAB_DRSETCOD VOCAB4('D','R','2','w') // 22 - -#define VOCAB_DRSETWHB VOCAB4('D','R','2','x') // 23 -#define VOCAB_DRGETWHB VOCAB4('D','R','2','y') // 24 - -#define VOCAB_DRGETF7M VOCAB4('D','R','2','z') // 25 -#define VOCAB_DRGETWF7 VOCAB4('D','R','2','A') // 26 -#define VOCAB_DRSETWF7 VOCAB4('D','R','2','B') // 27 - -#define VOCAB_DRSETOPM VOCAB4('D','R','2','C') // 28 -#define VOCAB_DRGETOPM VOCAB4('D','R','2','D') // 29 -#define VOCAB_DRSETTXM VOCAB4('D','R','2','E') // 30 -#define VOCAB_DRGETTXM VOCAB4('D','R','2','F') // 31 -//#define VOCAB_DRSETBAY VOCAB4('D','R','2','G') // 32 -//#define VOCAB_DRGETBAY VOCAB4('D','R','2','H') // 33 - -#define VOCAB_DRSETBCS VOCAB4('D','R','2','I') // 34 -#define VOCAB_DRSETDEF VOCAB4('D','R','2','J') // 35 -#define VOCAB_DRSETRST VOCAB4('D','R','2','K') // 36 -#define VOCAB_DRSETPWR VOCAB4('D','R','2','L') // 37 - -#define VOCAB_DRSETCAP VOCAB4('D','R','2','M') // 38 -#define VOCAB_DRSETBPP VOCAB4('D','R','2','N') // 39 -#define VOCAB_DRGETBPP VOCAB4('D','R','2','O') // 40 +constexpr yarp::conf::vocab32_t VOCAB_DRGETMSK = yarp::os::createVocab('D','R','2','m'); // 12 +constexpr yarp::conf::vocab32_t VOCAB_DRGETVMD = yarp::os::createVocab('D','R','2','n'); // 13 +constexpr yarp::conf::vocab32_t VOCAB_DRSETVMD = yarp::os::createVocab('D','R','2','o'); // 14 +constexpr yarp::conf::vocab32_t VOCAB_DRGETFPM = yarp::os::createVocab('D','R','2','p'); // 15 +constexpr yarp::conf::vocab32_t VOCAB_DRGETFPS = yarp::os::createVocab('D','R','2','q'); // 16 +constexpr yarp::conf::vocab32_t VOCAB_DRSETFPS = yarp::os::createVocab('D','R','2','r'); // 17 +constexpr yarp::conf::vocab32_t VOCAB_DRGETISO = yarp::os::createVocab('D','R','2','s'); // 18 +constexpr yarp::conf::vocab32_t VOCAB_DRSETISO = yarp::os::createVocab('D','R','2','t'); // 19 +constexpr yarp::conf::vocab32_t VOCAB_DRGETCCM = yarp::os::createVocab('D','R','2','u'); // 20 +constexpr yarp::conf::vocab32_t VOCAB_DRGETCOD = yarp::os::createVocab('D','R','2','v'); // 21 +constexpr yarp::conf::vocab32_t VOCAB_DRSETCOD = yarp::os::createVocab('D','R','2','w'); // 22 +constexpr yarp::conf::vocab32_t VOCAB_DRSETWHB = yarp::os::createVocab('D','R','2','x'); // 23 +constexpr yarp::conf::vocab32_t VOCAB_DRGETWHB = yarp::os::createVocab('D','R','2','y'); // 24 +constexpr yarp::conf::vocab32_t VOCAB_DRGETF7M = yarp::os::createVocab('D','R','2','z'); // 25 +constexpr yarp::conf::vocab32_t VOCAB_DRGETWF7 = yarp::os::createVocab('D','R','2','A'); // 26 +constexpr yarp::conf::vocab32_t VOCAB_DRSETWF7 = yarp::os::createVocab('D','R','2','B'); // 27 +constexpr yarp::conf::vocab32_t VOCAB_DRSETOPM = yarp::os::createVocab('D','R','2','C'); // 28 +constexpr yarp::conf::vocab32_t VOCAB_DRGETOPM = yarp::os::createVocab('D','R','2','D'); // 29 +constexpr yarp::conf::vocab32_t VOCAB_DRSETTXM = yarp::os::createVocab('D','R','2','E'); // 30 +constexpr yarp::conf::vocab32_t VOCAB_DRGETTXM = yarp::os::createVocab('D','R','2','F'); // 31 + + +constexpr yarp::conf::vocab32_t VOCAB_DRSETBCS = yarp::os::createVocab('D','R','2','I'); // 34 +constexpr yarp::conf::vocab32_t VOCAB_DRSETDEF = yarp::os::createVocab('D','R','2','J'); // 35 +constexpr yarp::conf::vocab32_t VOCAB_DRSETRST = yarp::os::createVocab('D','R','2','K'); // 36 +constexpr yarp::conf::vocab32_t VOCAB_DRSETPWR = yarp::os::createVocab('D','R','2','L'); // 37 +constexpr yarp::conf::vocab32_t VOCAB_DRSETCAP = yarp::os::createVocab('D','R','2','M'); // 38 +constexpr yarp::conf::vocab32_t VOCAB_DRSETBPP = yarp::os::createVocab('D','R','2','N'); // 39 +constexpr yarp::conf::vocab32_t VOCAB_DRGETBPP = yarp::os::createVocab('D','R','2','O'); // 40 namespace yarp{ namespace dev { @@ -522,7 +504,7 @@ class YARP_dev_API IFrameGrabberControls virtual double getIris() { return -1.0; } #endif - cameraFeature_id_t featureVocab2Enum(int vocab) + cameraFeature_id_t featureVOCABEnum(int vocab) { switch (vocab) { case VOCAB_BRIGHTNESS: diff --git a/src/libYARP_dev/include/yarp/dev/GenericVocabs.h b/src/libYARP_dev/include/yarp/dev/GenericVocabs.h new file mode 100644 index 0000000000..65eb7b134e --- /dev/null +++ b/src/libYARP_dev/include/yarp/dev/GenericVocabs.h @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2010 RobotCub Consortium + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#ifndef YARP_DEV_GENERIC_VOCABS_H +#define YARP_DEV_GENERIC_VOCABS_H + +#include +// Common +constexpr yarp::conf::vocab32_t VOCAB_SET = yarp::os::createVocab('s','e','t'); +constexpr yarp::conf::vocab32_t VOCAB_GET = yarp::os::createVocab('g','e','t'); +constexpr yarp::conf::vocab32_t VOCAB_IS = yarp::os::createVocab('i','s'); +constexpr yarp::conf::vocab32_t VOCAB_OK = yarp::os::createVocab('o','k'); +constexpr yarp::conf::vocab32_t VOCAB_FAILED = yarp::os::createVocab('f','a','i','l'); +constexpr yarp::conf::vocab32_t VOCAB_ERR = yarp::os::createVocab('e','r','r'); +constexpr yarp::conf::vocab32_t VOCAB_ERRS = yarp::os::createVocab('e','r','r','s'); +constexpr yarp::conf::vocab32_t VOCAB_HELP = yarp::os::createVocab('h','e','l','p'); +constexpr yarp::conf::vocab32_t VOCAB_QUIT = yarp::os::createVocab('q','u','i','t'); +constexpr yarp::conf::vocab32_t VOCAB_NOT = yarp::os::createVocab('n','o','t'); +constexpr yarp::conf::vocab32_t VOCAB_REMOVE = yarp::os::createVocab('r','m'); +constexpr yarp::conf::vocab32_t VOCAB_OFFSET = yarp::os::createVocab('o', 'f', 'f'); +constexpr yarp::conf::vocab32_t VOCAB_REF = yarp::os::createVocab('r','e','f'); +constexpr yarp::conf::vocab32_t VOCAB_REFS = yarp::os::createVocab('r','e','f','s'); +constexpr yarp::conf::vocab32_t VOCAB_REFG = yarp::os::createVocab('r','e','f','g'); +constexpr yarp::conf::vocab32_t VOCAB_LIM = yarp::os::createVocab('l','i','m'); +constexpr yarp::conf::vocab32_t VOCAB_LIMS = yarp::os::createVocab('l','i','m','s'); +constexpr yarp::conf::vocab32_t VOCAB_RESET = yarp::os::createVocab('r','e','s'); +constexpr yarp::conf::vocab32_t VOCAB_DISABLE = yarp::os::createVocab('d','i','s'); +constexpr yarp::conf::vocab32_t VOCAB_ENABLE = yarp::os::createVocab('e','n','a'); +constexpr yarp::conf::vocab32_t VOCAB_OUTPUT = yarp::os::createVocab('o','u','t'); +constexpr yarp::conf::vocab32_t VOCAB_OUTPUTS = yarp::os::createVocab('o','u','t','s'); +constexpr yarp::conf::vocab32_t VOCAB_REFERENCE = yarp::os::createVocab('r','e','f'); +constexpr yarp::conf::vocab32_t VOCAB_REFERENCES = yarp::os::createVocab('r','e','f','s'); +constexpr yarp::conf::vocab32_t VOCAB_AXES = yarp::os::createVocab('a','x','e','s'); +constexpr yarp::conf::vocab32_t VOCAB_COUNT = yarp::os::createVocab('c','n','t'); +constexpr yarp::conf::vocab32_t VOCAB_VALUE = yarp::os::createVocab('v','a','l'); + +// Image, matrix etc +constexpr yarp::conf::vocab32_t VOCAB_WIDTH = yarp::os::createVocab('w'); +constexpr yarp::conf::vocab32_t VOCAB_HEIGHT = yarp::os::createVocab('h'); + +#endif // YARP_DEV_GENERIC_VOCABS_H diff --git a/src/libYARP_dev/include/yarp/dev/IAmplifierControl.h b/src/libYARP_dev/include/yarp/dev/IAmplifierControl.h new file mode 100644 index 0000000000..aa46ed2e4b --- /dev/null +++ b/src/libYARP_dev/include/yarp/dev/IAmplifierControl.h @@ -0,0 +1,333 @@ +/* + * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2010 RobotCub Consortium + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#ifndef YARP_DEV_IAMPLIFIERCONTROL_H +#define YARP_DEV_IAMPLIFIERCONTROL_H + +#include + +/*! \file IAmplifierControl.h define control board standard interfaces*/ + +namespace yarp +{ + namespace dev + { + class IAmplifierControlRaw; + class IAmplifierControl; + } +} + +/** + * @ingroup dev_iface_motor + * + * Interface for control devices, amplifier commands. + */ +class YARP_dev_API yarp::dev::IAmplifierControl +{ +public: + /** + * Destructor. + */ + virtual ~IAmplifierControl() {} + + /** Enable the amplifier on a specific joint. Be careful, check that the output + * of the controller is appropriate (usually zero), to avoid + * generating abrupt movements. + * @return true/false on success/failure + */ + virtual bool enableAmp(int j)=0; + + /** Disable the amplifier on a specific joint. All computations within the board + * will be carried out normally, but the output will be disabled. + * @return true/false on success/failure + */ + virtual bool disableAmp(int j)=0; + + /* Get the status of the amplifiers, coded in a 32 bits integer for + * each amplifier (at the moment contains only the fault, it will be + * expanded in the future). + * @param st pointer to storage + * @return true in good luck, false otherwise. + */ + virtual bool getAmpStatus(int *st)=0; + + /* Get the status of a single amplifier, coded in a 32 bits integer + * @param j joint number + * @param st storage for return value + * @return true/false success failure. + */ + virtual bool getAmpStatus(int j, int *v)=0; + + /* Read the electric current going to all motors. + * @param vals pointer to storage for the output values + * @return hopefully true, false in bad luck. + */ + virtual bool getCurrents(double *vals)=0; + + /* Read the electric current going to a given motor. + * @param j motor number + * @param val pointer to storage for the output value + * @return probably true, might return false in bad times + */ + virtual bool getCurrent(int j, double *val)=0; + + /** + * Returns the maximum electric current allowed for a given motor. + * Exceeding this value will trigger instantaneous hardware fault. + * @param j motor number + * @param v the return value + * @return probably true, might return false in bad times + */ + virtual bool getMaxCurrent(int j, double *v)=0; + + /* Set the maximum electric current going to a given motor. + * Exceeding this value will trigger instantaneous hardware fault. + * @param j motor number + * @param v the new value + * @return probably true, might return false in bad times + */ + virtual bool setMaxCurrent(int j, double v)=0; + + /* Get the nominal current which can be kept for an indefinite amount of time + * without harming the motor. This value is specific for each motor and it is typically + * found in its data-sheet. The units are Ampere. + * This value and the peak current may be used by the firmware to configure + * an I2T filter. + * @param j joint number + * @param val storage for return value. [Ampere] + * @return true/false success failure. + */ + virtual bool getNominalCurrent(int m, double *val) {return false;}; + + /* Set the nominal current which can be kept for an indefinite amount of time + * without harming the motor. This value is specific for each motor and it is typically + * found in its data-sheet. The units are Ampere. + * This value and the peak current may be used by the firmware to configure + * an I2T filter. + * @param j joint number + * @param val storage for return value. [Ampere] + * @return true/false success failure. + */ + virtual bool setNominalCurrent(int m, const double val) { return false; }; + + /* Get the peak current which causes damage to the motor if maintained + * for a long amount of time. + * The value is often found in the motor data-sheet, units are Ampere. + * This value and the nominal current may be used by the firmware to configure + * an I2T filter. + * @param j joint number + * @param val storage for return value. [Ampere] + * @return true/false success failure. + */ + virtual bool getPeakCurrent(int m, double *val) {return false;}; + + /* Set the peak current. This value which causes damage to the motor if maintained + * for a long amount of time. + * The value is often found in the motor data-sheet, units are Ampere. + * This value and the nominal current may be used by the firmware to configure + * an I2T filter. + * @param j joint number + * @param val storage for return value. [Ampere] + * @return true/false success failure. + */ + virtual bool setPeakCurrent(int m, const double val) {return false;}; + + /* Get the the current PWM value used to control the motor. + * The units are firmware dependent, either machine units or percentage. + * @param j joint number + * @param val filled with PWM value. + * @return true/false success failure. + */ + virtual bool getPWM(int j, double* val) {return false;}; + + /* Get the PWM limit for the given motor. + * The units are firmware dependent, either machine units or percentage. + * @param j joint number + * @param val filled with PWM limit value. + * @return true/false success failure. + */ + virtual bool getPWMLimit(int j, double* val) {return false;}; + + /* Set the PWM limit for the given motor. + * The units are firmware dependent, either machine units or percentage. + * @param j joint number + * @param val new value for the PWM limit. + * @return true/false success failure. + */ + virtual bool setPWMLimit(int j, const double val) {return false;}; + + /* Get the power source voltage for the given motor in Volt. + * @param j joint number + * @param val filled with return value. + * @return true/false success failure. + */ + virtual bool getPowerSupplyVoltage(int j, double* val) {return false;}; +}; + +/** + * + * Interface for control devices, amplifier commands. + */ +class YARP_dev_API yarp::dev::IAmplifierControlRaw +{ +public: + /** + * Destructor. + */ + virtual ~IAmplifierControlRaw() {} + + /** Enable the amplifier on a specific joint. Be careful, check that the output + * of the controller is appropriate (usually zero), to avoid + * generating abrupt movements. + * @return true/false on success/failure + */ + virtual bool enableAmpRaw(int j)=0; + + /** Disable the amplifier on a specific joint. All computations within the board + * will be carried out normally, but the output will be disabled. + * @return true/false on success/failure + */ + virtual bool disableAmpRaw(int j)=0; + + /* Get the status of the amplifiers, coded in a 32 bits integer for + * each amplifier (at the moment contains only the fault, it will be + * expanded in the future). + * @param st pointer to storage + * @return true/false success failure. + */ + virtual bool getAmpStatusRaw(int *st)=0; + + /* Get the status of a single amplifier, coded in a 32 bits integer + * @param j joint number + * @param st storage for return value + * @return true/false success failure. + */ + virtual bool getAmpStatusRaw(int j, int *st)=0; + + /* Read the electric current going to all motors. + * @param vals pointer to storage for the output values + * @return hopefully true, false in bad luck. + */ + virtual bool getCurrentsRaw(double *vals)=0; + + /* Read the electric current going to a given motor. + * @param j motor number + * @param val pointer to storage for the output value + * @return probably true, might return false in bad times + */ + virtual bool getCurrentRaw(int j, double *val)=0; + + /* Set the maximum electric current going to a given motor. + * Exceeding this value will trigger instantaneous hardware fault. + * @param j motor number + * @param v the new value + * @return probably true, might return false in bad times + */ + virtual bool setMaxCurrentRaw(int j, double v)=0; + + /** + * Returns the maximum electric current allowed for a given motor. + * Exceeding this value will trigger instantaneous hardware fault. + * @param j motor number + * @param v the return value + * @return probably true, might return false in bad times + */ + virtual bool getMaxCurrentRaw(int j, double *v)=0; + + /* Get the nominal current which can be kept for an indefinite amount of time + * without harming the motor. This value is specific for each motor and it is typically + * found in its data-sheet. The units are Ampere. + * This value and the peak current may be used by the firmware to configure + * an I2T filter. + * @param j joint number + * @param val storage for return value. [Ampere] + * @return true/false success failure. + */ + virtual bool getNominalCurrentRaw(int m, double *val) {return false;}; + + /* Set the nominal current which can be kept for an indefinite amount of time + * without harming the motor. This value is specific for each motor and it is typically + * found in its data-sheet. The units are Ampere. + * This value and the peak current may be used by the firmware to configure + * an I2T filter. + * @param j joint number + * @param val storage for return value. [Ampere] + * @return true/false success failure. + */ + virtual bool setNominalCurrentRaw(int m, const double val) { return false; }; + + /* Get the peak current which causes damage to the motor if maintained + * for a long amount of time. + * The value is often found in the motor data-sheet, units are Ampere. + * This value and the nominal current may be used by the firmware to configure + * an I2T filter. + * @param j joint number + * @param val storage for return value. [Ampere] + * @return true/false success failure. + */ + virtual bool getPeakCurrentRaw(int m, double *val) {return false;}; + + /* Set the peak current. This value which causes damage to the motor if maintained + * for a long amount of time. + * The value is often found in the motor data-sheet, units are Ampere. + * This value and the nominal current may be used by the firmware to configure + * an I2T filter. + * @param j joint number + * @param val storage for return value. [Ampere] + * @return true/false success failure. + */ + virtual bool setPeakCurrentRaw(int m, const double val) {return false;}; + + /* Get the current PWM value used to control the motor. + * The units are firmware dependent, either machine units or percentage. + * @param j joint number + * @param val filled with PWM value. + * @return true/false success failure. + */ + virtual bool getPWMRaw(int j, double* val) {return false;}; + + /* Get the PWM limit for the given motor. + * The units are firmware dependent, either machine units or percentage. + * @param j joint number + * @param val filled with PWM limit value. + * @return true/false success failure. + */ + virtual bool getPWMLimitRaw(int j, double* val) {return false;}; + + /* Set the PWM limit for the given motor. + * The units are firmware dependent, either machine units or percentage. + * @param j joint number + * @param val new value for the PWM limit. + * @return true/false success failure. + */ + virtual bool setPWMLimitRaw(int j, const double val) {return false;}; + + /* Get the power source voltage for the given motor in Volt. + * @param j joint number + * @param val filled with return value. [Volt] + * @return true/false success failure. + */ + virtual bool getPowerSupplyVoltageRaw(int j, double* val) {return false;}; +}; + +// interface IAmplifierControl sets/gets +constexpr yarp::conf::vocab32_t VOCAB_AMP_ENABLE = yarp::os::createVocab('a','e','n'); +constexpr yarp::conf::vocab32_t VOCAB_AMP_DISABLE = yarp::os::createVocab('a','d','i'); +constexpr yarp::conf::vocab32_t VOCAB_AMP_STATUS = yarp::os::createVocab('a','s','t','a'); +constexpr yarp::conf::vocab32_t VOCAB_AMP_STATUS_SINGLE = yarp::os::createVocab('a','s','t','s'); +constexpr yarp::conf::vocab32_t VOCAB_AMP_CURRENT = yarp::os::createVocab('a','c','u'); +constexpr yarp::conf::vocab32_t VOCAB_AMP_CURRENTS = yarp::os::createVocab('a','c','u','s'); +constexpr yarp::conf::vocab32_t VOCAB_AMP_MAXCURRENT = yarp::os::createVocab('m','a','x','c'); +constexpr yarp::conf::vocab32_t VOCAB_AMP_NOMINAL_CURRENT = yarp::os::createVocab('a','c','n','o'); +constexpr yarp::conf::vocab32_t VOCAB_AMP_PEAK_CURRENT = yarp::os::createVocab('a','c','p','k'); +constexpr yarp::conf::vocab32_t VOCAB_AMP_PWM = yarp::os::createVocab('p','w','m'); +constexpr yarp::conf::vocab32_t VOCAB_AMP_PWM_LIMIT = yarp::os::createVocab('p','w','m','l'); +constexpr yarp::conf::vocab32_t VOCAB_AMP_VOLTAGE_SUPPLY = yarp::os::createVocab('a','v','s','u'); + +#endif // YARP_DEV_IAMPLIFIERCONTROL_H diff --git a/src/libYARP_dev/include/yarp/dev/IAnalogSensor.h b/src/libYARP_dev/include/yarp/dev/IAnalogSensor.h index da8f0c9831..af318b4163 100644 --- a/src/libYARP_dev/include/yarp/dev/IAnalogSensor.h +++ b/src/libYARP_dev/include/yarp/dev/IAnalogSensor.h @@ -10,11 +10,11 @@ #ifndef YARP_DEV_IANALOGSENSOR_H #define YARP_DEV_IANALOGSENSOR_H +#include #include #include -#define VOCAB_IANALOG VOCAB4('i','a','n','a') -#define VOCAB_CALIBRATE_CHANNEL VOCAB4('c','a','l','c') +constexpr yarp::conf::vocab32_t VOCAB_IANALOG = yarp::os::createVocab('i','a','n','a'); /*! \file IAnalogSensor.h analog sensor interface */ namespace yarp { diff --git a/src/libYARP_dev/include/yarp/dev/IAxisInfo.h b/src/libYARP_dev/include/yarp/dev/IAxisInfo.h new file mode 100644 index 0000000000..eacfaf3862 --- /dev/null +++ b/src/libYARP_dev/include/yarp/dev/IAxisInfo.h @@ -0,0 +1,91 @@ +/* + * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2010 RobotCub Consortium + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#ifndef YARP_DEV_IAXISINFO_H +#define YARP_DEV_IAXISINFO_H + +#include + +/*! \file IAxisInfo.h define control board standard interfaces*/ + +namespace yarp +{ + namespace dev + { + class IAxisInfo; + class IAxisInfoRaw; + + enum JointTypeEnum + { + VOCAB_JOINTTYPE_REVOLUTE = yarp::os::createVocab('a', 't', 'r', 'v'), + VOCAB_JOINTTYPE_PRISMATIC = yarp::os::createVocab('a', 't', 'p', 'r'), + VOCAB_JOINTTYPE_UNKNOWN = yarp::os::createVocab('u', 'n', 'k', 'n') + }; + } +} + +/** + * @ingroup dev_iface_motor + * + * Interface for getting information about specific axes, if available. + */ +class YARP_dev_API yarp::dev::IAxisInfo +{ +public: + /** + * Destructor. + */ + virtual ~IAxisInfo() {} + + /* Get the name for a particular axis. + * @param axis joint number + * @param name the axis name + * @return true if everything goes fine, false otherwise. + */ + virtual bool getAxisName(int axis, std::string& name) = 0; + + /* Get the joint type (e.g. revolute/prismatic) for a particular axis. + * @param axis joint number + * @param type the joint type + * @return true if everything goes fine, false otherwise. + */ + virtual bool getJointType(int axis, yarp::dev::JointTypeEnum& type) { yFatal("getJointType() not implemented on your device, cannot proceed further. Please report the problem on yarp issue tracker"); return false; } +}; + +/** +* Interface for getting information about specific axes, if available. +*/ +class YARP_dev_API yarp::dev::IAxisInfoRaw +{ +public: + /** + * Destructor. + */ + virtual ~IAxisInfoRaw() {} + + /* Get the name for a particular axis. + * @param axis joint number + * @param name the axis name + * @return true if everything goes fine, false otherwise. + */ + virtual bool getAxisNameRaw(int axis, std::string& name) = 0; + + /* Get the joint type (e.g. revolute/prismatic) for a particular axis. + * @param axis joint number + * @param type the joint type + * @return true if everything goes fine, false otherwise. + */ + virtual bool getJointTypeRaw(int axis, yarp::dev::JointTypeEnum& type) { yFatal("getJointType() not implemented on your device, cannot proceed further. Please report the problem on yarp issue tracker"); return false; }; +}; + +// interface IAxisInfo +constexpr yarp::conf::vocab32_t VOCAB_INFO_NAME = yarp::os::createVocab('n','a','m','e'); +constexpr yarp::conf::vocab32_t VOCAB_INFO_TYPE = yarp::os::createVocab('t','y','p','e'); + +#endif // YARP_DEV_IAXISINFO_H diff --git a/src/libYARP_dev/include/yarp/dev/IBattery.h b/src/libYARP_dev/include/yarp/dev/IBattery.h index 612f384451..1c6c388504 100644 --- a/src/libYARP_dev/include/yarp/dev/IBattery.h +++ b/src/libYARP_dev/include/yarp/dev/IBattery.h @@ -11,9 +11,10 @@ #include #include +#include -#define VOCAB_IBATTERY VOCAB4('i','b','a','t') -#define VOCAB_BATTERY_INFO VOCAB4('b','t','n','f') +constexpr yarp::conf::vocab32_t VOCAB_IBATTERY = yarp::os::createVocab('i','b','a','t'); +constexpr yarp::conf::vocab32_t VOCAB_BATTERY_INFO = yarp::os::createVocab('b','t','n','f'); /*! \file Ibattery.h battery interface */ namespace yarp { diff --git a/src/libYARP_dev/include/yarp/dev/IControlCalibration.h b/src/libYARP_dev/include/yarp/dev/IControlCalibration.h new file mode 100644 index 0000000000..0b8b88415e --- /dev/null +++ b/src/libYARP_dev/include/yarp/dev/IControlCalibration.h @@ -0,0 +1,153 @@ +/* + * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2010 RobotCub Consortium + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#ifndef YARP_DEV_ICONTROLCALIBRATION_H +#define YARP_DEV_ICONTROLCALIBRATION_H + +#include +#include +#include + +namespace yarp { + namespace dev { + class IControlCalibrationRaw; +#ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 +YARP_DEPRECATED_TYPEDEF_MSG("Use yarp::dev::IControlCalibrationRaw instead") IControlCalibrationRaw IControlCalibration2Raw; +#endif + class IControlCalibration; +#ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 +YARP_DEPRECATED_TYPEDEF_MSG("Use yarp::dev::IControlCalibration instead") IControlCalibration IControlCalibration2; +#endif + struct CalibrationParameters; + } +} + +struct YARP_dev_API yarp::dev::CalibrationParameters +{ + unsigned int type; + double param1; + double param2; + double param3; + double param4; + double param5; + double paramZero; + CalibrationParameters() { type = 0; param1 = 0; param2 = 0; param3 = 0; param4 = 0; param5 = 0; paramZero = 0; } +}; + +/** + * + * Interface for control devices, calibration commands. + */ +class YARP_dev_API yarp::dev::IControlCalibrationRaw +{ +public: + IControlCalibrationRaw(){} + /** + * Destructor. + */ + virtual ~IControlCalibrationRaw() {} + + /** + * Check if the calibration is terminated, on a particular joint. + * Non blocking. + * @return true/false + */ + virtual bool calibrationDoneRaw(int j)=0; + + /** + * Start calibration, this method is very often platform + * specific. + * @return true/false on success failure + */ + virtual bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3)=0; + + /** + * Start calibration, this method is very often platform + * specific. + * @return true/false on success failure + */ + virtual bool setCalibrationParametersRaw(int axis, const CalibrationParameters& params) { return false; } + +}; + +/** + * @ingroup dev_iface_motor + * + * Interface for control devices, calibration commands. + */ +class YARP_dev_API yarp::dev::IControlCalibration +{ +private: + ICalibrator *calibrator; + +public: + IControlCalibration(); + /** + * Destructor. + */ + virtual ~IControlCalibration() {} + + /** + * Start calibration, this method is very often platform + * specific. + * @return true/false on success failure + */ + virtual bool calibrateAxisWithParams(int axis, unsigned int type, double p1, double p2, double p3)=0; + + /** + * Start calibration, this method is very often platform + * specific. + * @return true/false on success failure + */ + virtual bool setCalibrationParameters(int axis, const CalibrationParameters& params) { return false; } + + /** + * Check if the calibration is terminated, on a particular joint. + * Non blocking. + * @return true/false + */ + virtual bool calibrationDone(int j)=0; + + /** + * Set the calibrator object to be used to calibrate the robot. + * @param c pointer to the calibrator object + * @return true/false on success failure + */ + virtual bool setCalibrator(ICalibrator *c); + + /** + * Calibrate robot by using an external calibrator. The external + * calibrator must be previously set by calling the setCalibration() + * method. + * @return true/false on success failure + */ + virtual bool calibrateRobot(); + + virtual bool park(bool wait=true); + + /* Abort calibration, force the function calibrate() to return.*/ + virtual bool abortCalibration(); + + /* Abort parking, force the function park() to return.*/ + virtual bool abortPark(); +}; + + +/* Vocabs representing the above interfaces */ + +constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_JOINT = yarp::os::createVocab('c','a','l','j'); +constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_JOINT_PARAMS = yarp::os::createVocab('c','l','j','p'); +constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE = yarp::os::createVocab('c','a','l'); +constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_CHANNEL = yarp::os::createVocab('c','a','l','c'); +constexpr yarp::conf::vocab32_t VOCAB_ABORTCALIB = yarp::os::createVocab('a','b','c','a'); +constexpr yarp::conf::vocab32_t VOCAB_ABORTPARK = yarp::os::createVocab('a','b','p','a'); +constexpr yarp::conf::vocab32_t VOCAB_CALIBRATE_DONE = yarp::os::createVocab('c','a','l','d'); +constexpr yarp::conf::vocab32_t VOCAB_PARK = yarp::os::createVocab('p','a','r','k'); + +#endif // YARP_DEV_ICONTROLCALIBRATION_H diff --git a/src/libYARP_dev/include/yarp/dev/IControlDebug.h b/src/libYARP_dev/include/yarp/dev/IControlDebug.h new file mode 100644 index 0000000000..b1ed0d2a1b --- /dev/null +++ b/src/libYARP_dev/include/yarp/dev/IControlDebug.h @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2010 RobotCub Consortium + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#ifndef YARP_DEV_ICONTROLDEBUG_H +#define YARP_DEV_ICONTROLDEBUG_H + +#include + +/*! \file ControlDebug.h define control board standard interfaces*/ + +namespace yarp +{ + namespace dev + { + class IControlDebug; + } +} + +/** + * @ingroup dev_iface_motor + * + * Interface for control devices, debug commands. + */ +class YARP_dev_API yarp::dev::IControlDebug +{ +public: + /** + * Destructor. + */ + virtual ~IControlDebug() {} + + /* Set the print function, pass here a pointer to your own function + * to print. This function should implement "printf" like parameters. + * @param a pointer to the print function + * @return I don't see good reasons why it should return false. + */ + virtual bool setPrintFunction(int (*f) (const char *fmt, ...))=0; + + /* Read the content of the board internal memory, this is usually done + * at boot time, but can be forced by calling this method. + * @return true/false on success failure + */ + virtual bool loadBootMemory()=0; + + /* Save the current board configuration to the internal memory, + * this values are read at boot time or if loadBootMemory() is called. + * @return true/false on success/failure + */ + virtual bool saveBootMemory()=0; +}; + +#endif // YARP_DEV_ICONTROLDEBUG_H diff --git a/src/libYARP_dev/include/yarp/dev/IControlLimits.h b/src/libYARP_dev/include/yarp/dev/IControlLimits.h new file mode 100644 index 0000000000..697073ab80 --- /dev/null +++ b/src/libYARP_dev/include/yarp/dev/IControlLimits.h @@ -0,0 +1,132 @@ +/* + * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2010 RobotCub Consortium + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#ifndef YARP_DEV_ICONTROLLIMITS_H +#define YARP_DEV_ICONTROLLIMITS_H + +#include + +/*! \file IControlLimits.h define control board standard interfaces*/ + +namespace yarp +{ + namespace dev + { + class IControlLimitsRaw; + class IControlLimits; + } +} + +/** + * @ingroup dev_iface_motor + * + * Interface for control devices, commands to get/set position and veloity limits. + */ +class YARP_dev_API yarp::dev::IControlLimits +{ +public: + /** + * Destructor. + */ + virtual ~IControlLimits() {} + + /** + * Set the software limits for a particular axis, the behavior of the + * control card when these limits are exceeded, depends on the implementation. + * @param axis joint number (why am I telling you this) + * @param min the value of the lower limit + * @param max the value of the upper limit + * @return true or false on success or failure + */ + virtual bool setLimits(int axis, double min, double max)=0; + + /** + * Get the software limits for a particular axis. + * @param axis joint number + * @param pointer to store the value of the lower limit + * @param pointer to store the value of the upper limit + * @return true if everything goes fine, false otherwise. + */ + virtual bool getLimits(int axis, double *min, double *max)=0; + + /** + * Set the software speed limits for a particular axis, the behavior of the + * control card when these limits are exceeded, depends on the implementation. + * @param axis joint number + * @param min the value of the lower limit + * @param max the value of the upper limit + * @return true or false on success or failure + */ + virtual bool setVelLimits(int axis, double min, double max)=0; + + /** + * Get the software speed limits for a particular axis. + * @param axis joint number + * @param min pointer to store the value of the lower limit + * @param max pointer to store the value of the upper limit + * @return true if everything goes fine, false otherwise. + */ + virtual bool getVelLimits(int axis, double *min, double *max)=0; +}; + +/** + * Interface for control devices. Limits commands. + */ +class YARP_dev_API yarp::dev::IControlLimitsRaw +{ +public: + /** + * Destructor. + */ + virtual ~IControlLimitsRaw() {} + + /** + * Set the software limits for a particular axis, the behavior of the + * control card when these limits are exceeded, depends on the implementation. + * @param axis joint number (why am I telling you this) + * @param min the value of the lower limit + * @param max the value of the upper limit + * @return true or false on success or failure + */ + virtual bool setLimitsRaw(int axis, double min, double max)=0; + + /** + * Get the software limits for a particular axis. + * @param axis joint number + * @param pointer to store the value of the lower limit + * @param pointer to store the value of the upper limit + * @return true if everything goes fine, false otherwise. + */ + virtual bool getLimitsRaw(int axis, double *min, double *max)=0; + + /** + * Set the software speed limits for a particular axis, the behavior of the + * control card when these limits are exceeded, depends on the implementation. + * @param axis joint number + * @param min the value of the lower limit + * @param max the value of the upper limit + * @return true or false on success or failure + */ + virtual bool setVelLimitsRaw(int axis, double min, double max)=0; + + /** + * Get the software speed limits for a particular axis. + * @param axis joint number + * @param min pointer to store the value of the lower limit + * @param max pointer to store the value of the upper limit + * @return true if everything goes fine, false otherwise. + */ + virtual bool getVelLimitsRaw(int axis, double *min, double *max)=0; +}; + +// interface IControlLimits sets/gets +constexpr yarp::conf::vocab32_t VOCAB_LIMITS = yarp::os::createVocab('l','l','i','m'); +constexpr yarp::conf::vocab32_t VOCAB_VEL_LIMITS = yarp::os::createVocab('v','l','i','m'); + +#endif // YARP_DEV_ICONTROLLIMITS_H diff --git a/src/libYARP_dev/include/yarp/dev/IControlMode.h b/src/libYARP_dev/include/yarp/dev/IControlMode.h index 628d21a28f..4bcd7e8000 100644 --- a/src/libYARP_dev/include/yarp/dev/IControlMode.h +++ b/src/libYARP_dev/include/yarp/dev/IControlMode.h @@ -115,40 +115,39 @@ class yarp::dev::IControlModeRaw // new style VOCABS // Interface -#define VOCAB_ICONTROLMODE VOCAB4('i','c','m','d') - +constexpr yarp::conf::vocab32_t VOCAB_ICONTROLMODE = yarp::os::createVocab('i','c','m','d'); // Methods -#define VOCAB_CM_CONTROL_MODE VOCAB4('c','m','o','d') -#define VOCAB_CM_CONTROL_MODE_GROUP VOCAB4('c','m','o','g') -#define VOCAB_CM_CONTROL_MODES VOCAB4('c','m','d','s') +constexpr yarp::conf::vocab32_t VOCAB_CM_CONTROL_MODE = yarp::os::createVocab('c','m','o','d'); +constexpr yarp::conf::vocab32_t VOCAB_CM_CONTROL_MODE_GROUP = yarp::os::createVocab('c','m','o','g'); +constexpr yarp::conf::vocab32_t VOCAB_CM_CONTROL_MODES = yarp::os::createVocab('c','m','d','s'); // Values // Read / Write -#define VOCAB_CM_IDLE VOCAB3('i','d','l') -#define VOCAB_CM_TORQUE VOCAB4('t','o','r','q') -#define VOCAB_CM_POSITION VOCAB3('p','o','s') -#define VOCAB_CM_POSITION_DIRECT VOCAB4('p','o','s','d') -#define VOCAB_CM_VELOCITY VOCAB3('v','e','l') -#define VOCAB_CM_CURRENT VOCAB4('i','c','u','r') -#define VOCAB_CM_PWM VOCAB4('i','p','w','m') -#define VOCAB_CM_IMPEDANCE_POS VOCAB4('i','m','p','o') // deprecated -#define VOCAB_CM_IMPEDANCE_VEL VOCAB4('i','m','v','e') // deprecated +constexpr yarp::conf::vocab32_t VOCAB_CM_IDLE = yarp::os::createVocab('i','d','l'); +constexpr yarp::conf::vocab32_t VOCAB_CM_TORQUE = yarp::os::createVocab('t','o','r','q'); +constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION = yarp::os::createVocab('p','o','s'); +constexpr yarp::conf::vocab32_t VOCAB_CM_POSITION_DIRECT = yarp::os::createVocab('p','o','s','d'); +constexpr yarp::conf::vocab32_t VOCAB_CM_VELOCITY = yarp::os::createVocab('v','e','l'); +constexpr yarp::conf::vocab32_t VOCAB_CM_CURRENT = yarp::os::createVocab('i','c','u','r'); +constexpr yarp::conf::vocab32_t VOCAB_CM_PWM = yarp::os::createVocab('i','p','w','m'); +constexpr yarp::conf::vocab32_t VOCAB_CM_IMPEDANCE_POS = yarp::os::createVocab('i','m','p','o'); // deprecated +constexpr yarp::conf::vocab32_t VOCAB_CM_IMPEDANCE_VEL = yarp::os::createVocab('i','m','v','e'); // deprecated // Values // Read / Write -#define VOCAB_CM_MIXED VOCAB3('m','i','x') +constexpr yarp::conf::vocab32_t VOCAB_CM_MIXED = yarp::os::createVocab('m','i','x'); // Write only (only from high level toward the joint) -#define VOCAB_CM_FORCE_IDLE VOCAB4('f','i','d','l') +constexpr yarp::conf::vocab32_t VOCAB_CM_FORCE_IDLE = yarp::os::createVocab('f','i','d','l'); // Read only (imposed by the board on special events) -#define VOCAB_CM_HW_FAULT VOCAB4('h','w','f','a') -#define VOCAB_CM_CALIBRATING VOCAB3('c','a','l') // the joint is calibrating -#define VOCAB_CM_CALIB_DONE VOCAB4('c','a','l','d') // calibration successfully completed -#define VOCAB_CM_NOT_CONFIGURED VOCAB4('c','f','g','n') // missing initial configuration (default value at start-up) -#define VOCAB_CM_CONFIGURED VOCAB4('c','f','g','y') // initial configuration completed, if any +constexpr yarp::conf::vocab32_t VOCAB_CM_HW_FAULT = yarp::os::createVocab('h','w','f','a'); +constexpr yarp::conf::vocab32_t VOCAB_CM_CALIBRATING = yarp::os::createVocab('c','a','l'); // the joint is calibrating +constexpr yarp::conf::vocab32_t VOCAB_CM_CALIB_DONE = yarp::os::createVocab('c','a','l','d'); // calibration successfully completed +constexpr yarp::conf::vocab32_t VOCAB_CM_NOT_CONFIGURED = yarp::os::createVocab('c','f','g','n'); // missing initial configuration (default value at start-up) +constexpr yarp::conf::vocab32_t VOCAB_CM_CONFIGURED = yarp::os::createVocab('c','f','g','y'); // initial configuration completed, if any // Read only (cannot be set from user) -#define VOCAB_CM_UNKNOWN VOCAB4('u','n','k','w') +constexpr yarp::conf::vocab32_t VOCAB_CM_UNKNOWN = yarp::os::createVocab('u','n','k','w'); #endif // YARP_DEV_ICONTROLMODE_H diff --git a/src/libYARP_dev/include/yarp/dev/ICurrentControl.h b/src/libYARP_dev/include/yarp/dev/ICurrentControl.h index 147e3afc1e..4f1568e359 100644 --- a/src/libYARP_dev/include/yarp/dev/ICurrentControl.h +++ b/src/libYARP_dev/include/yarp/dev/ICurrentControl.h @@ -193,13 +193,13 @@ class yarp::dev::ICurrentControlRaw }; // Interface name -#define VOCAB_CURRENTCONTROL_INTERFACE VOCAB4('i','c','u','r') +constexpr yarp::conf::vocab32_t VOCAB_CURRENTCONTROL_INTERFACE = yarp::os::createVocab('i','c','u','r'); // methods names -#define VOCAB_CURRENT_REF VOCAB3('r','e','f') -#define VOCAB_CURRENT_REFS VOCAB4('r','e','f','s') -#define VOCAB_CURRENT_REF_GROUP VOCAB4('r','e','f','g') -#define VOCAB_CURRENT_RANGE VOCAB3('r','n','g') -#define VOCAB_CURRENT_RANGES VOCAB4('r','n','g','s') +constexpr yarp::conf::vocab32_t VOCAB_CURRENT_REF = yarp::os::createVocab('r','e','f'); +constexpr yarp::conf::vocab32_t VOCAB_CURRENT_REFS = yarp::os::createVocab('r','e','f','s'); +constexpr yarp::conf::vocab32_t VOCAB_CURRENT_REF_GROUP = yarp::os::createVocab('r','e','f','g'); +constexpr yarp::conf::vocab32_t VOCAB_CURRENT_RANGE = yarp::os::createVocab('r','n','g'); +constexpr yarp::conf::vocab32_t VOCAB_CURRENT_RANGES = yarp::os::createVocab('r','n','g','s'); #endif // YARP_DEV_ICURRENTCONTROL_H diff --git a/src/libYARP_dev/include/yarp/dev/IEncoders.h b/src/libYARP_dev/include/yarp/dev/IEncoders.h index e303773c43..c100193dfe 100644 --- a/src/libYARP_dev/include/yarp/dev/IEncoders.h +++ b/src/libYARP_dev/include/yarp/dev/IEncoders.h @@ -206,15 +206,15 @@ class YARP_dev_API yarp::dev::IEncoders }; // interface IEncoders sets -#define VOCAB_E_RESET VOCAB3('e','r','e') -#define VOCAB_E_RESETS VOCAB4('e','r','e','s') -#define VOCAB_ENCODER VOCAB3('e','n','c') -#define VOCAB_ENCODERS VOCAB4('e','n','c','s') +constexpr yarp::conf::vocab32_t VOCAB_E_RESET = yarp::os::createVocab('e','r','e'); +constexpr yarp::conf::vocab32_t VOCAB_E_RESETS = yarp::os::createVocab('e','r','e','s'); +constexpr yarp::conf::vocab32_t VOCAB_ENCODER = yarp::os::createVocab('e','n','c'); +constexpr yarp::conf::vocab32_t VOCAB_ENCODERS = yarp::os::createVocab('e','n','c','s'); // interface IEncoders gets -#define VOCAB_ENCODER_SPEED VOCAB3('e','s','p') -#define VOCAB_ENCODER_SPEEDS VOCAB4('e','s','p','s') -#define VOCAB_ENCODER_ACCELERATION VOCAB3('e','a','c') -#define VOCAB_ENCODER_ACCELERATIONS VOCAB4('e','a','c','s') +constexpr yarp::conf::vocab32_t VOCAB_ENCODER_SPEED = yarp::os::createVocab('e','s','p'); +constexpr yarp::conf::vocab32_t VOCAB_ENCODER_SPEEDS = yarp::os::createVocab('e','s','p','s'); +constexpr yarp::conf::vocab32_t VOCAB_ENCODER_ACCELERATION = yarp::os::createVocab('e','a','c'); +constexpr yarp::conf::vocab32_t VOCAB_ENCODER_ACCELERATIONS = yarp::os::createVocab('e','a','c','s'); #endif // YARP_DEV_IENCODERS_H diff --git a/src/libYARP_dev/include/yarp/dev/IFrameTransform.h b/src/libYARP_dev/include/yarp/dev/IFrameTransform.h index 249c717fac..0da69bf04b 100644 --- a/src/libYARP_dev/include/yarp/dev/IFrameTransform.h +++ b/src/libYARP_dev/include/yarp/dev/IFrameTransform.h @@ -161,9 +161,9 @@ class YARP_dev_API yarp::dev::IFrameTransform virtual bool waitForTransform(const std::string &target_frame_id, const std::string &source_frame_id, const double &timeout) = 0; }; -#define VOCAB_ITRANSFORM VOCAB4('i','t','r','f') -#define VOCAB_TRANSFORM_SET VOCAB4('t','f','s','t') -#define VOCAB_TRANSFORM_DELETE VOCAB4('t','f','d','l') -#define VOCAB_TRANSFORM_DELETE_ALL VOCAB4('t','f','d','a') +constexpr yarp::conf::vocab32_t VOCAB_ITRANSFORM = yarp::os::createVocab('i','t','r','f'); +constexpr yarp::conf::vocab32_t VOCAB_TRANSFORM_SET = yarp::os::createVocab('t','f','s','t'); +constexpr yarp::conf::vocab32_t VOCAB_TRANSFORM_DELETE = yarp::os::createVocab('t','f','d','l'); +constexpr yarp::conf::vocab32_t VOCAB_TRANSFORM_DELETE_ALL = yarp::os::createVocab('t','f','d','a'); #endif // YARP_DEV_IFRAMETRANSFORM_H diff --git a/src/libYARP_dev/include/yarp/dev/IImpedanceControl.h b/src/libYARP_dev/include/yarp/dev/IImpedanceControl.h index 32d720b268..921cc38663 100644 --- a/src/libYARP_dev/include/yarp/dev/IImpedanceControl.h +++ b/src/libYARP_dev/include/yarp/dev/IImpedanceControl.h @@ -10,6 +10,7 @@ #define YARP_DEV_IIMPEDANCECONTROL_H #include +#include namespace yarp{ namespace dev { @@ -113,9 +114,9 @@ class YARP_dev_API yarp::dev::IImpedanceControl }; //interface -#define VOCAB_IMPEDANCE VOCAB4('i','i','m','p') -#define VOCAB_ICONTROLMODE VOCAB4('i','c','m','d') -#define VOCAB_POSITION VOCAB3('p','o','s') -#define VOCAB_VELOCITY VOCAB3('v','e','l') +constexpr yarp::conf::vocab32_t VOCAB_IMPEDANCE = yarp::os::createVocab('i','i','m','p'); + +constexpr yarp::conf::vocab32_t VOCAB_POSITION = yarp::os::createVocab('p','o','s'); +constexpr yarp::conf::vocab32_t VOCAB_VELOCITY = yarp::os::createVocab('v','e','l'); #endif // YARP_DEV_IIMPEDANCECONTROL_H diff --git a/src/libYARP_dev/include/yarp/dev/IInteractionMode.h b/src/libYARP_dev/include/yarp/dev/IInteractionMode.h index c3b6233f70..014696f44a 100644 --- a/src/libYARP_dev/include/yarp/dev/IInteractionMode.h +++ b/src/libYARP_dev/include/yarp/dev/IInteractionMode.h @@ -19,17 +19,17 @@ namespace yarp { enum InteractionModeEnum { - VOCAB_IM_STIFF = VOCAB4('s','t','i','f'), - VOCAB_IM_COMPLIANT = VOCAB4('c','o','m','p'), - VOCAB_IM_UNKNOWN = VOCAB4('u','n','k','n') + VOCAB_IM_STIFF = yarp::os::createVocab('s','t','i','f'), + VOCAB_IM_COMPLIANT = yarp::os::createVocab('c','o','m','p'), + VOCAB_IM_UNKNOWN = yarp::os::createVocab('u','n','k','n') }; } } -#define VOCAB_INTERFACE_INTERACTION_MODE VOCAB4('i','n','t','m') -#define VOCAB_INTERACTION_MODE VOCAB4('m','o','d','e') -#define VOCAB_INTERACTION_MODE_GROUP VOCAB4('m','o','d','g') -#define VOCAB_INTERACTION_MODES VOCAB4('m','o','d','s') +constexpr yarp::conf::vocab32_t VOCAB_INTERFACE_INTERACTION_MODE = yarp::os::createVocab('i','n','t','m'); +constexpr yarp::conf::vocab32_t VOCAB_INTERACTION_MODE = yarp::os::createVocab('m','o','d','e'); +constexpr yarp::conf::vocab32_t VOCAB_INTERACTION_MODE_GROUP = yarp::os::createVocab('m','o','d','g'); +constexpr yarp::conf::vocab32_t VOCAB_INTERACTION_MODES = yarp::os::createVocab('m','o','d','s'); diff --git a/src/libYARP_dev/include/yarp/dev/IJoypadController.h b/src/libYARP_dev/include/yarp/dev/IJoypadController.h index 71d7fba959..28f2c714a2 100644 --- a/src/libYARP_dev/include/yarp/dev/IJoypadController.h +++ b/src/libYARP_dev/include/yarp/dev/IJoypadController.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -281,23 +282,17 @@ class YARP_dev_API yarp::dev::IJoypadEventDriven : yarp::os::PeriodicThread, #define YRPJOY_HAT_LEFTUP (YRPJOY_HAT_LEFT |YRPJOY_HAT_UP) #define YRPJOY_HAT_LEFTDOWN (YRPJOY_HAT_LEFT |YRPJOY_HAT_DOWN) -#define VOCAB_OK VOCAB2('o','k') -#define VOCAB_FAILED VOCAB4('f','a','i','l') -#define VOCAB_IJOYPADCTRL VOCAB4('i','j','p','c') -#ifndef VOCAB_GET -#define VOCAB_GET VOCAB3('g','e','t') -#endif -#define VOCAB_BUTTON VOCAB4('b','u','t','n') -#define VOCAB_TRACKBALL VOCAB4('t','r','b','l') -#define VOCAB_HAT VOCAB3('h','a','t') -#define VOCAB_AXIS VOCAB4('a','x','i','s') -#define VOCAB_STICK VOCAB4('s','t','c','k') -#define VOCAB_STICKDOF VOCAB4('s','d','o','f') -#define VOCAB_TOUCH VOCAB4('t','u','c','h') -#define VOCAB_COUNT VOCAB3('c','n','t') -#define VOCAB_VALUE VOCAB3('v','a','l') -#define VOCAB_POLAR VOCAB4('p','o','l','r') -#define VOCAB_CARTESIAN VOCAB4('c','a','r','t') +constexpr yarp::conf::vocab32_t VOCAB_IJOYPADCTRL = yarp::os::createVocab('i','j','p','c'); + +constexpr yarp::conf::vocab32_t VOCAB_BUTTON = yarp::os::createVocab('b','u','t','n'); +constexpr yarp::conf::vocab32_t VOCAB_TRACKBALL = yarp::os::createVocab('t','r','b','l'); +constexpr yarp::conf::vocab32_t VOCAB_HAT = yarp::os::createVocab('h','a','t'); +constexpr yarp::conf::vocab32_t VOCAB_AXIS = yarp::os::createVocab('a','x','i','s'); +constexpr yarp::conf::vocab32_t VOCAB_STICK = yarp::os::createVocab('s','t','c','k'); +constexpr yarp::conf::vocab32_t VOCAB_STICKDOF = yarp::os::createVocab('s','d','o','f'); +constexpr yarp::conf::vocab32_t VOCAB_TOUCH = yarp::os::createVocab('t','u','c','h'); +constexpr yarp::conf::vocab32_t VOCAB_POLAR = yarp::os::createVocab('p','o','l','r'); +constexpr yarp::conf::vocab32_t VOCAB_CARTESIAN = yarp::os::createVocab('c','a','r','t'); #endif //#define YARP_DEV_IJOYPADCONTROLLER_H diff --git a/src/libYARP_dev/include/yarp/dev/ILocalization2D.h b/src/libYARP_dev/include/yarp/dev/ILocalization2D.h index c6d33e879c..d31712d501 100644 --- a/src/libYARP_dev/include/yarp/dev/ILocalization2D.h +++ b/src/libYARP_dev/include/yarp/dev/ILocalization2D.h @@ -47,25 +47,25 @@ class yarp::dev::ILocalization2D virtual bool setInitialPose(yarp::dev::Map2DLocation& loc) = 0; }; -#define VOCAB_INAVIGATION VOCAB4('i','n','a','v') +constexpr yarp::conf::vocab32_t VOCAB_INAVIGATION = yarp::os::createVocab('i','n','a','v'); -#define VOCAB_NAV_GOTOABS VOCAB4('s','a','b','s') -#define VOCAB_NAV_GOTOREL VOCAB4('s','r','e','l') +constexpr yarp::conf::vocab32_t VOCAB_NAV_GOTOABS = yarp::os::createVocab('s','a','b','s'); +constexpr yarp::conf::vocab32_t VOCAB_NAV_GOTOREL = yarp::os::createVocab('s','r','e','l'); -#define VOCAB_NAV_GET_LOCATION VOCAB4('g','l','o','c') -#define VOCAB_NAV_GET_LOCATION_LIST VOCAB4('l','i','s','t') -#define VOCAB_NAV_GET_ABS_TARGET VOCAB4('g','a','b','s') -#define VOCAB_NAV_GET_REL_TARGET VOCAB4('g','r','e','l') -#define VOCAB_NAV_GET_NAME_TARGET VOCAB4('g','n','a','m') -#define VOCAB_NAV_GET_CURRENT_POS VOCAB4('g','p','o','s') -#define VOCAB_NAV_SET_INITIAL_POS VOCAB4('i','p','o','s') -#define VOCAB_NAV_GET_STATUS VOCAB4('g','s','t','s') -#define VOCAB_NAV_CLEAR VOCAB4('c','l','r','l') -#define VOCAB_NAV_DELETE VOCAB4('d','e','l','l') -#define VOCAB_NAV_STORE_ABS VOCAB4('s','t','o','a') +constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_LOCATION = yarp::os::createVocab('g','l','o','c'); +constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_LOCATION_LIST = yarp::os::createVocab('l','i','s','t'); +constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_ABS_TARGET = yarp::os::createVocab('g','a','b','s'); +constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_REL_TARGET = yarp::os::createVocab('g','r','e','l'); +constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_NAME_TARGET = yarp::os::createVocab('g','n','a','m'); +constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_CURRENT_POS = yarp::os::createVocab('g','p','o','s'); +constexpr yarp::conf::vocab32_t VOCAB_NAV_SET_INITIAL_POS = yarp::os::createVocab('i','p','o','s'); +constexpr yarp::conf::vocab32_t VOCAB_NAV_GET_STATUS = yarp::os::createVocab('g','s','t','s'); +constexpr yarp::conf::vocab32_t VOCAB_NAV_CLEAR = yarp::os::createVocab('c','l','r','l'); +constexpr yarp::conf::vocab32_t VOCAB_NAV_DELETE = yarp::os::createVocab('d','e','l','l'); +constexpr yarp::conf::vocab32_t VOCAB_NAV_STORE_ABS = yarp::os::createVocab('s','t','o','a'); -#define VOCAB_NAV_STOP VOCAB4('s','t','o','p') -#define VOCAB_NAV_SUSPEND VOCAB4('s','u','s','p') -#define VOCAB_NAV_RESUME VOCAB4('r','e','s','m') +constexpr yarp::conf::vocab32_t VOCAB_NAV_STOP = yarp::os::createVocab('s','t','o','p'); +constexpr yarp::conf::vocab32_t VOCAB_NAV_SUSPEND = yarp::os::createVocab('s','u','s','p'); +constexpr yarp::conf::vocab32_t VOCAB_NAV_RESUME = yarp::os::createVocab('r','e','s','m'); #endif // YARP_DEV_ILOCALIZATION2D_H diff --git a/src/libYARP_dev/include/yarp/dev/IMap2D.h b/src/libYARP_dev/include/yarp/dev/IMap2D.h index 323b260880..840e06e343 100644 --- a/src/libYARP_dev/include/yarp/dev/IMap2D.h +++ b/src/libYARP_dev/include/yarp/dev/IMap2D.h @@ -104,15 +104,15 @@ class YARP_dev_API yarp::dev::IMap2D virtual bool clearAllLocations() = 0; }; -#define VOCAB_IMAP VOCAB4('i','m','a','p') -#define VOCAB_IMAP_SET_MAP VOCAB3('s','e','t') -#define VOCAB_IMAP_GET_MAP VOCAB3('g','e','t') -#define VOCAB_IMAP_GET_NAMES VOCAB4('n','a','m','s') -#define VOCAB_IMAP_CLEAR VOCAB3('c','l','r') -#define VOCAB_IMAP_REMOVE VOCAB4('r','e','m','v') -#define VOCAB_IMAP_LOAD_COLLECTION VOCAB4('l','d','c','l') -#define VOCAB_IMAP_SAVE_COLLECTION VOCAB4('s','v','c','l') -#define VOCAB_IMAP_OK VOCAB3('o','k','k') -#define VOCAB_IMAP_ERROR VOCAB3('e','r','r') +constexpr yarp::conf::vocab32_t VOCAB_IMAP = yarp::os::createVocab('i','m','a','p'); +constexpr yarp::conf::vocab32_t VOCAB_IMAP_SET_MAP = yarp::os::createVocab('s','e','t'); +constexpr yarp::conf::vocab32_t VOCAB_IMAP_GET_MAP = yarp::os::createVocab('g','e','t'); +constexpr yarp::conf::vocab32_t VOCAB_IMAP_GET_NAMES = yarp::os::createVocab('n','a','m','s'); +constexpr yarp::conf::vocab32_t VOCAB_IMAP_CLEAR = yarp::os::createVocab('c','l','r'); +constexpr yarp::conf::vocab32_t VOCAB_IMAP_REMOVE = yarp::os::createVocab('r','e','m','v'); +constexpr yarp::conf::vocab32_t VOCAB_IMAP_LOAD_COLLECTION = yarp::os::createVocab('l','d','c','l'); +constexpr yarp::conf::vocab32_t VOCAB_IMAP_SAVE_COLLECTION = yarp::os::createVocab('s','v','c','l'); +constexpr yarp::conf::vocab32_t VOCAB_IMAP_OK = yarp::os::createVocab('o','k','k'); +constexpr yarp::conf::vocab32_t VOCAB_IMAP_ERROR = yarp::os::createVocab('e','r','r'); #endif // YARP_DEV_IMAP2D_H diff --git a/src/libYARP_dev/include/yarp/dev/IMotor.h b/src/libYARP_dev/include/yarp/dev/IMotor.h index b0842258a9..d8568a359b 100644 --- a/src/libYARP_dev/include/yarp/dev/IMotor.h +++ b/src/libYARP_dev/include/yarp/dev/IMotor.h @@ -160,11 +160,11 @@ class YARP_dev_API yarp::dev::IMotor }; // interface IMotorEncoders gets -#define VOCAB_MOTORS_NUMBER VOCAB4('m','t','n','m') -#define VOCAB_TEMPERATURE VOCAB3('t','m','p') -#define VOCAB_GEARBOX_RATIO VOCAB4('g','b','x','r') -#define VOCAB_TEMPERATURES VOCAB4('t','m','p','s') -#define VOCAB_TEMPERATURE_LIMIT VOCAB4('t','m','p','l') +constexpr yarp::conf::vocab32_t VOCAB_MOTORS_NUMBER = yarp::os::createVocab('m','t','n','m'); +constexpr yarp::conf::vocab32_t VOCAB_TEMPERATURE = yarp::os::createVocab('t','m','p'); +constexpr yarp::conf::vocab32_t VOCAB_GEARBOX_RATIO = yarp::os::createVocab('g','b','x','r'); +constexpr yarp::conf::vocab32_t VOCAB_TEMPERATURES = yarp::os::createVocab('t','m','p','s'); +constexpr yarp::conf::vocab32_t VOCAB_TEMPERATURE_LIMIT = yarp::os::createVocab('t','m','p','l'); #endif // YARP_DEV_IMOTOR_H diff --git a/src/libYARP_dev/include/yarp/dev/IMotorEncoders.h b/src/libYARP_dev/include/yarp/dev/IMotorEncoders.h index a79b56904b..59ff0cf0a5 100644 --- a/src/libYARP_dev/include/yarp/dev/IMotorEncoders.h +++ b/src/libYARP_dev/include/yarp/dev/IMotorEncoders.h @@ -273,17 +273,17 @@ class YARP_dev_API yarp::dev::IMotorEncoders }; // interface IMotorEncoders sets -#define VOCAB_MOTOR_E_RESET VOCAB3('m','r','e') -#define VOCAB_MOTOR_E_RESETS VOCAB4('m','r','e','s') -#define VOCAB_MOTOR_ENCODER VOCAB3('m','n','c') -#define VOCAB_MOTOR_ENCODERS VOCAB4('m','n','c','s') -#define VOCAB_MOTOR_CPR VOCAB4('m','c','p','r') +constexpr yarp::conf::vocab32_t VOCAB_MOTOR_E_RESET = yarp::os::createVocab('m','r','e'); +constexpr yarp::conf::vocab32_t VOCAB_MOTOR_E_RESETS = yarp::os::createVocab('m','r','e','s'); +constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER = yarp::os::createVocab('m','n','c'); +constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODERS = yarp::os::createVocab('m','n','c','s'); +constexpr yarp::conf::vocab32_t VOCAB_MOTOR_CPR = yarp::os::createVocab('m','c','p','r'); // interface IMotorEncoders gets -#define VOCAB_MOTOR_ENCODER_NUMBER VOCAB4('m','n','u','m') -#define VOCAB_MOTOR_ENCODER_SPEED VOCAB3('m','s','p') -#define VOCAB_MOTOR_ENCODER_SPEEDS VOCAB4('m','s','p','s') -#define VOCAB_MOTOR_ENCODER_ACCELERATION VOCAB3('m','a','c') -#define VOCAB_MOTOR_ENCODER_ACCELERATIONS VOCAB4('m','a','c','s') +constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_NUMBER = yarp::os::createVocab('m','n','u','m'); +constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_SPEED = yarp::os::createVocab('m','s','p'); +constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_SPEEDS = yarp::os::createVocab('m','s','p','s'); +constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_ACCELERATION = yarp::os::createVocab('m','a','c'); +constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_ACCELERATIONS = yarp::os::createVocab('m','a','c','s'); #endif // YARP_DEV_IMOTORENCODERS_H diff --git a/src/libYARP_dev/include/yarp/dev/INavigation2D.h b/src/libYARP_dev/include/yarp/dev/INavigation2D.h index f78d5811de..0619ec3a2d 100644 --- a/src/libYARP_dev/include/yarp/dev/INavigation2D.h +++ b/src/libYARP_dev/include/yarp/dev/INavigation2D.h @@ -11,6 +11,7 @@ #include #include +#include #include #include @@ -20,16 +21,16 @@ namespace yarp { enum NavigationStatusEnum { - navigation_status_idle = VOCAB4('i', 'd', 'l', 'e'), - navigation_status_preparing_before_move = VOCAB4('p', 'r', 'e', 'p'), - navigation_status_moving = VOCAB4('m', 'o', 'v', 'g'), - navigation_status_waiting_obstacle = VOCAB4('w', 'a', 'i', 't'), - navigation_status_goal_reached = VOCAB4('r', 'e', 'c', 'h'), - navigation_status_aborted = VOCAB4('a', 'b', 'r', 't'), - navigation_status_failing = VOCAB4('f', 'a', 'i', 'l'), - navigation_status_paused = VOCAB4('p', 'a', 'u', 's'), - navigation_status_thinking = VOCAB4('t', 'h', 'n', 'k'), - navigation_status_error = VOCAB3('e', 'r', 'r'), + navigation_status_idle = yarp::os::createVocab('i', 'd', 'l', 'e'), + navigation_status_preparing_before_move = yarp::os::createVocab('p', 'r', 'e', 'p'), + navigation_status_moving = yarp::os::createVocab('m', 'o', 'v', 'g'), + navigation_status_waiting_obstacle = yarp::os::createVocab('w', 'a', 'i', 't'), + navigation_status_goal_reached = yarp::os::createVocab('r', 'e', 'c', 'h'), + navigation_status_aborted = yarp::os::createVocab('a', 'b', 'r', 't'), + navigation_status_failing = yarp::os::createVocab('f', 'a', 'i', 'l'), + navigation_status_paused = yarp::os::createVocab('p', 'a', 'u', 's'), + navigation_status_thinking = yarp::os::createVocab('t', 'h', 'n', 'k'), + navigation_status_error = yarp::os::createVocab('e', 'r', 'r'), }; } } @@ -39,7 +40,7 @@ namespace yarp { * * An interface to control the navigation of a mobile robot in a 2D environment. */ -class yarp::dev::INavigation2D +class yarp::dev::INavigation2D : public yarp::dev::ILocalization2D { public: /** @@ -70,20 +71,6 @@ class yarp::dev::INavigation2D */ virtual bool gotoTargetByRelativeLocation(double x, double y, double theta) = 0; - /** - * Gets the current position of the robot w.r.t world reference frame - * @param loc the location of the robot - * @return true/false - */ - virtual bool getCurrentPosition(yarp::dev::Map2DLocation& loc) = 0; - - /** - * Sets the initial pose for the localization algorithm which estimates the current position of the robot w.r.t world reference frame. - * @param loc the location of the robot - * @return true/false - */ - virtual bool setInitialPose(yarp::dev::Map2DLocation& loc) = 0; - /** * Gets the last navigation target in the world reference frame * @param loc the location of the robot @@ -168,29 +155,7 @@ class yarp::dev::INavigation2D virtual bool resumeNavigation() = 0; }; -#define VOCAB_INAVIGATION VOCAB4('i','n','a','v') - -#define VOCAB_NAV_GOTOABS VOCAB4('s','a','b','s') -#define VOCAB_NAV_GOTOREL VOCAB4('s','r','e','l') - -#define VOCAB_NAV_GET_LOCATION VOCAB4('g','l','o','c') -#define VOCAB_NAV_GET_LOCATION_LIST VOCAB4('l','i','s','t') -#define VOCAB_NAV_GET_ABS_TARGET VOCAB4('g','a','b','s') -#define VOCAB_NAV_GET_REL_TARGET VOCAB4('g','r','e','l') -#define VOCAB_NAV_GET_NAME_TARGET VOCAB4('g','n','a','m') -#define VOCAB_NAV_GET_CURRENT_POS VOCAB4('g','p','o','s') -#define VOCAB_NAV_SET_INITIAL_POS VOCAB4('i','p','o','s') -#define VOCAB_NAV_GET_STATUS VOCAB4('g','s','t','s') -#define VOCAB_NAV_CLEAR VOCAB4('c','l','r','l') -#define VOCAB_NAV_DELETE VOCAB4('d','e','l','l') -#define VOCAB_NAV_STORE_ABS VOCAB4('s','t','o','a') - -#define VOCAB_NAV_STOP VOCAB4('s','t','o','p') -#define VOCAB_NAV_SUSPEND VOCAB4('s','u','s','p') -#define VOCAB_NAV_RESUME VOCAB4('r','e','s','m') - -#define VOCAB_OK VOCAB2('o','k') -#define VOCAB_ERR VOCAB3('e','r','r') + #endif // YARP_DEV_INAVIGATION2D_H diff --git a/src/libYARP_dev/include/yarp/dev/IPWMControl.h b/src/libYARP_dev/include/yarp/dev/IPWMControl.h index b6371c12b1..9abbd2d4f7 100644 --- a/src/libYARP_dev/include/yarp/dev/IPWMControl.h +++ b/src/libYARP_dev/include/yarp/dev/IPWMControl.h @@ -10,6 +10,7 @@ #define YARP_DEV_IPWMCONTROL_H #include +#include namespace yarp { namespace dev { @@ -140,11 +141,11 @@ class yarp::dev::IPWMControlRaw }; // Interface name -#define VOCAB_PWMCONTROL_INTERFACE VOCAB4('i','p','w','m') +constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_INTERFACE = yarp::os::createVocab('i','p','w','m'); // methods names -#define VOCAB_PWMCONTROL_REF_PWM VOCAB3('r','e','f') -#define VOCAB_PWMCONTROL_REF_PWMS VOCAB4('r','e','f','s') -#define VOCAB_PWMCONTROL_PWM_OUTPUT VOCAB3('p','w','m') -#define VOCAB_PWMCONTROL_PWM_OUTPUTS VOCAB4('p','w','m','s') +constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_REF_PWM = yarp::os::createVocab('r','e','f'); +constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_REF_PWMS = yarp::os::createVocab('r','e','f','s'); +constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_PWM_OUTPUT = yarp::os::createVocab('p','w','m'); +constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_PWM_OUTPUTS = yarp::os::createVocab('p','w','m','s'); #endif // YARP_DEV_IPWMCONTROL_H diff --git a/src/libYARP_dev/include/yarp/dev/IPidControl.h b/src/libYARP_dev/include/yarp/dev/IPidControl.h index f6730e94b5..3b807e93e5 100644 --- a/src/libYARP_dev/include/yarp/dev/IPidControl.h +++ b/src/libYARP_dev/include/yarp/dev/IPidControl.h @@ -9,8 +9,8 @@ #ifndef YARP_DEV_PIDCONTROL_H #define YARP_DEV_PIDCONTROL_H -#include #include +#include #include #include @@ -379,37 +379,9 @@ class YARP_dev_API yarp::dev::IPidControl virtual bool isPidEnabled(const PidControlTypeEnum& pidtype, int j, bool* enabled) = 0; }; -/* Vocabs representing the above interfaces */ - -#define VOCAB_SET VOCAB3('s','e','t') -#define VOCAB_GET VOCAB3('g','e','t') -#define VOCAB_IS VOCAB2('i','s') -#define VOCAB_FAILED VOCAB4('f','a','i','l') -#define VOCAB_OK VOCAB2('o','k') - -#define VOCAB_OFFSET VOCAB3('o', 'f', 'f') - -// interface IPidControl sets. -#define VOCAB_PID VOCAB3('p','i','d') -#define VOCAB_PIDS VOCAB4('p','i','d','s') -#define VOCAB_REF VOCAB3('r','e','f') -#define VOCAB_REFS VOCAB4('r','e','f','s') -#define VOCAB_REFG VOCAB4('r','e','f','g') -#define VOCAB_LIM VOCAB3('l','i','m') -#define VOCAB_LIMS VOCAB4('l','i','m','s') -#define VOCAB_RESET VOCAB3('r','e','s') -#define VOCAB_DISABLE VOCAB3('d','i','s') -#define VOCAB_ENABLE VOCAB3('e','n','a') - -// interface IPidControl gets. -#define VOCAB_ERR VOCAB3('e','r','r') -#define VOCAB_ERRS VOCAB4('e','r','r','s') -#define VOCAB_OUTPUT VOCAB3('o','u','t') -#define VOCAB_OUTPUTS VOCAB4('o','u','t','s') -#define VOCAB_REFERENCE VOCAB3('r','e','f') -#define VOCAB_REFERENCES VOCAB4('r','e','f','s') - // interface IPositionControl gets -#define VOCAB_AXES VOCAB4('a','x','e','s') +constexpr yarp::conf::vocab32_t VOCAB_PID = yarp::os::createVocab('p','i','d'); +constexpr yarp::conf::vocab32_t VOCAB_PIDS = yarp::os::createVocab('p','i','d','s'); + #endif // YARP_DEV_CONTROLBOARDINTERFACES_H diff --git a/src/libYARP_dev/include/yarp/dev/IPositionControl.h b/src/libYARP_dev/include/yarp/dev/IPositionControl.h index f51a2b02de..1fd33f59db 100644 --- a/src/libYARP_dev/include/yarp/dev/IPositionControl.h +++ b/src/libYARP_dev/include/yarp/dev/IPositionControl.h @@ -11,6 +11,7 @@ #define YARP_DEV_IPOSITIONCONTROL_H #include +#include namespace yarp { namespace dev { @@ -473,11 +474,11 @@ class YARP_dev_API yarp::dev::IPositionControl virtual bool getTargetPositions(const int n_joint, const int *joints, double *refs) { return false;} }; -#define VOCAB_POSITION_MOVE_GROUP VOCAB4('p','o','s','g') -#define VOCAB_RELATIVE_MOVE_GROUP VOCAB4('r','e','l','g') -#define VOCAB_MOTION_DONE_GROUP VOCAB4('d','o','n','g') -#define VOCAB_REF_SPEED_GROUP VOCAB4('v','e','l','g') -#define VOCAB_REF_ACCELERATION_GROUP VOCAB4('a','c','c','g') -#define VOCAB_STOP_GROUP VOCAB4('s','t','o','g') +constexpr yarp::conf::vocab32_t VOCAB_POSITION_MOVE_GROUP = yarp::os::createVocab('p','o','s','g'); +constexpr yarp::conf::vocab32_t VOCAB_RELATIVE_MOVE_GROUP = yarp::os::createVocab('r','e','l','g'); +constexpr yarp::conf::vocab32_t VOCAB_MOTION_DONE_GROUP = yarp::os::createVocab('d','o','n','g'); +constexpr yarp::conf::vocab32_t VOCAB_REF_SPEED_GROUP = yarp::os::createVocab('v','e','l','g'); +constexpr yarp::conf::vocab32_t VOCAB_REF_ACCELERATION_GROUP = yarp::os::createVocab('a','c','c','g'); +constexpr yarp::conf::vocab32_t VOCAB_STOP_GROUP = yarp::os::createVocab('s','t','o','g'); #endif // YARP_DEV_IPOSITIONCONTROL_H diff --git a/src/libYARP_dev/include/yarp/dev/IPositionDirect.h b/src/libYARP_dev/include/yarp/dev/IPositionDirect.h index 83208d527e..e4325d3526 100644 --- a/src/libYARP_dev/include/yarp/dev/IPositionDirect.h +++ b/src/libYARP_dev/include/yarp/dev/IPositionDirect.h @@ -189,8 +189,8 @@ class yarp::dev::IPositionDirectRaw virtual bool getRefPositionsRaw(const int n_joint, const int *joints, double *refs) {return false;} }; -#define VOCAB_POSITION_DIRECT VOCAB3('d','p','o') -#define VOCAB_POSITION_DIRECTS VOCAB4('d','p','o','s') -#define VOCAB_POSITION_DIRECT_GROUP VOCAB4('d','p','o','g') +constexpr yarp::conf::vocab32_t VOCAB_POSITION_DIRECT = yarp::os::createVocab('d','p','o'); +constexpr yarp::conf::vocab32_t VOCAB_POSITION_DIRECTS = yarp::os::createVocab('d','p','o','s'); +constexpr yarp::conf::vocab32_t VOCAB_POSITION_DIRECT_GROUP = yarp::os::createVocab('d','p','o','g'); #endif // YARP_IPOSITIONDIRECT_H diff --git a/src/libYARP_dev/include/yarp/dev/IRGBDSensor.h b/src/libYARP_dev/include/yarp/dev/IRGBDSensor.h index cc25e6c6e7..2a0d332ac3 100644 --- a/src/libYARP_dev/include/yarp/dev/IRGBDSensor.h +++ b/src/libYARP_dev/include/yarp/dev/IRGBDSensor.h @@ -23,16 +23,16 @@ namespace yarp { // Interface name -#define VOCAB_RGBD_SENSOR VOCAB4('r','g','d','b') -#define VOCAB_RGBD_PROTOCOL_VERSION VOCAB4('p','r','o','t') +constexpr yarp::conf::vocab32_t VOCAB_RGBD_SENSOR = yarp::os::createVocab('r','g','d','b'); +constexpr yarp::conf::vocab32_t VOCAB_RGBD_PROTOCOL_VERSION = yarp::os::createVocab('p','r','o','t'); // Methods -#define VOCAB_EXTRINSIC_PARAM VOCAB3('e','x','t') -#define VOCAB_ERROR_MSG VOCAB4('m','e','s','s') -#define VOCAB_RGB_IMAGE VOCAB4('i','m','g','r') -#define VOCAB_DEPTH_IMAGE VOCAB4('i','m','g'.'d') -#define VOCAB_IMAGES VOCAB4('i','m','m','s') -#define VOCAB_STATUS VOCAB4('s','t','a','t') +constexpr yarp::conf::vocab32_t VOCAB_EXTRINSIC_PARAM = yarp::os::createVocab('e','x','t'); +constexpr yarp::conf::vocab32_t VOCAB_ERROR_MSG = yarp::os::createVocab('m','e','s','s'); +constexpr yarp::conf::vocab32_t VOCAB_RGB_IMAGE = yarp::os::createVocab('i','m','g','r'); +constexpr yarp::conf::vocab32_t VOCAB_DEPTH_IMAGE = yarp::os::createVocab('i','m','g','d'); +constexpr yarp::conf::vocab32_t VOCAB_IMAGES = yarp::os::createVocab('i','m','m','s'); +constexpr yarp::conf::vocab32_t VOCAB_STATUS = yarp::os::createVocab('s','t','a','t'); /** diff --git a/src/libYARP_dev/include/yarp/dev/IRangefinder2D.h b/src/libYARP_dev/include/yarp/dev/IRangefinder2D.h index 9643359b19..ca627b1f7e 100644 --- a/src/libYARP_dev/include/yarp/dev/IRangefinder2D.h +++ b/src/libYARP_dev/include/yarp/dev/IRangefinder2D.h @@ -9,17 +9,18 @@ #ifndef YARP_DEV_IRANGEFINDER2D_H #define YARP_DEV_IRANGEFINDER2D_H +#include #include #include #include #include -#define VOCAB_ILASER2D VOCAB4('i','l','a','s') -#define VOCAB_DEVICE_INFO VOCAB4('l','s','n','f') -#define VOCAB_LASER_DISTANCE_RANGE VOCAB4('l','s','d','r') -#define VOCAB_LASER_ANGULAR_RANGE VOCAB4('l','s','a','r') -#define VOCAB_LASER_ANGULAR_STEP VOCAB4('l','s','a','s') -#define VOCAB_LASER_SCAN_RATE VOCAB4('l','s','s','r') +constexpr yarp::conf::vocab32_t VOCAB_ILASER2D = yarp::os::createVocab('i','l','a','s'); +constexpr yarp::conf::vocab32_t VOCAB_DEVICE_INFO = yarp::os::createVocab('l','s','n','f'); +constexpr yarp::conf::vocab32_t VOCAB_LASER_DISTANCE_RANGE = yarp::os::createVocab('l','s','d','r'); +constexpr yarp::conf::vocab32_t VOCAB_LASER_ANGULAR_RANGE = yarp::os::createVocab('l','s','a','r'); +constexpr yarp::conf::vocab32_t VOCAB_LASER_ANGULAR_STEP = yarp::os::createVocab('l','s','a','s'); +constexpr yarp::conf::vocab32_t VOCAB_LASER_SCAN_RATE = yarp::os::createVocab('l','s','s','r'); namespace yarp { namespace dev { diff --git a/src/libYARP_dev/include/yarp/dev/IRemoteVariables.h b/src/libYARP_dev/include/yarp/dev/IRemoteVariables.h index ca172e2dd8..3c44f27040 100644 --- a/src/libYARP_dev/include/yarp/dev/IRemoteVariables.h +++ b/src/libYARP_dev/include/yarp/dev/IRemoteVariables.h @@ -58,9 +58,9 @@ class YARP_dev_API yarp::dev::IRemoteVariables virtual bool getRemoteVariablesList(yarp::os::Bottle* listOfKeys) = 0; }; -#define VOCAB_REMOTE_VARIABILE_INTERFACE VOCAB4('i','v','a','r') -#define VOCAB_VARIABLE VOCAB4('m','v','a','r') -#define VOCAB_LIST_VARIABLES VOCAB4('l','v','a','r') +constexpr yarp::conf::vocab32_t VOCAB_REMOTE_VARIABILE_INTERFACE = yarp::os::createVocab('i','v','a','r'); +constexpr yarp::conf::vocab32_t VOCAB_VARIABLE = yarp::os::createVocab('m','v','a','r'); +constexpr yarp::conf::vocab32_t VOCAB_LIST_VARIABLES = yarp::os::createVocab('l','v','a','r'); #endif // YARP_DEV_IREMOTEVARIABLES_H diff --git a/src/libYARP_dev/include/yarp/dev/IRobotDescription.h b/src/libYARP_dev/include/yarp/dev/IRobotDescription.h index 782445a1cd..a42a0cd70a 100644 --- a/src/libYARP_dev/include/yarp/dev/IRobotDescription.h +++ b/src/libYARP_dev/include/yarp/dev/IRobotDescription.h @@ -62,12 +62,12 @@ class yarp::dev::IRobotDescription virtual bool unregisterDevice(const std::string& device_name) = 0; }; -#define VOCAB_IROBOT_DESCRIPTION VOCAB4('i','r','o','b') -#define VOCAB_IROBOT_GET VOCAB3('g','e','t') -#define VOCAB_IROBOT_SET VOCAB3('s','e','t') -#define VOCAB_IROBOT_DELETE VOCAB3('d','e','l') -#define VOCAB_IROBOT_ALL VOCAB3('a','l','l') -#define VOCAB_IROBOT_DEVICE VOCAB3('d','e','v') -#define VOCAB_IROBOT_BY_TYPE VOCAB4('t','y','p','e') +constexpr yarp::conf::vocab32_t VOCAB_IROBOT_DESCRIPTION = yarp::os::createVocab('i','r','o','b'); +constexpr yarp::conf::vocab32_t VOCAB_IROBOT_GET = yarp::os::createVocab('g','e','t'); +constexpr yarp::conf::vocab32_t VOCAB_IROBOT_SET = yarp::os::createVocab('s','e','t'); +constexpr yarp::conf::vocab32_t VOCAB_IROBOT_DELETE = yarp::os::createVocab('d','e','l'); +constexpr yarp::conf::vocab32_t VOCAB_IROBOT_ALL = yarp::os::createVocab('a','l','l'); +constexpr yarp::conf::vocab32_t VOCAB_IROBOT_DEVICE = yarp::os::createVocab('d','e','v'); +constexpr yarp::conf::vocab32_t VOCAB_IROBOT_BY_TYPE = yarp::os::createVocab('t','y','p','e'); #endif // YARP_DEV_IROBOTDESCRIPTION_H diff --git a/src/libYARP_dev/include/yarp/dev/ITorqueControl.h b/src/libYARP_dev/include/yarp/dev/ITorqueControl.h index 2b5fb484cb..85d5071617 100644 --- a/src/libYARP_dev/include/yarp/dev/ITorqueControl.h +++ b/src/libYARP_dev/include/yarp/dev/ITorqueControl.h @@ -235,4 +235,18 @@ class yarp::dev::ITorqueControlRaw virtual bool setMotorTorqueParamsRaw(int j, const yarp::dev::MotorTorqueParameters params) {return false;} }; +constexpr yarp::conf::vocab32_t VOCAB_TORQUE = yarp::os::createVocab('t', 'o', 'r', 'q'); +constexpr yarp::conf::vocab32_t VOCAB_TORQUE_MODE = yarp::os::createVocab('t', 'r', 'q', 'd'); +constexpr yarp::conf::vocab32_t VOCAB_TRQS = yarp::os::createVocab('t', 'r', 'q', 's'); +constexpr yarp::conf::vocab32_t VOCAB_TRQ = yarp::os::createVocab('t', 'r', 'q'); +constexpr yarp::conf::vocab32_t VOCAB_BEMF = yarp::os::createVocab('b', 'm', 'f'); +constexpr yarp::conf::vocab32_t VOCAB_MOTOR_PARAMS = yarp::os::createVocab('m', 't', 'p', 's'); +constexpr yarp::conf::vocab32_t VOCAB_RANGES = yarp::os::createVocab('r', 'n', 'g', 's'); +constexpr yarp::conf::vocab32_t VOCAB_RANGE = yarp::os::createVocab('r', 'n', 'g'); +constexpr yarp::conf::vocab32_t VOCAB_IMP_PARAM = yarp::os::createVocab('i', 'p', 'r'); +constexpr yarp::conf::vocab32_t VOCAB_IMP_OFFSET = yarp::os::createVocab('i', 'o', 'f'); +constexpr yarp::conf::vocab32_t VOCAB_TORQUES_DIRECTS = yarp::os::createVocab('d', 't', 'q', 's'); //This implements the setRefTorques for the whole part +constexpr yarp::conf::vocab32_t VOCAB_TORQUES_DIRECT = yarp::os::createVocab('d', 't', 'q'); //This implements the setRefTorque for a single joint +constexpr yarp::conf::vocab32_t VOCAB_TORQUES_DIRECT_GROUP = yarp::os::createVocab('d', 't', 'q', 'g'); //This implements the setRefTorques with joint list + #endif // YARP_DEV_ITORQUECONTROL_H diff --git a/src/libYARP_dev/include/yarp/dev/IVelocityControl.h b/src/libYARP_dev/include/yarp/dev/IVelocityControl.h index 15a963a427..f3d5292d79 100644 --- a/src/libYARP_dev/include/yarp/dev/IVelocityControl.h +++ b/src/libYARP_dev/include/yarp/dev/IVelocityControl.h @@ -302,8 +302,8 @@ class YARP_dev_API yarp::dev::IVelocityControl virtual bool stop(const int n_joint, const int *joints)=0; }; -#define VOCAB_VELOCITY_MOVE_GROUP VOCAB4('v','m','o','g') -#define VOCAB_VEL_PID VOCAB3('v','p','d') -#define VOCAB_VEL_PIDS VOCAB4('v','p','d','s') +constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVE_GROUP = yarp::os::createVocab('v','m','o','g') ; +constexpr yarp::conf::vocab32_t VOCAB_VEL_PID = yarp::os::createVocab('v','p','d'); +constexpr yarp::conf::vocab32_t VOCAB_VEL_PIDS = yarp::os::createVocab('v','p','d','s'); #endif // YARP_DEV_IVELOCITYCONTROL_H diff --git a/src/libYARP_dev/include/yarp/dev/IVirtualAnalogSensor.h b/src/libYARP_dev/include/yarp/dev/IVirtualAnalogSensor.h index 9a8dc6a809..37bad1975e 100644 --- a/src/libYARP_dev/include/yarp/dev/IVirtualAnalogSensor.h +++ b/src/libYARP_dev/include/yarp/dev/IVirtualAnalogSensor.h @@ -9,11 +9,11 @@ #ifndef YARP_DEV_IVIRTUALANALOGSENSOR_H #define YARP_DEV_IVIRTUALANALOGSENSOR_H +#include #include #include -#define VOCAB_IVIRTUAL_ANALOG VOCAB4('i','v','a','n') -#define VOCAB_CALIBRATE_CHANNEL VOCAB4('c','a','l','c') +constexpr yarp::conf::vocab32_t VOCAB_IVIRTUAL_ANALOG = yarp::os::createVocab('i','v','a','n'); /*! \file IVirtualAnalogSensor.h virtual analog sensor interface */ namespace yarp { diff --git a/src/libYARP_dev/include/yarp/dev/IVisualParams.h b/src/libYARP_dev/include/yarp/dev/IVisualParams.h index 102c1f2135..e9a6e51403 100644 --- a/src/libYARP_dev/include/yarp/dev/IVisualParams.h +++ b/src/libYARP_dev/include/yarp/dev/IVisualParams.h @@ -40,32 +40,25 @@ struct yarp::dev::CameraConfig { YARP_END_PACK // Interface name -#define VOCAB_RGB_VISUAL_PARAMS VOCAB4('v','i','s','r') -#define VOCAB_DEPTH_VISUAL_PARAMS VOCAB4('v','i','s','d') +constexpr yarp::conf::vocab32_t VOCAB_RGB_VISUAL_PARAMS = yarp::os::createVocab('v','i','s','r'); +constexpr yarp::conf::vocab32_t VOCAB_DEPTH_VISUAL_PARAMS = yarp::os::createVocab('v','i','s','d'); // Common -#define VOCAB_SET VOCAB3('s','e','t') -#define VOCAB_GET VOCAB3('g','e','t') -#define VOCAB_IS VOCAB2('i','s') -#define VOCAB_OK VOCAB2('o','k') -#define VOCAB_FAILED VOCAB4('f','a','i','l') // Rgb and depth -#define VOCAB_RGB VOCAB3('r','g','b') -#define VOCAB_DEPTH VOCAB4('d','e','p','t') -#define VOCAB_MIRROR VOCAB4('m','i','r','r') +constexpr yarp::conf::vocab32_t VOCAB_RGB = yarp::os::createVocab('r','g','b'); +constexpr yarp::conf::vocab32_t VOCAB_DEPTH = yarp::os::createVocab('d','e','p','t'); +constexpr yarp::conf::vocab32_t VOCAB_MIRROR = yarp::os::createVocab('m','i','r','r'); // Methods -#define VOCAB_WIDTH VOCAB1('w') -#define VOCAB_HEIGHT VOCAB1('h') -#define VOCAB_RESOLUTION VOCAB3('r','e','s') -#define VOCAB_FOV VOCAB3('f','o','v') -#define VOCAB_INTRINSIC_PARAM VOCAB4('i','n','t','p') -#define VOCAB_SUPPORTED_CONF VOCAB4('c','o','n','f') +constexpr yarp::conf::vocab32_t VOCAB_RESOLUTION = yarp::os::createVocab('r','e','s'); +constexpr yarp::conf::vocab32_t VOCAB_FOV = yarp::os::createVocab('f','o','v'); +constexpr yarp::conf::vocab32_t VOCAB_INTRINSIC_PARAM = yarp::os::createVocab('i','n','t','p'); +constexpr yarp::conf::vocab32_t VOCAB_SUPPORTED_CONF = yarp::os::createVocab('c','o','n','f'); // Depth only -#define VOCAB_ACCURACY VOCAB4('a','c','r','c') -#define VOCAB_CLIP_PLANES VOCAB4('c','l','i','p') +constexpr yarp::conf::vocab32_t VOCAB_ACCURACY = yarp::os::createVocab('a','c','r','c'); +constexpr yarp::conf::vocab32_t VOCAB_CLIP_PLANES = yarp::os::createVocab('c','l','i','p'); diff --git a/src/libYARP_dev/include/yarp/dev/ImplementAmplifierControl.h b/src/libYARP_dev/include/yarp/dev/ImplementAmplifierControl.h new file mode 100644 index 0000000000..c09d39078d --- /dev/null +++ b/src/libYARP_dev/include/yarp/dev/ImplementAmplifierControl.h @@ -0,0 +1,191 @@ +/* + * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#ifndef YARP_DEV_IMPLEMENTAMPLIFIERCONTROL_H +#define YARP_DEV_IMPLEMENTAMPLIFIERCONTROL_H + +#include +#include + +namespace yarp +{ + namespace dev + { + class ImplementAmplifierControl; + } +} + +class YARP_dev_API yarp::dev::ImplementAmplifierControl : public IAmplifierControl +{ +protected: + IAmplifierControlRaw *iAmplifier; + void *helper; + double *dTemp; + int *iTemp; + + /** + * Initialize the internal data and alloc memory. + * @param size is the number of controlled axes the driver deals with. + * @param amap is a lookup table mapping axes onto physical drivers. + * @param enc is an array containing the encoder to angles conversion factors. + * @param zos is an array containing the zeros of the encoders. + * @return true if initialized succeeded, false if it wasn't executed, or assert. + */ + bool initialize (int size, const int *amap, const double *enc, const double *zos, const double *ampereFactor=NULL, const double *voltFactor=NULL); + + /** + * Clean up internal data and memory. + * @return true if uninitialization is executed, false otherwise. + */ + bool uninitialize (); + +public: + /* Constructor. + * @param y is the pointer to the class instance inheriting from this + * implementation. + */ + ImplementAmplifierControl(yarp::dev::IAmplifierControlRaw *y); + + /** + * Destructor. Perform uninitialize if needed. + */ + virtual ~ImplementAmplifierControl(); + + /** Enable the amplifier on a specific joint. Be careful, check that the output + * of the controller is appropriate (usually zero), to avoid + * generating abrupt movements. + * @return true/false on success/failure + */ + virtual bool enableAmp(int j) override; + + /** Disable the amplifier on a specific joint. All computations within the board + * will be carried out normally, but the output will be disabled. + * @return true/false on success/failure + */ + virtual bool disableAmp(int j) override; + + /* Get the status of the amplifiers, coded in a 32 bits integer for + * each amplifier (at the moment contains only the fault, it will be + * expanded in the future). + * @param st pointer to storage + * @return true in good luck, false otherwise. + */ + virtual bool getAmpStatus(int *st) override; + + virtual bool getAmpStatus(int j, int *st) override; + + /* Read the electric current going to all motors. + * @param vals pointer to storage for the output values + * @return hopefully true, false in bad luck. + */ + virtual bool getCurrents(double *vals) override; + + /* Read the electric current going to a given motor. + * @param j motor number + * @param val pointer to storage for the output value + * @return probably true, might return false in bad times + */ + virtual bool getCurrent(int j, double *val) override; + + /* Set the maximum electric current going to a given motor. The behavior + * of the board/amplifier when this limit is reached depends on the + * implementation. + * @param j motor number + * @param v the new value + * @return probably true, might return false in bad times + */ + virtual bool setMaxCurrent(int j, double v) override; + + /** + * Returns the maximum electric current allowed for a given motor. The behavior + * of the board/amplifier when this limit is reached depends on the + * implementation. + * @param j motor number + * @param v the return value + * @return probably true, might return false in bad times + */ + virtual bool getMaxCurrent(int j, double *v) override; + + /* Get the the nominal current which can be kept for an indefinite amount of time + * without harming the motor. This value is specific for each motor and it is typically + * found in its datasheet. The units are Ampere. + * This value and the peak current may be used by the firmware to configure + * an I2T filter. + * @param j joint number + * @param val storage for return value. [Ampere] + * @return true/false success failure. + */ + virtual bool getNominalCurrent(int m, double *val) override; + + /* Set the the nominal current which can be kept for an indefinite amount of time + * without harming the motor. This value is specific for each motor and it is typically + * found in its datasheet. The units are Ampere. + * This value and the peak current may be used by the firmware to configure + * an I2T filter. + * @param j joint number + * @param val storage for return value. [Ampere] + * @return true/false success failure. + */ + virtual bool setNominalCurrent(int m, const double val) override; + + /* Get the the peak current which causes damage to the motor if maintained + * for a long amount of time. + * The value is often found in the motor datasheet, units are Ampere. + * This value and the nominal current may be used by the firmware to configure + * an I2T filter. + * @param j joint number + * @param val storage for return value. [Ampere] + * @return true/false success failure. + */ + virtual bool getPeakCurrent(int m, double *val) override; + + /* Set the the peak current. This value which causes damage to the motor if maintained + * for a long amount of time. + * The value is often found in the motor datasheet, units are Ampere. + * This value and the nominal current may be used by the firmware to configure + * an I2T filter. + * @param j joint number + * @param val storage for return value. [Ampere] + * @return true/false success failure. + */ + virtual bool setPeakCurrent(int m, const double val) override; + + /* Get the the current PWM value used to control the motor. + * The units are firmware dependent, either machine units or percentage. + * @param j joint number + * @param val filled with PWM value. + * @return true/false success failure. + */ + virtual bool getPWM(int j, double* val) override; + + /* Get the PWM limit for the given motor. + * The units are firmware dependent, either machine units or percentage. + * @param j joint number + * @param val filled with PWM limit value. + * @return true/false success failure. + */ + virtual bool getPWMLimit(int j, double* val) override; + + /* Set the PWM limit for the given motor. + * The units are firmware dependent, either machine units or percentage. + * @param j joint number + * @param val new value for the PWM limit. + * @return true/false success failure. + */ + virtual bool setPWMLimit(int j, const double val) override; + + /* Get the power source voltage for the given motor in Volt. + * @param j joint number + * @param val filled with return value. + * @return true/false success failure. + */ + virtual bool getPowerSupplyVoltage(int j, double* val) override; +}; + + +#endif // YARP_DEV_IMPLEMENTAMPLIFIERCONTROL_H diff --git a/src/libYARP_dev/include/yarp/dev/ImplementControlBoardInterfaces.h b/src/libYARP_dev/include/yarp/dev/ImplementControlBoardInterfaces.h index dc5d2e3c60..4a44e145d7 100644 --- a/src/libYARP_dev/include/yarp/dev/ImplementControlBoardInterfaces.h +++ b/src/libYARP_dev/include/yarp/dev/ImplementControlBoardInterfaces.h @@ -10,350 +10,18 @@ #define YARP_DEV_IMPLEMENTCONTROLBOARDINTERFACES_H #include +#include #include -namespace yarp{ - namespace dev { - template class ImplementEncoders; - template class ImplementAmplifierControl; - template class ImplementControlCalibration; + +namespace yarp +{ + namespace dev + { class StubImplPositionControlRaw; class StubImplEncodersRaw; } } -template -class yarp::dev::ImplementEncoders : public IMPLEMENT -{ -protected: - IEncodersRaw *iEncoders; - void *helper; - double *temp; - - /** - * Initialize the internal data and alloc memory. - * @param size is the number of controlled axes the driver deals with. - * @param amap is a lookup table mapping axes onto physical drivers. - * @param enc is an array containing the encoder to angles conversion factors. - * @param zos is an array containing the zeros of the encoders. - * @return true if initialized succeeded, false if it wasn't executed, or assert. - */ - bool initialize (int size, const int *amap, const double *enc, const double *zos); - - /** - * Clean up internal data and memory. - * @return true if uninitialization is executed, false otherwise. - */ - bool uninitialize (); - -public: - /* Constructor. - * @param y is the pointer to the class instance inheriting from this - * implementation. - */ - ImplementEncoders(DERIVED *y); - - /** - * Destructor. Perform uninitialize if needed. - */ - virtual ~ImplementEncoders(); - - - /** - * Get the number of controlled axes. This command asks the number of controlled - * axes for the current physical interface. - * @return the number of controlled axes. - */ - virtual bool getAxes(int *ax) override; - - /** - * Reset encoder, single joint. Set the encoder value to zero - * @param j encoder number - * @return true/false - */ - virtual bool resetEncoder(int j) override; - - /** - * Reset encoders. Set the encoders value to zero - * @return true/false - */ - virtual bool resetEncoders() override; - - /** - * Set the value of the encoder for a given joint. - * @param j encoder number - * @param val new value - * @return true/false - */ - virtual bool setEncoder(int j, double val) override; - - /** - * Set the value of all encoders. - * @param vals pointer to the new values - * @return true/false - */ - virtual bool setEncoders(const double *vals) override; - - /** - * Read the value of an encoder. - * @param j encoder number - * @param v pointer to storage for the return value - * @return true/false, upon success/failure (you knew it, uh?) - */ - virtual bool getEncoder(int j, double *v) override; - - /** - * Read the position of all axes. - * @param encs pointer to the array that will contain the output - * @return true/false on success/failure - */ - virtual bool getEncoders(double *encs) override; - - /** - * Read the instantaneous speed of an axis. - * @param j axis number - * @param spds pointer to storage for the output - * @return true if successful, false ... otherwise. - */ - virtual bool getEncoderSpeed(int j, double *spds) override; - - /** - * Read the instantaneous speed of all axes. - * @param spds pointer to storage for the output values - * @return guess what? (true/false on success or failure). - */ - virtual bool getEncoderSpeeds(double *spds) override; - - /** - * Read the instantaneous acceleration of an axis. - * @param j axis number - * @param spds pointer to the array that will contain the output - */ - virtual bool getEncoderAcceleration(int j, double *spds) override; - - /** - * Read the instantaneous acceleration of all axes. - * @param accs pointer to the array that will contain the output - * @return true if all goes well, false if anything bad happens. - */ - virtual bool getEncoderAccelerations(double *accs) override; -}; - -template -class yarp::dev::ImplementControlCalibration: public IMPLEMENT -{ -protected: - IControlCalibrationRaw *iCalibrate; - void *helper; - double *temp; - - /** - * Initialize the internal data and alloc memory. - * @param size is the number of controlled axes the driver deals with. - * @param amap is a lookup table mapping axes onto physical drivers. - * @param enc is an array containing the encoder to angles conversion factors. - * @param zos is an array containing the zeros of the encoders. - * @return true if initialized succeeded, false if it wasn't executed, or assert. - */ - bool initialize (int size, const int *amap, const double *enc, const double *zos); - - /** - * Clean up internal data and memory. - * @return true if uninitialization is executed, false otherwise. - */ - bool uninitialize (); - -public: - /* Constructor. - * @param y is the pointer to the class instance inheriting from this - * implementation. - */ - ImplementControlCalibration(DERIVED *y); - - /** - * Destructor. Perform uninitialize if needed. - */ - virtual ~ImplementControlCalibration(); - - using yarp::dev::IControlCalibration::calibrate; -#ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 - virtual bool calibrate(int j, double p) override; -#endif - virtual bool calibrate(int axis, unsigned int type, double p1, double p2, double p3) override; - - virtual bool setCalibrationParameters(int axis, const CalibrationParameters& params) override; - - virtual bool done(int j) override; -}; - - -template -class yarp::dev::ImplementAmplifierControl: public IMPLEMENT -{ -protected: - IAmplifierControlRaw *iAmplifier; - void *helper; - double *dTemp; - int *iTemp; - - /** - * Initialize the internal data and alloc memory. - * @param size is the number of controlled axes the driver deals with. - * @param amap is a lookup table mapping axes onto physical drivers. - * @param enc is an array containing the encoder to angles conversion factors. - * @param zos is an array containing the zeros of the encoders. - * @return true if initialized succeeded, false if it wasn't executed, or assert. - */ - bool initialize (int size, const int *amap, const double *enc, const double *zos, const double *ampereFactor=NULL, const double *voltFactor=NULL); - - /** - * Clean up internal data and memory. - * @return true if uninitialization is executed, false otherwise. - */ - bool uninitialize (); - -public: - /* Constructor. - * @param y is the pointer to the class instance inheriting from this - * implementation. - */ - ImplementAmplifierControl(DERIVED *y); - - /** - * Destructor. Perform uninitialize if needed. - */ - virtual ~ImplementAmplifierControl(); - - /** Enable the amplifier on a specific joint. Be careful, check that the output - * of the controller is appropriate (usually zero), to avoid - * generating abrupt movements. - * @return true/false on success/failure - */ - virtual bool enableAmp(int j) override; - - /** Disable the amplifier on a specific joint. All computations within the board - * will be carried out normally, but the output will be disabled. - * @return true/false on success/failure - */ - virtual bool disableAmp(int j) override; - - /* Get the status of the amplifiers, coded in a 32 bits integer for - * each amplifier (at the moment contains only the fault, it will be - * expanded in the future). - * @param st pointer to storage - * @return true in good luck, false otherwise. - */ - virtual bool getAmpStatus(int *st) override; - - virtual bool getAmpStatus(int j, int *st) override; - - /* Read the electric current going to all motors. - * @param vals pointer to storage for the output values - * @return hopefully true, false in bad luck. - */ - virtual bool getCurrents(double *vals) override; - - /* Read the electric current going to a given motor. - * @param j motor number - * @param val pointer to storage for the output value - * @return probably true, might return false in bad times - */ - virtual bool getCurrent(int j, double *val) override; - - /* Set the maximum electric current going to a given motor. The behavior - * of the board/amplifier when this limit is reached depends on the - * implementation. - * @param j motor number - * @param v the new value - * @return probably true, might return false in bad times - */ - virtual bool setMaxCurrent(int j, double v) override; - - /** - * Returns the maximum electric current allowed for a given motor. The behavior - * of the board/amplifier when this limit is reached depends on the - * implementation. - * @param j motor number - * @param v the return value - * @return probably true, might return false in bad times - */ - virtual bool getMaxCurrent(int j, double *v) override; - - /* Get the the nominal current which can be kept for an indefinite amount of time - * without harming the motor. This value is specific for each motor and it is typically - * found in its datasheet. The units are Ampere. - * This value and the peak current may be used by the firmware to configure - * an I2T filter. - * @param j joint number - * @param val storage for return value. [Ampere] - * @return true/false success failure. - */ - virtual bool getNominalCurrent(int m, double *val) override; - - /* Set the the nominal current which can be kept for an indefinite amount of time - * without harming the motor. This value is specific for each motor and it is typically - * found in its datasheet. The units are Ampere. - * This value and the peak current may be used by the firmware to configure - * an I2T filter. - * @param j joint number - * @param val storage for return value. [Ampere] - * @return true/false success failure. - */ - virtual bool setNominalCurrent(int m, const double val) override; - - /* Get the the peak current which causes damage to the motor if maintained - * for a long amount of time. - * The value is often found in the motor datasheet, units are Ampere. - * This value and the nominal current may be used by the firmware to configure - * an I2T filter. - * @param j joint number - * @param val storage for return value. [Ampere] - * @return true/false success failure. - */ - virtual bool getPeakCurrent(int m, double *val) override; - - /* Set the the peak current. This value which causes damage to the motor if maintained - * for a long amount of time. - * The value is often found in the motor datasheet, units are Ampere. - * This value and the nominal current may be used by the firmware to configure - * an I2T filter. - * @param j joint number - * @param val storage for return value. [Ampere] - * @return true/false success failure. - */ - virtual bool setPeakCurrent(int m, const double val) override; - - /* Get the the current PWM value used to control the motor. - * The units are firmware dependent, either machine units or percentage. - * @param j joint number - * @param val filled with PWM value. - * @return true/false success failure. - */ - virtual bool getPWM(int j, double* val) override; - - /* Get the PWM limit for the given motor. - * The units are firmware dependent, either machine units or percentage. - * @param j joint number - * @param val filled with PWM limit value. - * @return true/false success failure. - */ - virtual bool getPWMLimit(int j, double* val) override; - - /* Set the PWM limit for the given motor. - * The units are firmware dependent, either machine units or percentage. - * @param j joint number - * @param val new value for the PWM limit. - * @return true/false success failure. - */ - virtual bool setPWMLimit(int j, const double val) override; - - /* Get the power source voltage for the given motor in Volt. - * @param j joint number - * @param val filled with return value. - * @return true/false success failure. - */ - virtual bool getPowerSupplyVoltage(int j, double* val) override; -}; - /** * Stub implementation of IEncodersRaw interface. * Inherit from this class if you want a stub implementation diff --git a/src/libYARP_dev/include/yarp/dev/ImplementControlCalibration.h b/src/libYARP_dev/include/yarp/dev/ImplementControlCalibration.h new file mode 100644 index 0000000000..f6364ca495 --- /dev/null +++ b/src/libYARP_dev/include/yarp/dev/ImplementControlCalibration.h @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#ifndef YARP_DEV_IMPLEMENTCONTROLCALIBRATION_H +#define YARP_DEV_IMPLEMENTCONTROLCALIBRATION_H + +#include +#include +#include + +#include +namespace yarp{ + namespace dev { + class ImplementControlCalibration; + } +} + +class YARP_dev_API yarp::dev::ImplementControlCalibration : public IControlCalibration +{ +protected: + IControlCalibrationRaw *iCalibrate; + void *helper; + double *temp; + + /** + * Initialize the internal data and alloc memory. + * @param size is the number of controlled axes the driver deals with. + * @param amap is a lookup table mapping axes onto physical drivers. + * @param enc is an array containing the encoder to angles conversion factors. + * @param zos is an array containing the zeros of the encoders. + * @return true if initialized succeeded, false if it wasn't executed, or assert. + */ + bool initialize (int size, const int *amap, const double *enc, const double *zos); + + /** + * Clean up internal data and memory. + * @return true if uninitialization is executed, false otherwise. + */ + bool uninitialize (); + +public: + /* Constructor. + * @param y is the pointer to the class instance inheriting from this + * implementation. + */ + ImplementControlCalibration(yarp::dev::IControlCalibrationRaw *y); + + /** + * Destructor. Perform uninitialize if needed. + */ + virtual ~ImplementControlCalibration(); + + virtual bool calibrateAxisWithParams(int axis, unsigned int type, double p1, double p2, double p3) override; + + virtual bool setCalibrationParameters(int axis, const CalibrationParameters& params) override; + + virtual bool calibrationDone(int j) override; +}; + + +#endif // YARP_DEV_IMPLEMENTCONTROLCALIBRATION_H diff --git a/src/libYARP_dev/include/yarp/dev/IControlLimitsImpl.h b/src/libYARP_dev/include/yarp/dev/ImplementControlLimits.h similarity index 100% rename from src/libYARP_dev/include/yarp/dev/IControlLimitsImpl.h rename to src/libYARP_dev/include/yarp/dev/ImplementControlLimits.h diff --git a/src/libYARP_dev/include/yarp/dev/IControlLimits2Impl.h b/src/libYARP_dev/include/yarp/dev/ImplementControlLimits2.h similarity index 100% rename from src/libYARP_dev/include/yarp/dev/IControlLimits2Impl.h rename to src/libYARP_dev/include/yarp/dev/ImplementControlLimits2.h diff --git a/src/libYARP_dev/include/yarp/dev/ImplementEncoders.h b/src/libYARP_dev/include/yarp/dev/ImplementEncoders.h new file mode 100644 index 0000000000..a00585a953 --- /dev/null +++ b/src/libYARP_dev/include/yarp/dev/ImplementEncoders.h @@ -0,0 +1,139 @@ +/* + * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#ifndef YARP_DEV_IMPLEMENTENCODERS_H +#define YARP_DEV_IMPLEMENTENCODERS_H + +#include +#include + +namespace yarp +{ + namespace dev + { + class ImplementEncoders; + } +} + +class YARP_dev_API yarp::dev::ImplementEncoders : public IEncoders +{ +protected: + IEncodersRaw *iEncoders; + void *helper; + double *temp; + + /** + * Initialize the internal data and alloc memory. + * @param size is the number of controlled axes the driver deals with. + * @param amap is a lookup table mapping axes onto physical drivers. + * @param enc is an array containing the encoder to angles conversion factors. + * @param zos is an array containing the zeros of the encoders. + * @return true if initialized succeeded, false if it wasn't executed, or assert. + */ + bool initialize (int size, const int *amap, const double *enc, const double *zos); + + /** + * Clean up internal data and memory. + * @return true if uninitialization is executed, false otherwise. + */ + bool uninitialize (); + +public: + /* Constructor. + * @param y is the pointer to the class instance inheriting from this + * implementation. + */ + ImplementEncoders(yarp::dev::IEncodersRaw *y); + + /** + * Destructor. Perform uninitialize if needed. + */ + virtual ~ImplementEncoders(); + + + /** + * Get the number of controlled axes. This command asks the number of controlled + * axes for the current physical interface. + * @return the number of controlled axes. + */ + virtual bool getAxes(int *ax) override; + + /** + * Reset encoder, single joint. Set the encoder value to zero + * @param j encoder number + * @return true/false + */ + virtual bool resetEncoder(int j) override; + + /** + * Reset encoders. Set the encoders value to zero + * @return true/false + */ + virtual bool resetEncoders() override; + + /** + * Set the value of the encoder for a given joint. + * @param j encoder number + * @param val new value + * @return true/false + */ + virtual bool setEncoder(int j, double val) override; + + /** + * Set the value of all encoders. + * @param vals pointer to the new values + * @return true/false + */ + virtual bool setEncoders(const double *vals) override; + + /** + * Read the value of an encoder. + * @param j encoder number + * @param v pointer to storage for the return value + * @return true/false, upon success/failure (you knew it, uh?) + */ + virtual bool getEncoder(int j, double *v) override; + + /** + * Read the position of all axes. + * @param encs pointer to the array that will contain the output + * @return true/false on success/failure + */ + virtual bool getEncoders(double *encs) override; + + /** + * Read the instantaneous speed of an axis. + * @param j axis number + * @param spds pointer to storage for the output + * @return true if successful, false ... otherwise. + */ + virtual bool getEncoderSpeed(int j, double *spds) override; + + /** + * Read the instantaneous speed of all axes. + * @param spds pointer to storage for the output values + * @return guess what? (true/false on success or failure). + */ + virtual bool getEncoderSpeeds(double *spds) override; + + /** + * Read the instantaneous acceleration of an axis. + * @param j axis number + * @param spds pointer to the array that will contain the output + */ + virtual bool getEncoderAcceleration(int j, double *spds) override; + + /** + * Read the instantaneous acceleration of all axes. + * @param accs pointer to the array that will contain the output + * @return true if all goes well, false if anything bad happens. + */ + virtual bool getEncoderAccelerations(double *accs) override; +}; + +#endif // YARP_DEV_IMPLEMENTENCODERS_H diff --git a/src/libYARP_dev/include/yarp/dev/IInteractionModeImpl.h b/src/libYARP_dev/include/yarp/dev/ImplementInteractionMode.h similarity index 100% rename from src/libYARP_dev/include/yarp/dev/IInteractionModeImpl.h rename to src/libYARP_dev/include/yarp/dev/ImplementInteractionMode.h diff --git a/src/libYARP_dev/include/yarp/dev/IPidControlImpl.h b/src/libYARP_dev/include/yarp/dev/ImplementPidControl.h similarity index 100% rename from src/libYARP_dev/include/yarp/dev/IPidControlImpl.h rename to src/libYARP_dev/include/yarp/dev/ImplementPidControl.h diff --git a/src/libYARP_dev/include/yarp/dev/IPositionControlImpl.h b/src/libYARP_dev/include/yarp/dev/ImplementPositionControl.h similarity index 100% rename from src/libYARP_dev/include/yarp/dev/IPositionControlImpl.h rename to src/libYARP_dev/include/yarp/dev/ImplementPositionControl.h diff --git a/src/libYARP_dev/include/yarp/dev/IPositionControl2Impl.h b/src/libYARP_dev/include/yarp/dev/ImplementPositionControl2.h similarity index 94% rename from src/libYARP_dev/include/yarp/dev/IPositionControl2Impl.h rename to src/libYARP_dev/include/yarp/dev/ImplementPositionControl2.h index 951a56be0f..84bead2ccb 100644 --- a/src/libYARP_dev/include/yarp/dev/IPositionControl2Impl.h +++ b/src/libYARP_dev/include/yarp/dev/ImplementPositionControl2.h @@ -10,7 +10,7 @@ #define YARP_DEV_IPOSITIONCONTROL2IMPL_H -#include +#include namespace yarp{ namespace dev { diff --git a/src/libYARP_dev/include/yarp/dev/IPositionDirectImpl.h b/src/libYARP_dev/include/yarp/dev/ImplementPositionDirect.h similarity index 100% rename from src/libYARP_dev/include/yarp/dev/IPositionDirectImpl.h rename to src/libYARP_dev/include/yarp/dev/ImplementPositionDirect.h diff --git a/src/libYARP_dev/include/yarp/dev/IVelocityControlImpl.h b/src/libYARP_dev/include/yarp/dev/ImplementVelocityControl.h similarity index 100% rename from src/libYARP_dev/include/yarp/dev/IVelocityControlImpl.h rename to src/libYARP_dev/include/yarp/dev/ImplementVelocityControl.h diff --git a/src/libYARP_dev/include/yarp/dev/IVelocityControl2Impl.h b/src/libYARP_dev/include/yarp/dev/ImplementVelocityControl2.h similarity index 94% rename from src/libYARP_dev/include/yarp/dev/IVelocityControl2Impl.h rename to src/libYARP_dev/include/yarp/dev/ImplementVelocityControl2.h index 45514d856b..9ae2e0c7c8 100644 --- a/src/libYARP_dev/include/yarp/dev/IVelocityControl2Impl.h +++ b/src/libYARP_dev/include/yarp/dev/ImplementVelocityControl2.h @@ -10,7 +10,7 @@ #define YARP_DEV_IVELOCITYCONTROL2IMPL_H -#include +#include #include namespace yarp { diff --git a/src/libYARP_dev/include/yarp/dev/IVirtualAnalogSensorImpl.h b/src/libYARP_dev/include/yarp/dev/ImplementVirtualAnalogSensor.h similarity index 100% rename from src/libYARP_dev/include/yarp/dev/IVirtualAnalogSensorImpl.h rename to src/libYARP_dev/include/yarp/dev/ImplementVirtualAnalogSensor.h diff --git a/src/libYARP_dev/include/yarp/dev/MultipleAnalogSensorsInterfaces.h b/src/libYARP_dev/include/yarp/dev/MultipleAnalogSensorsInterfaces.h index b72bf52ed0..12f28e0a8d 100644 --- a/src/libYARP_dev/include/yarp/dev/MultipleAnalogSensorsInterfaces.h +++ b/src/libYARP_dev/include/yarp/dev/MultipleAnalogSensorsInterfaces.h @@ -17,21 +17,32 @@ namespace yarp { -namespace dev -{ - -/** - * Status of a given analog sensor exposed by a multiple analog sensors interface. - */ -enum MAS_status -{ - MAS_OK=0, ///< The sensor is working correctly. - MAS_ERROR=1, ///< The sensor is in generic error state. - MAS_OVF=2, ///< The sensor reached an overflow. - MAS_TIMEOUT=3, ///< The sensor is read through the network, and the latest measurement was received before an implementation-define timeout period. - MAS_WAITING_FOR_FIRST_READ=4, ///< The sensor is read through the network, and the device is waiting to receive the first measurement. - MAS_UNKNOWN=5 ///< The sensor is in an unknown state. -}; + namespace dev + { + class IThreeAxisGyroscopes; + class IThreeAxisLinearAccelerometers; + class IThreeAxisMagnetometers; + class IOrientationSensors; + class ITemperatureSensors; + class ISixAxisForceTorqueSensors; + class IContactLoadCellArrays; + class IEncoderArrays; + class ISkinPatches; + + /** + * Status of a given analog sensor exposed by a multiple analog sensors interface. + */ + enum MAS_status + { + MAS_OK=0, ///< The sensor is working correctly. + MAS_ERROR=1, ///< The sensor is in generic error state. + MAS_OVF=2, ///< The sensor reached an overflow. + MAS_TIMEOUT=3, ///< The sensor is read through the network, and the latest measurement was received before an implementation-define timeout period. + MAS_WAITING_FOR_FIRST_READ=4, ///< The sensor is read through the network, and the device is waiting to receive the first measurement. + MAS_UNKNOWN=5 ///< The sensor is in an unknown state. + }; + } +} /** * @ingroup dev_iface_multiple_analog @@ -48,7 +59,7 @@ enum MAS_status * | `ThreeAxisGyroscopes` | * */ -class YARP_dev_API IThreeAxisGyroscopes +class YARP_dev_API yarp::dev::IThreeAxisGyroscopes { public: /** @@ -102,7 +113,7 @@ class YARP_dev_API IThreeAxisGyroscopes * |:-----------------:| * | `ThreeAxisLinearAccelerometers` | */ -class YARP_dev_API IThreeAxisLinearAccelerometers +class YARP_dev_API yarp::dev::IThreeAxisLinearAccelerometers { public: /** @@ -149,7 +160,7 @@ class YARP_dev_API IThreeAxisLinearAccelerometers * |:-----------------:| * | `ThreeAxisMagnetometers` | */ -class YARP_dev_API IThreeAxisMagnetometers +class YARP_dev_API yarp::dev::IThreeAxisMagnetometers { public: /** @@ -201,7 +212,7 @@ class YARP_dev_API IThreeAxisMagnetometers * |:-----------------:| * | `OrientationSensors` | */ -class YARP_dev_API IOrientationSensors +class YARP_dev_API yarp::dev::IOrientationSensors { public: /** @@ -292,7 +303,7 @@ class YARP_dev_API IOrientationSensors * |:-----------------:| * | `TemperatureSensors` | */ -class YARP_dev_API ITemperatureSensors +class YARP_dev_API yarp::dev::ITemperatureSensors { public: /** @@ -346,7 +357,7 @@ class YARP_dev_API ITemperatureSensors * |:-----------------:| * | `SixAxisForceTorqueSensors` | */ -class YARP_dev_API ISixAxisForceTorqueSensors +class YARP_dev_API yarp::dev::ISixAxisForceTorqueSensors { public: /** @@ -395,7 +406,7 @@ class YARP_dev_API ISixAxisForceTorqueSensors * @note This interface is meant to expose array of sensors of normal force * for contact, such as the FSR array present in the Nao Robot ( http://doc.aldebaran.com/1-14/family/robots/fsr_robot.html ). */ -class YARP_dev_API IContactLoadCellArrays +class YARP_dev_API yarp::dev::IContactLoadCellArrays { public: /** @@ -448,7 +459,7 @@ class YARP_dev_API IContactLoadCellArrays * motor control, such as IEncoders and IPositionControl, such as * encoders measuring the complete state in an underactuated mechanism. */ -class YARP_dev_API IEncoderArrays +class YARP_dev_API yarp::dev::IEncoderArrays { public: /** @@ -497,7 +508,7 @@ class YARP_dev_API IEncoderArrays * * \brief Device interface to one or multiple patches of tacticle sensors. */ -class YARP_dev_API ISkinPatches +class YARP_dev_API yarp::dev::ISkinPatches { public: /** @@ -534,9 +545,6 @@ class YARP_dev_API ISkinPatches virtual ~ISkinPatches(){} }; -} -} - #endif diff --git a/src/libYARP_dev/include/yarp/dev/PidEnums.h b/src/libYARP_dev/include/yarp/dev/PidEnums.h index 09330458b4..f8d61251d8 100644 --- a/src/libYARP_dev/include/yarp/dev/PidEnums.h +++ b/src/libYARP_dev/include/yarp/dev/PidEnums.h @@ -18,10 +18,10 @@ namespace yarp { enum YARP_dev_API PidControlTypeEnum { - VOCAB_PIDTYPE_POSITION = VOCAB3('p', 'o', 's'), - VOCAB_PIDTYPE_VELOCITY = VOCAB3('v', 'e', 'l'), - VOCAB_PIDTYPE_TORQUE = VOCAB3('t', 'r', 'q'), - VOCAB_PIDTYPE_CURRENT = VOCAB3('c', 'u', 'r') + VOCAB_PIDTYPE_POSITION = yarp::os::createVocab('p', 'o', 's'), + VOCAB_PIDTYPE_VELOCITY = yarp::os::createVocab('v', 'e', 'l'), + VOCAB_PIDTYPE_TORQUE = yarp::os::createVocab('t', 'r', 'q'), + VOCAB_PIDTYPE_CURRENT = yarp::os::createVocab('c', 'u', 'r') }; YARP_WARNING_PUSH diff --git a/src/libYARP_dev/include/yarp/dev/all.h b/src/libYARP_dev/include/yarp/dev/all.h index 759fb3f036..d4c74ded81 100644 --- a/src/libYARP_dev/include/yarp/dev/all.h +++ b/src/libYARP_dev/include/yarp/dev/all.h @@ -16,6 +16,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include @@ -32,6 +36,7 @@ #include #include #include +#include #include #include #include diff --git a/src/libYARP_dev/src/ControlBoardInterfacesImpl.cpp b/src/libYARP_dev/src/ControlBoardInterfacesImpl.cpp index ddce20679b..ba85bacb00 100644 --- a/src/libYARP_dev/src/ControlBoardInterfacesImpl.cpp +++ b/src/libYARP_dev/src/ControlBoardInterfacesImpl.cpp @@ -8,7 +8,6 @@ */ #include "yarp/dev/ControlBoardInterfacesImpl.h" -#include "yarp/dev/ControlBoardInterfacesImpl-inl.h" //ControlBoardHelper #include #include diff --git a/src/libYARP_dev/src/ControlCalibrationImpl.cpp b/src/libYARP_dev/src/ControlCalibrationImpl.cpp deleted file mode 100644 index 1fc568d844..0000000000 --- a/src/libYARP_dev/src/ControlCalibrationImpl.cpp +++ /dev/null @@ -1,76 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * Copyright (C) 2006-2010 RobotCub Consortium - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#include -#include -#include - -#include - -using namespace yarp::dev; - -IControlCalibration::IControlCalibration() -{ - calibrator=nullptr; -} - -bool IControlCalibration::setCalibrator(ICalibrator *c) -{ - if (c!=nullptr) - { - calibrator=c; - return true; - } - - return false; -} - -bool IControlCalibration::calibrate() -{ - bool ret = false; - if (calibrator!=nullptr) - { - yDebug("Going to call calibrator\n"); - ret=calibrator->calibrate(dynamic_cast(this)); - } - else - yWarning("Warning called calibrate but no calibrator was set\n"); - - return ret; -} - -bool IControlCalibration::abortCalibration() -{ - bool ret=false; - if (calibrator!=nullptr) - ret=calibrator->quitCalibrate(); - return ret; -} - -bool IControlCalibration::abortPark() -{ - bool ret=false; - if (calibrator!=nullptr) - ret=calibrator->quitPark(); - return ret; -} - -bool IControlCalibration::park(bool wait) -{ - bool ret=false; - if (calibrator!=nullptr) - { - yDebug("Going to call calibrator\n"); - ret=calibrator->park(dynamic_cast(this), wait); - } - else - yWarning("Warning called park but no calibrator was set\n"); - - return ret; -} diff --git a/src/libYARP_dev/src/DeviceDriver.cpp b/src/libYARP_dev/src/DeviceDriver.cpp index ef9470f605..4a2eb76fc1 100644 --- a/src/libYARP_dev/src/DeviceDriver.cpp +++ b/src/libYARP_dev/src/DeviceDriver.cpp @@ -38,7 +38,7 @@ void DeviceResponder::addUsage(const Bottle& bot, const char *explain) { bool DeviceResponder::respond(const Bottle& command, Bottle& reply) { switch (command.get(0).asVocab()) { - case VOCAB4('h','e','l','p'): + case yarp::os::createVocab('h','e','l','p'): if (examples.size()>=1) { reply.add(Value::makeVocab("many")); if (command.get(1).toString()=="more") { diff --git a/src/libYARP_dev/src/FrameGrabberControlImpl.cpp b/src/libYARP_dev/src/FrameGrabberControlImpl.cpp index 05e683f6ec..627882e4b4 100644 --- a/src/libYARP_dev/src/FrameGrabberControlImpl.cpp +++ b/src/libYARP_dev/src/FrameGrabberControlImpl.cpp @@ -8,6 +8,7 @@ #include #include +#include using namespace yarp::os; using namespace yarp::dev; diff --git a/src/libYARP_dev/src/IVisualParamsImpl.cpp b/src/libYARP_dev/src/IVisualParamsImpl.cpp index d635a6efcd..2bdc7362ea 100644 --- a/src/libYARP_dev/src/IVisualParamsImpl.cpp +++ b/src/libYARP_dev/src/IVisualParamsImpl.cpp @@ -8,6 +8,7 @@ #include #include +#include #include using namespace yarp::os; diff --git a/src/libYARP_dev/src/ImplementAmplifierControl.cpp b/src/libYARP_dev/src/ImplementAmplifierControl.cpp new file mode 100644 index 0000000000..00c000d45b --- /dev/null +++ b/src/libYARP_dev/src/ImplementAmplifierControl.cpp @@ -0,0 +1,196 @@ +/* + * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2010 RobotCub Consortium + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#include +#include + +#include + +// Be careful: this file contains template implementations and is included by translation +// units that use the template (e.g. .cpp files). Avoid putting here non-template functions to +// avoid multiple definitions. + +using namespace yarp::dev; + +ImplementAmplifierControl::ImplementAmplifierControl(yarp::dev::IAmplifierControlRaw *y) +{ + iAmplifier= y; + helper = 0; + dTemp=0; + iTemp=0; +} + +ImplementAmplifierControl::~ImplementAmplifierControl() +{ + uninitialize(); +} + +bool ImplementAmplifierControl:: initialize (int size, const int *amap, const double *enc, const double *zos, const double *ampereFactor, const double *voltFactor) +{ + if (helper!=0) + return false; + + helper=(void *)(new ControlBoardHelper(size, amap, enc, zos,nullptr, ampereFactor, voltFactor)); + yAssert (helper != 0); + dTemp=new double[size]; + yAssert (dTemp != 0); + iTemp=new int[size]; + yAssert (iTemp != 0); + + return true; +} + +/** +* Clean up internal data and memory. +* @return true if uninitialization is executed, false otherwise. +*/ +bool ImplementAmplifierControl::uninitialize () +{ + if (helper!=0) + delete castToMapper(helper); + + delete [] dTemp; + delete [] iTemp; + + helper=0; + dTemp=0; + iTemp=0; + return true; +} + +bool ImplementAmplifierControl::enableAmp(int j) +{ + int k=castToMapper(helper)->toHw(j); + + return iAmplifier->enableAmpRaw(k); +} + +bool ImplementAmplifierControl::disableAmp(int j) +{ + int k=castToMapper(helper)->toHw(j); + + return iAmplifier->disableAmpRaw(k); +} + +bool ImplementAmplifierControl::getCurrents(double *currs) +{ + bool ret=iAmplifier->getCurrentsRaw(dTemp); + castToMapper(helper)->ampereS2A(dTemp, currs); + return ret; +} + +bool ImplementAmplifierControl::getCurrent(int j, double *c) +{ + double temp = 0; + int k = castToMapper(helper)->toHw(j); + bool ret = iAmplifier->getCurrentRaw(k, &temp); + castToMapper(helper)->ampereS2A(temp, k, *c, j); + return ret; +} + +bool ImplementAmplifierControl::getAmpStatus(int *st) +{ + bool ret=iAmplifier->getAmpStatusRaw(iTemp); + castToMapper(helper)->toUser(iTemp, st); + + return ret; +} + +bool ImplementAmplifierControl::getAmpStatus(int k, int *st) +{ + int j=castToMapper(helper)->toHw(k); + bool ret=iAmplifier->getAmpStatusRaw(j, st); + + return ret; +} + +bool ImplementAmplifierControl::setMaxCurrent(int m, double v) +{ + int k; + double curr; + castToMapper(helper)->ampereA2S(v, m, curr, k); + return iAmplifier->setMaxCurrentRaw(k, curr); +} + +bool ImplementAmplifierControl::getMaxCurrent(int j, double* v) +{ + double val; + int k=castToMapper(helper)->toHw(j); + bool ret = iAmplifier->getMaxCurrentRaw(k, &val); + *v = castToMapper(helper)->ampereS2A(val, k); + return ret; +} + +bool ImplementAmplifierControl::getNominalCurrent(int m, double *curr) +{ + int k; + bool ret; + double tmp; + + k=castToMapper(helper)->toHw(m); + ret=iAmplifier->getNominalCurrentRaw(k, &tmp); + *curr=castToMapper(helper)->ampereS2A(tmp, k); + return ret; +} + +bool ImplementAmplifierControl::getPeakCurrent(int m, double *curr) +{ + int k; + bool ret; + double tmp; + + k=castToMapper(helper)->toHw(m); + ret=iAmplifier->getPeakCurrentRaw(k, &tmp); + *curr=castToMapper(helper)->ampereS2A(tmp, k); + return ret; +} + +bool ImplementAmplifierControl::setPeakCurrent(int m, const double curr) +{ + int k; + double val; + castToMapper(helper)->ampereA2S(curr, m, val, k); + return iAmplifier->setPeakCurrentRaw(k, val); +} + +bool ImplementAmplifierControl::setNominalCurrent(int m, const double curr) +{ + int k; + double val; + castToMapper(helper)->ampereA2S(curr, m, val, k); + return iAmplifier->setNominalCurrentRaw(k, val); +} + +bool ImplementAmplifierControl::getPWM(int m, double* pwm) +{ + int k; + k=castToMapper(helper)->toHw(m); + return iAmplifier->getPWMRaw(k, pwm); +} + +bool ImplementAmplifierControl::getPWMLimit(int m, double* limit) +{ + int k; + k=castToMapper(helper)->toHw(m); + return iAmplifier->getPWMLimitRaw(k, limit); +} + +bool ImplementAmplifierControl::setPWMLimit(int m, const double limit) +{ + int k; + k=castToMapper(helper)->toHw(m); + return iAmplifier->setPWMLimitRaw(k, limit); +} + +bool ImplementAmplifierControl::getPowerSupplyVoltage(int m, double *voltage) +{ + int k; + k=castToMapper(helper)->toHw(m); + return iAmplifier->getPowerSupplyVoltageRaw(k, voltage); +} diff --git a/src/libYARP_dev/src/IAxisInfoImpl.cpp b/src/libYARP_dev/src/ImplementAxisInfo.cpp similarity index 100% rename from src/libYARP_dev/src/IAxisInfoImpl.cpp rename to src/libYARP_dev/src/ImplementAxisInfo.cpp diff --git a/src/libYARP_dev/src/ImplementControlCalibration.cpp b/src/libYARP_dev/src/ImplementControlCalibration.cpp new file mode 100644 index 0000000000..ef24aed5b8 --- /dev/null +++ b/src/libYARP_dev/src/ImplementControlCalibration.cpp @@ -0,0 +1,141 @@ +/* + * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2010 RobotCub Consortium + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#include +#include +#include +#include + +#include + +using namespace yarp::dev; + +IControlCalibration::IControlCalibration() +{ + calibrator=nullptr; +} + +bool IControlCalibration::setCalibrator(ICalibrator *c) +{ + if (c!=nullptr) + { + calibrator=c; + return true; + } + + return false; +} + +bool IControlCalibration::calibrateRobot() +{ + bool ret = false; + if (calibrator!=nullptr) + { + yDebug("Going to call calibrator\n"); + ret=calibrator->calibrate(dynamic_cast(this)); + } + else + yWarning("Warning called calibrate but no calibrator was set\n"); + + return ret; +} + +bool IControlCalibration::abortCalibration() +{ + bool ret=false; + if (calibrator!=nullptr) + ret=calibrator->quitCalibrate(); + return ret; +} + +bool IControlCalibration::abortPark() +{ + bool ret=false; + if (calibrator!=nullptr) + ret=calibrator->quitPark(); + return ret; +} + +bool IControlCalibration::park(bool wait) +{ + bool ret=false; + if (calibrator!=nullptr) + { + yDebug("Going to call calibrator\n"); + ret=calibrator->park(dynamic_cast(this), wait); + } + else + yWarning("Warning called park but no calibrator was set\n"); + + return ret; +} + +//////////////////////// +// ControlCalibration Interface Implementation +ImplementControlCalibration::ImplementControlCalibration(yarp::dev::IControlCalibrationRaw *y) +{ + iCalibrate = dynamic_cast (y); + helper = 0; + temp = 0; +} + +ImplementControlCalibration::~ImplementControlCalibration() +{ + uninitialize(); +} + +bool ImplementControlCalibration::initialize(int size, const int *amap, const double *enc, const double *zos) +{ + if (helper != 0) + return false; + + helper = (void *)(new ControlBoardHelper(size, amap, enc, zos)); + yAssert(helper != 0); + temp = new double[size]; + yAssert(temp != 0); + return true; +} + +/** +* Clean up internal data and memory. +* @return true if uninitialization is executed, false otherwise. +*/ +bool ImplementControlCalibration::uninitialize() +{ + if (helper != 0) + { + delete castToMapper(helper); + helper = 0; + } + + checkAndDestroy(temp); + + return true; +} + +bool ImplementControlCalibration::calibrationDone(int j) +{ + int k = castToMapper(helper)->toHw(j); + + return iCalibrate->calibrationDoneRaw(k); +} + +bool ImplementControlCalibration::calibrateAxisWithParams(int axis, unsigned int type, double p1, double p2, double p3) +{ + int k = castToMapper(helper)->toHw(axis); + + return iCalibrate->calibrateAxisWithParamsRaw(k, type, p1, p2, p3); +} + +bool ImplementControlCalibration::setCalibrationParameters(int axis, const CalibrationParameters& params) +{ + int k = castToMapper(helper)->toHw(axis); + + return iCalibrate->setCalibrationParametersRaw(k, params); +} \ No newline at end of file diff --git a/src/libYARP_dev/src/IControlLimitsImpl.cpp b/src/libYARP_dev/src/ImplementControlLimits.cpp similarity index 98% rename from src/libYARP_dev/src/IControlLimitsImpl.cpp rename to src/libYARP_dev/src/ImplementControlLimits.cpp index a0b857624a..f5efd522a9 100644 --- a/src/libYARP_dev/src/IControlLimitsImpl.cpp +++ b/src/libYARP_dev/src/ImplementControlLimits.cpp @@ -8,7 +8,7 @@ #include -#include +#include #include using namespace yarp::dev; diff --git a/src/libYARP_dev/src/ControlModeImpl.cpp b/src/libYARP_dev/src/ImplementControlMode.cpp similarity index 100% rename from src/libYARP_dev/src/ControlModeImpl.cpp rename to src/libYARP_dev/src/ImplementControlMode.cpp diff --git a/src/libYARP_dev/src/CurrentControlImpl.cpp b/src/libYARP_dev/src/ImplementCurrentControl.cpp similarity index 100% rename from src/libYARP_dev/src/CurrentControlImpl.cpp rename to src/libYARP_dev/src/ImplementCurrentControl.cpp diff --git a/src/libYARP_dev/src/ImplementEncoders.cpp b/src/libYARP_dev/src/ImplementEncoders.cpp new file mode 100644 index 0000000000..45f902afff --- /dev/null +++ b/src/libYARP_dev/src/ImplementEncoders.cpp @@ -0,0 +1,177 @@ +/* + * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2006-2010 RobotCub Consortium + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#include +#include + +#include + +// Be careful: this file contains template implementations and is included by translation +// units that use the template (e.g. .cpp files). Avoid putting here non-template functions to +// avoid multiple definitions. + +using namespace yarp::dev; + +//////////////////////// +// Encoder Interface Implementation +ImplementEncoders::ImplementEncoders(yarp::dev::IEncodersRaw *y) +{ + iEncoders= y; + helper = 0; + temp=0; +} + +ImplementEncoders::~ImplementEncoders() +{ + uninitialize(); +} + +bool ImplementEncoders:: initialize (int size, const int *amap, const double *enc, const double *zos) +{ + if (helper!=0) + return false; + + helper=(void *)(new ControlBoardHelper(size, amap, enc, zos)); + yAssert (helper != 0); + temp=new double [size]; + yAssert (temp != 0); + return true; +} + +/** +* Clean up internal data and memory. +* @return true if uninitialization is executed, false otherwise. +*/ +bool ImplementEncoders::uninitialize () +{ + if (helper!=0) + { + delete castToMapper(helper); + helper=0; + } + + checkAndDestroy(temp); + + return true; +} + +bool ImplementEncoders::getAxes(int *ax) +{ + (*ax)=castToMapper(helper)->axes(); + return true; +} + +bool ImplementEncoders::resetEncoder(int j) +{ + int k; + k=castToMapper(helper)->toHw(j); + + return iEncoders->resetEncoderRaw(k); +} + + +bool ImplementEncoders::resetEncoders() +{ + return iEncoders->resetEncodersRaw(); +} + +bool ImplementEncoders::setEncoder(int j, double val) +{ + int k; + double enc; + + castToMapper(helper)->posA2E(val, j, enc, k); + + return iEncoders->setEncoderRaw(k, enc); +} + +bool ImplementEncoders::setEncoders(const double *val) +{ + castToMapper(helper)->posA2E(val, temp); + + return iEncoders->setEncodersRaw(temp); +} + +bool ImplementEncoders::getEncoder(int j, double *v) +{ + int k; + double enc; + bool ret; + + k=castToMapper(helper)->toHw(j); + + ret=iEncoders->getEncoderRaw(k, &enc); + + *v=castToMapper(helper)->posE2A(enc, k); + + return ret; +} + +bool ImplementEncoders::getEncoders(double *v) +{ + bool ret; + castToMapper(helper)->axes(); + + ret=iEncoders->getEncodersRaw(temp); + + castToMapper(helper)->posE2A(temp, v); + + return ret; +} + +bool ImplementEncoders::getEncoderSpeed(int j, double *v) +{ + int k; + double enc; + bool ret; + + k=castToMapper(helper)->toHw(j); + + ret=iEncoders->getEncoderSpeedRaw(k, &enc); + + *v=castToMapper(helper)->velE2A(enc, k); + + return ret; +} + +bool ImplementEncoders::getEncoderSpeeds(double *v) +{ + bool ret; + ret=iEncoders->getEncoderSpeedsRaw(temp); + + castToMapper(helper)->velE2A(temp, v); + + return ret; +} + +bool ImplementEncoders::getEncoderAcceleration(int j, double *v) +{ + int k; + double enc; + bool ret; + + k=castToMapper(helper)->toHw(j); + + ret=iEncoders->getEncoderAccelerationRaw(k, &enc); + + *v=castToMapper(helper)->accE2A(enc, k); + + return ret; +} + +bool ImplementEncoders::getEncoderAccelerations(double *v) +{ + bool ret; + ret=iEncoders->getEncoderAccelerationsRaw(temp); + + castToMapper(helper)->accE2A(temp, v); + + return ret; +} + diff --git a/src/libYARP_dev/src/IEncodersTimedImpl.cpp b/src/libYARP_dev/src/ImplementEncodersTimed.cpp similarity index 100% rename from src/libYARP_dev/src/IEncodersTimedImpl.cpp rename to src/libYARP_dev/src/ImplementEncodersTimed.cpp diff --git a/src/libYARP_dev/src/ImpedanceControlImpl.cpp b/src/libYARP_dev/src/ImplementImpedanceControl.cpp similarity index 100% rename from src/libYARP_dev/src/ImpedanceControlImpl.cpp rename to src/libYARP_dev/src/ImplementImpedanceControl.cpp diff --git a/src/libYARP_dev/src/IInteractionModeImpl.cpp b/src/libYARP_dev/src/ImplementInteractionMode.cpp similarity index 98% rename from src/libYARP_dev/src/IInteractionModeImpl.cpp rename to src/libYARP_dev/src/ImplementInteractionMode.cpp index 8d2c4e28cb..80c5546ec9 100644 --- a/src/libYARP_dev/src/IInteractionModeImpl.cpp +++ b/src/libYARP_dev/src/ImplementInteractionMode.cpp @@ -10,7 +10,7 @@ #include #include -#include +#include using namespace yarp::dev; #define JOINTIDCHECK if (j >= castToMapper(helper)->axes()){yError("joint id out of bound"); return false;} diff --git a/src/libYARP_dev/src/IMotorImpl.cpp b/src/libYARP_dev/src/ImplementMotor.cpp similarity index 100% rename from src/libYARP_dev/src/IMotorImpl.cpp rename to src/libYARP_dev/src/ImplementMotor.cpp diff --git a/src/libYARP_dev/src/IMotorEncodersImpl.cpp b/src/libYARP_dev/src/ImplementMotorEncoders.cpp similarity index 100% rename from src/libYARP_dev/src/IMotorEncodersImpl.cpp rename to src/libYARP_dev/src/ImplementMotorEncoders.cpp diff --git a/src/libYARP_dev/src/PWMControlImpl.cpp b/src/libYARP_dev/src/ImplementPWMControl.cpp similarity index 100% rename from src/libYARP_dev/src/PWMControlImpl.cpp rename to src/libYARP_dev/src/ImplementPWMControl.cpp diff --git a/src/libYARP_dev/src/IPidControlImpl.cpp b/src/libYARP_dev/src/ImplementPidControl.cpp similarity index 99% rename from src/libYARP_dev/src/IPidControlImpl.cpp rename to src/libYARP_dev/src/ImplementPidControl.cpp index 6e661aacf2..d1a008edc3 100644 --- a/src/libYARP_dev/src/IPidControlImpl.cpp +++ b/src/libYARP_dev/src/ImplementPidControl.cpp @@ -6,7 +6,7 @@ * BSD-3-Clause license. See the accompanying LICENSE file for details. */ -#include +#include #include #include #include diff --git a/src/libYARP_dev/src/IPositionControlImpl.cpp b/src/libYARP_dev/src/ImplementPositionControl.cpp similarity index 99% rename from src/libYARP_dev/src/IPositionControlImpl.cpp rename to src/libYARP_dev/src/ImplementPositionControl.cpp index bed758f9ca..9ca0c8b420 100644 --- a/src/libYARP_dev/src/IPositionControlImpl.cpp +++ b/src/libYARP_dev/src/ImplementPositionControl.cpp @@ -8,7 +8,7 @@ #include -#include +#include #include #include diff --git a/src/libYARP_dev/src/IPositionDirectImpl.cpp b/src/libYARP_dev/src/ImplementPositionDirect.cpp similarity index 100% rename from src/libYARP_dev/src/IPositionDirectImpl.cpp rename to src/libYARP_dev/src/ImplementPositionDirect.cpp diff --git a/src/libYARP_dev/src/RemoteCalibratorImpl.cpp b/src/libYARP_dev/src/ImplementRemoteCalibrator.cpp similarity index 100% rename from src/libYARP_dev/src/RemoteCalibratorImpl.cpp rename to src/libYARP_dev/src/ImplementRemoteCalibrator.cpp diff --git a/src/libYARP_dev/src/IRemoteVariablesImpl.cpp b/src/libYARP_dev/src/ImplementRemoteVariables.cpp similarity index 100% rename from src/libYARP_dev/src/IRemoteVariablesImpl.cpp rename to src/libYARP_dev/src/ImplementRemoteVariables.cpp diff --git a/src/libYARP_dev/src/TorqueControlImpl.cpp b/src/libYARP_dev/src/ImplementTorqueControl.cpp similarity index 100% rename from src/libYARP_dev/src/TorqueControlImpl.cpp rename to src/libYARP_dev/src/ImplementTorqueControl.cpp diff --git a/src/libYARP_dev/src/IVelocityControlImpl.cpp b/src/libYARP_dev/src/ImplementVelocityControl.cpp similarity index 99% rename from src/libYARP_dev/src/IVelocityControlImpl.cpp rename to src/libYARP_dev/src/ImplementVelocityControl.cpp index e745b79bac..5864b8cfd1 100644 --- a/src/libYARP_dev/src/IVelocityControlImpl.cpp +++ b/src/libYARP_dev/src/ImplementVelocityControl.cpp @@ -8,7 +8,7 @@ #include -#include +#include #include #include diff --git a/src/libYARP_dev/src/VirtualAnalogSensorImpl.cpp b/src/libYARP_dev/src/ImplementVirtualAnalogSensor.cpp similarity index 97% rename from src/libYARP_dev/src/VirtualAnalogSensorImpl.cpp rename to src/libYARP_dev/src/ImplementVirtualAnalogSensor.cpp index 692f776307..6e1ce6f1af 100644 --- a/src/libYARP_dev/src/VirtualAnalogSensorImpl.cpp +++ b/src/libYARP_dev/src/ImplementVirtualAnalogSensor.cpp @@ -6,7 +6,7 @@ * BSD-3-Clause license. See the accompanying LICENSE file for details. */ -#include "yarp/dev/IVirtualAnalogSensorImpl.h" +#include "yarp/dev/ImplementVirtualAnalogSensor.h" #include #include "yarp/sig/Vector.h" #include diff --git a/src/libYARP_dev/src/devices/ControlBoardRemapper/ControlBoardRemapper.cpp b/src/libYARP_dev/src/devices/ControlBoardRemapper/ControlBoardRemapper.cpp index 66ddf79f20..f874df89dc 100644 --- a/src/libYARP_dev/src/devices/ControlBoardRemapper/ControlBoardRemapper.cpp +++ b/src/libYARP_dev/src/devices/ControlBoardRemapper/ControlBoardRemapper.cpp @@ -3104,31 +3104,7 @@ bool ControlBoardRemapper::quitPark() /* IControlCalibration */ -#ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 -bool ControlBoardRemapper::calibrate(int j, double p) -{ - int off = (int) remappedControlBoards.lut[j].axisIndexInSubControlBoard; - size_t subIndex = remappedControlBoards.lut[j].subControlBoardIndex; - - yarp::dev::RemappedSubControlBoard *s = remappedControlBoards.getSubControlBoard(subIndex); - if (!s) - { - return false; - } - - if (s->calib) - { -YARP_WARNING_PUSH -YARP_DISABLE_DEPRECATED_WARNING - return s->calib->calibrate(off, p); -YARP_WARNING_POP - } - - return false; -} -#endif - -bool ControlBoardRemapper::calibrate(int j, unsigned int ui, double v1, double v2, double v3) +bool ControlBoardRemapper::calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3) { int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard; size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex; @@ -3137,7 +3113,7 @@ bool ControlBoardRemapper::calibrate(int j, unsigned int ui, double v1, double v if (p && p->calib) { - return p->calib->calibrate(off, ui,v1,v2,v3); + return p->calib->calibrateAxisWithParams(off, ui,v1,v2,v3); } return false; } @@ -3157,7 +3133,7 @@ bool ControlBoardRemapper::setCalibrationParameters(int j, const CalibrationPara return false; } -bool ControlBoardRemapper::done(int j) +bool ControlBoardRemapper::calibrationDone(int j) { int off=(int)remappedControlBoards.lut[j].axisIndexInSubControlBoard; size_t subIndex=remappedControlBoards.lut[j].subControlBoardIndex; @@ -3171,7 +3147,7 @@ bool ControlBoardRemapper::done(int j) if (p->calib) { - return p->calib->done(off); + return p->calib->calibrationDone(off); } return false; diff --git a/src/libYARP_dev/src/devices/ControlBoardRemapper/ControlBoardRemapper.h b/src/libYARP_dev/src/devices/ControlBoardRemapper/ControlBoardRemapper.h index 8c385ff393..254e373a08 100644 --- a/src/libYARP_dev/src/devices/ControlBoardRemapper/ControlBoardRemapper.h +++ b/src/libYARP_dev/src/devices/ControlBoardRemapper/ControlBoardRemapper.h @@ -443,16 +443,11 @@ class ControlBoardRemapper : public yarp::dev::DeviceDriver, /* IControlCalibration */ -#ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 - virtual bool calibrate(int j, double p) override; -#endif - - using yarp::dev::IControlCalibration::calibrate; - virtual bool calibrate(int j, unsigned int ui, double v1, double v2, double v3) override; + virtual bool calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3) override; virtual bool setCalibrationParameters(int j, const CalibrationParameters ¶ms) override; - virtual bool done(int j) override; + virtual bool calibrationDone(int j) override; virtual bool abortPark() override; diff --git a/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.cpp b/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.cpp index b57192e470..45ef743c33 100644 --- a/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.cpp +++ b/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.cpp @@ -3791,28 +3791,7 @@ bool ControlBoardWrapper::quitPark() /* IControlCalibration */ -#ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 -bool ControlBoardWrapper::calibrate(int j, double p) -{ - int off; try{off = device.lut.at(j).offset;} catch(...){yError() << "joint number " << j << " out of bound [0-"<< controlledJoints << "] for part " << partName; return false; } - int subIndex=device.lut[j].deviceEntry; - - yarp::dev::impl::SubDevice *s=device.getSubdevice(subIndex); - if (!s) - return false; - - if (s->calib) - { -YARP_WARNING_PUSH -YARP_DISABLE_DEPRECATED_WARNING - return s->calib->calibrate(off+s->base, p); -YARP_WARNING_POP - } - return false; -} -#endif - -bool ControlBoardWrapper::calibrate(int j, unsigned int ui, double v1, double v2, double v3) +bool ControlBoardWrapper::calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3) { int off; try{off = device.lut.at(j).offset;} catch(...){yError() << "joint number " << j << " out of bound [0-"<< controlledJoints << "] for part " << partName; return false; } int subIndex=device.lut[j].deviceEntry; @@ -3820,7 +3799,7 @@ bool ControlBoardWrapper::calibrate(int j, unsigned int ui, double v1, double v2 yarp::dev::impl::SubDevice *p = device.getSubdevice(subIndex); if (p && p->calib) { - return p->calib->calibrate(off+p->base, ui,v1,v2,v3); + return p->calib->calibrateAxisWithParams(off+p->base, ui,v1,v2,v3); } return false; } @@ -3838,7 +3817,7 @@ bool ControlBoardWrapper::setCalibrationParameters(int j, const CalibrationParam return false; } -bool ControlBoardWrapper::done(int j) +bool ControlBoardWrapper::calibrationDone(int j) { int off; try{off = device.lut.at(j).offset;} catch(...){yError() << "joint number " << j << " out of bound [0-"<< controlledJoints << "] for part " << partName; return false; } int subIndex=device.lut[j].deviceEntry; @@ -3849,7 +3828,7 @@ bool ControlBoardWrapper::done(int j) if (p->calib) { - return p->calib->done(off+p->base); + return p->calib->calibrationDone(off+p->base); } return false; } diff --git a/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.h b/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.h index 330f790541..20c67d58d6 100644 --- a/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.h +++ b/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.h @@ -345,8 +345,8 @@ class yarp::dev::ControlBoardWrapper: public yarp::dev::DeviceDriver, inline void printError(std::string func_name, std::string info, bool result) { //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) - yError() << "CBW(" << partName << "): " << func_name.c_str() << " on device" << info.c_str() << " returns false"; + // if(false == result) + // yError() << "CBW(" << partName << "): " << func_name.c_str() << " on device" << info.c_str() << " returns false"; //Commented in order to maintain the old behaviour (none message appear if device desn't implement the interface) //else // yError() << "CBW(" << partName << "): " << func_name.c_str() << " on device" << info.c_str() << ": the interface is not available."; @@ -1090,19 +1090,7 @@ class yarp::dev::ControlBoardWrapper: public yarp::dev::DeviceDriver, virtual bool quitPark() override; /* IControlCalibration */ - using yarp::dev::IControlCalibration::calibrate; -#ifndef YARP_NO_DEPRECATED // Since YARP 3.0.0 - /** - * Calibrate a single joint, the calibration method accepts a parameter - * that is used to accomplish various things internally and is implementation - * dependent. - * @param j the axis number. - * @param p is a double value that is passed to the calibration procedure. - * @return true/false on success/failure. - */ - virtual bool calibrate(int j, double p) override; -#endif - virtual bool calibrate(int j, unsigned int ui, double v1, double v2, double v3) override; + virtual bool calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3) override; virtual bool setCalibrationParameters(int j, const CalibrationParameters& params) override; @@ -1111,7 +1099,7 @@ class yarp::dev::ControlBoardWrapper: public yarp::dev::DeviceDriver, * @param j is the joint that has started a calibration procedure. * @return true/false on success/failure. */ - virtual bool done(int j) override; + virtual bool calibrationDone(int j) override; virtual bool abortPark() override; diff --git a/src/libYARP_dev/src/devices/ControlBoardWrapper/RPCMessagesParser.cpp b/src/libYARP_dev/src/devices/ControlBoardWrapper/RPCMessagesParser.cpp index b121051f10..02d084323b 100644 --- a/src/libYARP_dev/src/devices/ControlBoardWrapper/RPCMessagesParser.cpp +++ b/src/libYARP_dev/src/devices/ControlBoardWrapper/RPCMessagesParser.cpp @@ -1688,7 +1688,7 @@ bool RPCMessagesParser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& r if (rpc_Icalib==nullptr) yError("Sorry I don't have a IControlCalibration2 interface\n"); else - ok=rpc_Icalib->calibrate(j,ui,v1,v2,v3); + ok=rpc_Icalib->calibrateAxisWithParams(j,ui,v1,v2,v3); } break; @@ -1717,7 +1717,7 @@ bool RPCMessagesParser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& r rec=true; if (ControlBoardWrapper_p->verbose()) yDebug("Calling calibrate\n"); - ok=rpc_Icalib->calibrate(); + ok=rpc_Icalib->calibrateRobot(); } break; @@ -1727,7 +1727,7 @@ bool RPCMessagesParser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& r if (ControlBoardWrapper_p->verbose()) yDebug("Calling calibrate done\n"); int j=cmd.get(1).asInt32(); - ok=rpc_Icalib->done(j); + ok=rpc_Icalib->calibrationDone(j); } break; diff --git a/src/libYARP_dev/src/devices/RGBDSensorClient/RGBDSensorClient.cpp b/src/libYARP_dev/src/devices/RGBDSensorClient/RGBDSensorClient.cpp index 213f2799d8..84bbd0f32b 100644 --- a/src/libYARP_dev/src/devices/RGBDSensorClient/RGBDSensorClient.cpp +++ b/src/libYARP_dev/src/devices/RGBDSensorClient/RGBDSensorClient.cpp @@ -10,6 +10,7 @@ #include "RGBDSensorClient_StreamingMsgParser.h" #include #include +#include using namespace yarp::os; using namespace yarp::sig; diff --git a/src/libYARP_dev/src/devices/RGBDSensorWrapper/RGBDSensorWrapper.cpp b/src/libYARP_dev/src/devices/RGBDSensorWrapper/RGBDSensorWrapper.cpp index 339be6ad5e..0fece54921 100644 --- a/src/libYARP_dev/src/devices/RGBDSensorWrapper/RGBDSensorWrapper.cpp +++ b/src/libYARP_dev/src/devices/RGBDSensorWrapper/RGBDSensorWrapper.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include "rosPixelCode.h" diff --git a/src/libYARP_dev/src/devices/RGBDSensorWrapper/RGBDSensorWrapper.h b/src/libYARP_dev/src/devices/RGBDSensorWrapper/RGBDSensorWrapper.h index 06515de313..4341805dc2 100644 --- a/src/libYARP_dev/src/devices/RGBDSensorWrapper/RGBDSensorWrapper.h +++ b/src/libYARP_dev/src/devices/RGBDSensorWrapper/RGBDSensorWrapper.h @@ -61,7 +61,7 @@ namespace yarp{ // Following three definitions would fit better in a header file // shared between client and server ... where to place it? -#define VOCAB_PROTOCOL_VERSION VOCAB('p', 'r', 'o', 't') +constexpr yarp::conf::vocab32_t VOCAB_PROTOCOL_VERSION = yarp::os::createVocab('p', 'r', 'o', 't'); #define RGBD_WRAPPER_PROTOCOL_VERSION_MAJOR 1 #define RGBD_WRAPPER_PROTOCOL_VERSION_MINOR 0 diff --git a/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp b/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp index 5eae5f8229..8901e0d3a6 100644 --- a/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp +++ b/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp @@ -2453,7 +2453,7 @@ class yarp::dev::RemoteControlBoard : } /* IControlCalibration */ - bool virtual calibrate() override + bool virtual calibrateRobot() override { return send1V(VOCAB_CALIBRATE); } bool virtual abortCalibration() override @@ -2465,8 +2465,7 @@ class yarp::dev::RemoteControlBoard : bool virtual park(bool wait=true) override { return send1V(VOCAB_PARK); } - using yarp::dev::IControlCalibration::calibrate; - bool virtual calibrate(int j, unsigned int ui, double v1, double v2, double v3) override + bool virtual calibrateAxisWithParams(int j, unsigned int ui, double v1, double v2, double v3) override { Bottle cmd, response; @@ -2505,7 +2504,7 @@ class yarp::dev::RemoteControlBoard : return false; } - bool virtual done(int j) override + bool virtual calibrationDone(int j) override { return send1V1I(VOCAB_CALIBRATE_DONE, j); } bool getRefTorque(int j, double *t) override diff --git a/src/libYARP_dev/src/devices/RemoteFrameGrabber/RemoteFrameGrabber.h b/src/libYARP_dev/src/devices/RemoteFrameGrabber/RemoteFrameGrabber.h index 121ddc6ee7..7af5eb096f 100644 --- a/src/libYARP_dev/src/devices/RemoteFrameGrabber/RemoteFrameGrabber.h +++ b/src/libYARP_dev/src/devices/RemoteFrameGrabber/RemoteFrameGrabber.h @@ -17,6 +17,7 @@ #include #include #include +#include namespace yarp{ namespace dev { diff --git a/src/libYARP_dev/src/devices/ServerFrameGrabber/ServerFrameGrabber.cpp b/src/libYARP_dev/src/devices/ServerFrameGrabber/ServerFrameGrabber.cpp index e3a4c12deb..df75d47512 100644 --- a/src/libYARP_dev/src/devices/ServerFrameGrabber/ServerFrameGrabber.cpp +++ b/src/libYARP_dev/src/devices/ServerFrameGrabber/ServerFrameGrabber.cpp @@ -181,18 +181,6 @@ bool ServerFrameGrabber::open(yarp::os::Searchable& config) { singleThreaded); active = true; -/* -#define VOCAB_BRIGHTNESS VOCAB3('b','r','i') -#define VOCAB_EXPOSURE VOCAB4('e','x','p','o') -#define VOCAB_SHARPNESS VOCAB4('s','h','a','r') -#define VOCAB_WHITE VOCAB4('w','h','i','t') -#define VOCAB_HUE VOCAB3('h','u','e') -#define VOCAB_SATURATION VOCAB4('s','a','t','u') -#define VOCAB_GAMMA VOCAB4('g','a','m','m') -#define VOCAB_SHUTTER VOCAB4('s','h','u','t') -#define VOCAB_GAIN VOCAB4('g','a','i','n') -#define VOCAB_IRIS VOCAB4('i','r','i','s') -*/ DeviceResponder::makeUsage(); addUsage("[set] [bri] $fBrightness", "set brightness"); diff --git a/src/libYARP_dev/src/devices/ServerGrabber/ServerGrabber.cpp b/src/libYARP_dev/src/devices/ServerGrabber/ServerGrabber.cpp index f9b668f1b8..cc156492cd 100644 --- a/src/libYARP_dev/src/devices/ServerGrabber/ServerGrabber.cpp +++ b/src/libYARP_dev/src/devices/ServerGrabber/ServerGrabber.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include diff --git a/src/libYARP_dev/src/devices/msgs/yarp/src/jointData.cpp b/src/libYARP_dev/src/devices/msgs/yarp/src/jointData.cpp index 7208c0ad99..c73b91af89 100644 --- a/src/libYARP_dev/src/devices/msgs/yarp/src/jointData.cpp +++ b/src/libYARP_dev/src/devices/msgs/yarp/src/jointData.cpp @@ -950,7 +950,7 @@ bool jointData::Editor::read(yarp::os::ConnectionReader& connection) { yarp::os::idl::WireWriter writer(reader); if (writer.isNull()) return true; writer.writeListHeader(1); - writer.writeVocab(VOCAB2('o','k')); + writer.writeVocab(yarp::os::createVocab('o','k')); return true; } diff --git a/src/libYARP_serversql/include/yarp/serversql/impl/NameServiceOnTriples.h b/src/libYARP_serversql/include/yarp/serversql/impl/NameServiceOnTriples.h index 7d88967fac..7b8869eeef 100644 --- a/src/libYARP_serversql/include/yarp/serversql/impl/NameServiceOnTriples.h +++ b/src/libYARP_serversql/include/yarp/serversql/impl/NameServiceOnTriples.h @@ -123,6 +123,8 @@ class NameServiceOnTriples : public yarp::name::NameService bool cmdList(NameTripleState& act); + bool cmdListRunners(NameTripleState& act); + bool cmdSet(NameTripleState& act); bool cmdGet(NameTripleState& act); diff --git a/src/libYARP_serversql/include/yarp/serversql/impl/Subscriber.h b/src/libYARP_serversql/include/yarp/serversql/impl/Subscriber.h index a01f051194..77fd526562 100644 --- a/src/libYARP_serversql/include/yarp/serversql/impl/Subscriber.h +++ b/src/libYARP_serversql/include/yarp/serversql/impl/Subscriber.h @@ -106,7 +106,7 @@ class Subscriber : public yarp::name::NameService { dest, mode); reply.clear(); - reply.addVocab(ok?VOCAB2('o','k'):VOCAB4('f','a','i','l')); + reply.addVocab(ok?yarp::os::createVocab('o','k'):yarp::os::createVocab('f','a','i','l')); return ok; } else { // list subscriptions @@ -118,7 +118,7 @@ class Subscriber : public yarp::name::NameService { ok = removeSubscription(cmd.get(1).asString().c_str(), cmd.get(2).asString().c_str()); reply.clear(); - reply.addVocab(ok?VOCAB2('o','k'):VOCAB4('f','a','i','l')); + reply.addVocab(ok?yarp::os::createVocab('o','k'):yarp::os::createVocab('f','a','i','l')); return ok; } if (tag=="announce") { @@ -128,7 +128,7 @@ class Subscriber : public yarp::name::NameService { welcome(cmd.get(1).asString().c_str(),true); } reply.clear(); - reply.addVocab(VOCAB2('o','k')); + reply.addVocab(yarp::os::createVocab('o','k')); return true; } if (tag=="topic") { @@ -185,7 +185,7 @@ class Subscriber : public yarp::name::NameService { } int replyCode(bool flag) { - return flag?VOCAB2('o','k'):VOCAB4('f','a','i','l'); + return flag?yarp::os::createVocab('o','k'):yarp::os::createVocab('f','a','i','l'); } void setDelegate(yarp::os::NameSpace *delegate) { diff --git a/src/libYARP_serversql/src/NameServiceOnTriples.cpp b/src/libYARP_serversql/src/NameServiceOnTriples.cpp index 5fd2fcd8c9..ff999f1a10 100644 --- a/src/libYARP_serversql/src/NameServiceOnTriples.cpp +++ b/src/libYARP_serversql/src/NameServiceOnTriples.cpp @@ -374,6 +374,54 @@ bool NameServiceOnTriples::cmdUnregister(NameTripleState& act) { return cmdQuery(act); } +bool NameServiceOnTriples::cmdListRunners(NameTripleState& act) +{ // this is a combination of cmdList and cmdCheck codes + if (!act.bottleMode) + { + act.reply.addString("old"); + } else + { + act.reply.addString("ports"); + } + lock(); + Triple t; + t.setNameValue("port","*"); + + // obtain all ports names + list lst = act.mem.query(t, nullptr); + act.nestedMode = true; + + for (auto& it : lst) + { // check yarprun property for each port + std::string port = it.value.c_str(); + act.mem.reset(); + + Triple t; + t.setNameValue("port",port.c_str()); + int rid = act.mem.find(t, nullptr); + if (rid == -1) { + unlock(); + return false; + } + + // find all triples with yarprun = true for the specified RID (at most one) + TripleContext context; + context.setRid(rid); + t.setNameValue("yarprun","true"); + list lst = act.mem.query(t,&context); + + if (!lst.empty()) + { // if the port is a runner, do a classic query to build the reply with complete information about the port + act.cmd.clear(); + act.cmd.addString("query"); + act.cmd.addString(port); + act.mem.reset(); + cmdQuery(act, true); + } + } + unlock(); + return true; +} bool NameServiceOnTriples::cmdList(NameTripleState& act) { if (!act.bottleMode) { @@ -390,15 +438,15 @@ bool NameServiceOnTriples::cmdList(NameTripleState& act) { } list lst = act.mem.query(t, nullptr); act.nestedMode = true; - for (list::iterator it=lst.begin(); it!=lst.end(); it++) { + for (auto& it : lst) { if (prefix=="") { act.cmd.clear(); act.cmd.addString("query"); - act.cmd.addString(it->value.c_str()); + act.cmd.addString(it.value.c_str()); act.mem.reset(); cmdQuery(act,true); } else { - std::string iname = it->value.c_str(); + std::string iname = it.value.c_str(); if (iname.find(prefix)==0) { if (iname==prefix || iname[prefix.length()]=='/' || prefix[prefix.length()-1]=='/') { @@ -564,7 +612,7 @@ bool NameServiceOnTriples::cmdHelp(NameTripleState& act) { bot.addString("Here are some ways to use the name server:"); } bot.addString("+ help"); - bot.addString("+ list"); + bot.addString("+ list [$prefix]"); bot.addString("+ register $portname"); bot.addString("+ register $portname $carrier $ipAddress $portNumber"); bot.addString(" (if you want a field set automatically, write '...')"); @@ -573,8 +621,9 @@ bool NameServiceOnTriples::cmdHelp(NameTripleState& act) { bot.addString("+ set $portname $property $value"); bot.addString("+ get $portname $property"); bot.addString("+ check $portname $property"); - bot.addString("+ match $portname $property $prefix"); bot.addString("+ route $port1 $port2"); + bot.addString("+ runners"); + bot.addString(" (to get a list of the yarprun ports)"); return true; } @@ -632,6 +681,8 @@ bool NameServiceOnTriples::apply(yarp::os::Bottle& cmd, return cmdQuery(act); } else if (key=="list") { return cmdList(act); + } else if (key=="runners") { + return cmdListRunners(act); } else if (key=="set") { return cmdSet(act); } else if (key=="get") { diff --git a/src/libYARP_sig/include/yarp/sig/Image.h b/src/libYARP_sig/include/yarp/sig/Image.h index c0c2f74c80..3d229537de 100644 --- a/src/libYARP_sig/include/yarp/sig/Image.h +++ b/src/libYARP_sig/include/yarp/sig/Image.h @@ -45,32 +45,32 @@ namespace yarp { enum YarpVocabPixelTypesEnum { VOCAB_PIXEL_INVALID = 0, - VOCAB_PIXEL_MONO = VOCAB4('m','o','n','o'), - VOCAB_PIXEL_MONO16 = VOCAB4('m','o','1','6'), - VOCAB_PIXEL_RGB = VOCAB3('r','g','b'), - VOCAB_PIXEL_RGBA = VOCAB4('r','g','b','a'), - VOCAB_PIXEL_BGRA = VOCAB4('b','g','r','a'), - VOCAB_PIXEL_INT = VOCAB3('i','n','t'), - VOCAB_PIXEL_HSV = VOCAB3('h','s','v'), - VOCAB_PIXEL_BGR = VOCAB3('b','g','r'), - VOCAB_PIXEL_MONO_SIGNED = VOCAB4('s','i','g','n'), - VOCAB_PIXEL_RGB_SIGNED = VOCAB4('r','g','b','-'), - VOCAB_PIXEL_RGB_INT = VOCAB4('r','g','b','i'), - VOCAB_PIXEL_MONO_FLOAT = VOCAB3('d','e','c'), - VOCAB_PIXEL_RGB_FLOAT = VOCAB4('r','g','b','.'), - VOCAB_PIXEL_HSV_FLOAT = VOCAB4('h','s','v','.'), - VOCAB_PIXEL_ENCODING_BAYER_GRBG8 = VOCAB4('g', 'r', 'b', 'g'), //grbg8 - VOCAB_PIXEL_ENCODING_BAYER_GRBG16 = VOCAB4('g', 'r', '1', '6'), //grbg16 - VOCAB_PIXEL_ENCODING_BAYER_BGGR8 = VOCAB4('b', 'g', 'g', 'r'), //bggr8 - VOCAB_PIXEL_ENCODING_BAYER_BGGR16 = VOCAB4('b', 'g', '1', '6'), //bggr16 - VOCAB_PIXEL_ENCODING_BAYER_GBRG8 = VOCAB4('g', 'b', 'r', 'g'), //gbrg8 - VOCAB_PIXEL_ENCODING_BAYER_GBRG16 = VOCAB4('g', 'b', '1', '6'), //gbrg16 - VOCAB_PIXEL_ENCODING_BAYER_RGGB8 = VOCAB4('r', 'g', 'g', 'b'), //rggb8 - VOCAB_PIXEL_ENCODING_BAYER_RGGB16 = VOCAB4('r', 'g', '1', '6'), //rggb16 - VOCAB_PIXEL_YUV_420 = VOCAB4('y','u','v','a'), - VOCAB_PIXEL_YUV_444 = VOCAB4('y','u','v','b'), - VOCAB_PIXEL_YUV_422 = VOCAB4('y','u','v','c'), - VOCAB_PIXEL_YUV_411 = VOCAB4('y','u','v','d') + VOCAB_PIXEL_MONO = yarp::os::createVocab('m','o','n','o'), + VOCAB_PIXEL_MONO16 = yarp::os::createVocab('m','o','1','6'), + VOCAB_PIXEL_RGB = yarp::os::createVocab('r','g','b'), + VOCAB_PIXEL_RGBA = yarp::os::createVocab('r','g','b','a'), + VOCAB_PIXEL_BGRA = yarp::os::createVocab('b','g','r','a'), + VOCAB_PIXEL_INT = yarp::os::createVocab('i','n','t'), + VOCAB_PIXEL_HSV = yarp::os::createVocab('h','s','v'), + VOCAB_PIXEL_BGR = yarp::os::createVocab('b','g','r'), + VOCAB_PIXEL_MONO_SIGNED = yarp::os::createVocab('s','i','g','n'), + VOCAB_PIXEL_RGB_SIGNED = yarp::os::createVocab('r','g','b','-'), + VOCAB_PIXEL_RGB_INT = yarp::os::createVocab('r','g','b','i'), + VOCAB_PIXEL_MONO_FLOAT = yarp::os::createVocab('d','e','c'), + VOCAB_PIXEL_RGB_FLOAT = yarp::os::createVocab('r','g','b','.'), + VOCAB_PIXEL_HSV_FLOAT = yarp::os::createVocab('h','s','v','.'), + VOCAB_PIXEL_ENCODING_BAYER_GRBG8 = yarp::os::createVocab('g', 'r', 'b', 'g'), //grbg8 + VOCAB_PIXEL_ENCODING_BAYER_GRBG16 = yarp::os::createVocab('g', 'r', '1', '6'), //grbg16 + VOCAB_PIXEL_ENCODING_BAYER_BGGR8 = yarp::os::createVocab('b', 'g', 'g', 'r'), //bggr8 + VOCAB_PIXEL_ENCODING_BAYER_BGGR16 = yarp::os::createVocab('b', 'g', '1', '6'), //bggr16 + VOCAB_PIXEL_ENCODING_BAYER_GBRG8 = yarp::os::createVocab('g', 'b', 'r', 'g'), //gbrg8 + VOCAB_PIXEL_ENCODING_BAYER_GBRG16 = yarp::os::createVocab('g', 'b', '1', '6'), //gbrg16 + VOCAB_PIXEL_ENCODING_BAYER_RGGB8 = yarp::os::createVocab('r', 'g', 'g', 'b'), //rggb8 + VOCAB_PIXEL_ENCODING_BAYER_RGGB16 = yarp::os::createVocab('r', 'g', '1', '6'), //rggb16 + VOCAB_PIXEL_YUV_420 = yarp::os::createVocab('y','u','v','a'), + VOCAB_PIXEL_YUV_444 = yarp::os::createVocab('y','u','v','b'), + VOCAB_PIXEL_YUV_422 = yarp::os::createVocab('y','u','v','c'), + VOCAB_PIXEL_YUV_411 = yarp::os::createVocab('y','u','v','d') }; /** diff --git a/src/libYARP_sig/include/yarp/sig/ImageNetworkHeader.h b/src/libYARP_sig/include/yarp/sig/ImageNetworkHeader.h index 057d0dded6..fcf75a38f9 100644 --- a/src/libYARP_sig/include/yarp/sig/ImageNetworkHeader.h +++ b/src/libYARP_sig/include/yarp/sig/ImageNetworkHeader.h @@ -59,7 +59,7 @@ class yarp::sig::ImageNetworkHeader listTag = BOTTLE_TAG_LIST; listLen = 4; paramNameTag = BOTTLE_TAG_VOCAB; - paramName = VOCAB3('m','a','t'); + paramName = yarp::os::createVocab('m','a','t'); paramIdTag = BOTTLE_TAG_VOCAB; id = image.getPixelCode(); paramListTag = BOTTLE_TAG_LIST + BOTTLE_TAG_INT32; diff --git a/src/libYARP_sig/include/yarp/sig/Vector.h b/src/libYARP_sig/include/yarp/sig/Vector.h index a3bba597db..8571ae7bd6 100644 --- a/src/libYARP_sig/include/yarp/sig/Vector.h +++ b/src/libYARP_sig/include/yarp/sig/Vector.h @@ -10,7 +10,7 @@ #ifndef YARP_SIG_VECTOR_H #define YARP_SIG_VECTOR_H -//#include //defines size_t +#include #include //defines size_t #include #include @@ -26,8 +26,15 @@ namespace yarp { namespace sig { class VectorBase; - class Vector; template class VectorOf; + // Swig(3.0.12) crashes when generating + // ruby bindings without these guards. + // Bindings for Vector are generated + // anyways throught the %template directive + // in the interface file. +#ifndef SWIG + typedef VectorOf Vector; +#endif } } @@ -61,6 +68,10 @@ class YARP_sig_API yarp::sig::VectorBase : public yarp::os::Portable * return true iff a vector was written correctly */ virtual bool write(yarp::os::ConnectionWriter& connection) const override; + +protected: + virtual std::string getFormatStr(int tag) const; + }; /* @@ -131,14 +142,51 @@ class yarp::sig::VectorOf : public VectorBase _updatePointers(); } + /** + * Build a vector and initialize it with def. + * @param s the size + * @param def a default value used to fill the vector + */ + VectorOf(size_t s, const T&def) + { + this->resize(s,def); + } + + /** + * Builds a vector and initialize it with + * values from 'p'. Copies memory. + * @param s the size of the data to be copied + * @param T* the pointer to the data + */ + VectorOf(size_t s, const T *p) + { + len = 0; + this->resize(s); + + memcpy(this->data(), p, sizeof(T)*s); + } + VectorOf(const VectorOf &r) : VectorBase() { bytes = r.bytes; _updatePointers(); } + /** + * Copy operator; + */ const VectorOf &operator=(const VectorOf &r) { - bytes = r.bytes; - _updatePointers(); + + if (this == &r) return *this; + + if(this->size() == r.size()) + { + memcpy(this->data(), r.data(), sizeof(T)*r.size()); + } + else + { + bytes = r.bytes; + _updatePointers(); + } return *this; } @@ -175,6 +223,25 @@ class yarp::sig::VectorOf : public VectorBase return first; } + /** + * Return a pointer to the first element of the vector. + * @return a pointer to double (or nullptr if the vector is of zero length) + */ + inline T *data() + { return first; } + + /** + * Return a pointer to the first element of the vector, + * const version + * @return a (const) pointer to double (or nullptr if the vector is of zero length) + */ + inline const T *data() const + { return first;} + + /** + * Resize the vector. + * @param s the new size + */ virtual void resize(size_t size) override { size_t prev_len = len; @@ -186,6 +253,11 @@ class yarp::sig::VectorOf : public VectorBase } } + /** + * Resize the vector and initilize the element to a default value. + * @param s the new size + * @param def the default value + */ void resize(size_t size, const T&def) { bytes.allocateOnNeed(size*sizeof(T),size*sizeof(T)); @@ -196,6 +268,9 @@ class yarp::sig::VectorOf : public VectorBase } } + /** + * Push a new element in the vector: size is changed + */ inline void push_back (const T &elem) { bytes.allocateOnNeed(bytes.used()+sizeof(T),bytes.length()*2+sizeof(T)); @@ -204,6 +279,9 @@ class yarp::sig::VectorOf : public VectorBase first[len-1] = elem; } + /** + * Pop an element out of the vector: size is changed + */ inline void pop_back (void) { if (bytes.used()>sizeof(T)) { @@ -257,100 +335,20 @@ class yarp::sig::VectorOf : public VectorBase return len; } - void clear() { - bytes.clear(); - bytes.setUsed(0); - len = 0; - first = nullptr; - } -}; - - -#ifdef _MSC_VER -/*YARP_sig_EXTERN*/ template class YARP_sig_API yarp::sig::VectorOf; -#endif - -/** -* \ingroup sig_class -* -* A class for a Vector. A Vector can be sent/read to/from -* a port. Use the [] and () operator for single element -* access. -*/ -class YARP_sig_API yarp::sig::Vector : public yarp::os::Portable -{ - VectorOf storage; - -public: - Vector() - {} - - explicit Vector(size_t s):storage(s) - {} - - /** - * Build a vector and initialize it with def. - * @param s the size - * @param def a default value used to fill the vector - */ - explicit Vector(size_t s, const double &def) - { - storage.resize(s,def); - } - - ~Vector() - {} - - /** - * Builds a vector and initialize it with - * values from 'p'. Copies memory. - */ - Vector(size_t s, const double *p); - - /** - * Copy constructor. - */ - Vector(const Vector &r): yarp::os::Portable(), storage(r.storage) - {} - - /** - * Copy operator; - */ - const Vector &operator=(const Vector &r); - - /** - * Resize the vector. - * @param s the new size - */ - void resize(size_t s) - { - storage.resize(s); - } - - /** - * Resize the vector, if the vector is not empty preserve old content. - * @param size the new size - * @param def a default value used to fill the vector - */ - void resize(size_t size, const double &def) - { - storage.resize(size, def); - } - - inline size_t size() const - { return storage.size();} - /** * Get the length of the vector. * @return the length of the vector. */ inline size_t length() const - { return storage.size();} + { return this->size();} /** * Zero the elements of the vector. */ - void zero(); + void zero() + { + memset(this->data(), 0, sizeof(T)*this->size()); + } /** * Creates a string object containing a text representation of the object. Useful for printing. @@ -361,8 +359,41 @@ class YARP_sig_API yarp::sig::Vector : public yarp::os::Portable * Warning: the string format might change in the future. This method * is here to ease debugging. */ - std::string toString(int precision=-1, int width=-1) const; + std::string toString(int precision=-1, int width=-1) const + { + std::string ret = ""; + size_t c = 0; + const size_t buffSize = 256; + char tmp[buffSize]; + std::string formatStr; + if (getBottleTag() == BOTTLE_TAG_FLOAT64) { + if (width<0) { + formatStr = "% .*lf\t"; + for (c=0;c=1) + return ret.substr(0, ret.length()-1); + return ret; + } /** * Creates and returns a new vector, being the portion of the original @@ -370,7 +401,17 @@ class YARP_sig_API yarp::sig::Vector : public yarp::os::Portable * in the subvector. The indexes are checked: if wrong, a null vector is * returned. */ - Vector subVector(unsigned int first, unsigned int last) const; + VectorOf subVector(unsigned int first, unsigned int last) const + { + VectorOf ret; + if ((first<=last)&&((int)last<(int)this->size())) + { + ret.resize(last-first+1); + for (unsigned int k=first; k<=last; k++) + ret[k-first]=(*this)[k]; + } + return ret; + } /** * Set a portion of this vector with the values of the specified vector. @@ -381,104 +422,66 @@ class YARP_sig_API yarp::sig::Vector : public yarp::os::Portable * @param v vector containing the values to set * @return true if the operation succeeded, false otherwise */ - bool setSubvector(int position, const Vector &v); - - /** - * Set all elements of the vector to a scalar. - */ - const Vector &operator=(double v); - - /** - * Return a pointer to the first element of the vector. - * @return a pointer to double (or nullptr if the vector is of zero length) - */ - inline double *data() - { return storage.getFirst(); } - - /** - * Return a pointer to the first element of the vector, - * const version - * @return a (const) pointer to double (or nullptr if the vector is of zero length) - */ - inline const double *data() const - { return storage.getFirst();} - - /** - * True iff all elements of 'a' match all element of 'b'. - */ - bool operator==(const yarp::sig::Vector &r) const; - - /** - * Push a new element in the vector: size is changed - */ - inline void push_back (const double &elem) + bool setSubvector(int position, const VectorOf &v) { - storage.push_back(elem); + if (position+v.size() > this->size()) + return false; + for (size_t i=0;i &operator=(T v) { - storage.pop_back(); - } + T *tmp = this->data(); - /** - * Single element access, no range check. - * @param i the index of the element to access. - * @return a reference to the requested element. - */ - inline double &operator[](size_t i) - {return storage[i];} - - /** - * Single element access, no range check, const version. - * @param i the index of the element to access. - * @return a reference to the requested element. - */ - inline const double &operator[](size_t i) const - {return storage[i];} + for(size_t k=0; k &r) const + { + //check dimensions first + size_t c=size(); + if (c!=r.size()) + return false; + + const T *tmp1=data(); + const T *tmp2=r.data(); + + while(c--) + { + if (*tmp1++!=*tmp2++) + return false; + } - /** - * Clears out the vector, it does not reallocate - * the buffer, but just sets the dynamic size to 0. - */ - void clear () - { storage.clear();} + return true; - ///////// Serialization methods - /* - * Read vector from a connection. - * return true iff a vector was read correctly - */ - virtual bool read(yarp::os::ConnectionReader& connection) override; + } - /** - * Write vector to a connection. - * return true iff a vector was written correctly - */ - virtual bool write(yarp::os::ConnectionWriter& connection) const override; + void clear() { + bytes.clear(); + bytes.setUsed(0); + len = 0; + first = nullptr; + } virtual yarp::os::Type getType() const override { return yarp::os::Type::byName("yarp/vector"); } }; + +#ifdef _MSC_VER +/*YARP_sig_EXTERN*/ template class YARP_sig_API yarp::sig::VectorOf; +#endif + #endif // YARP_SIG_VECTOR_H diff --git a/src/libYARP_sig/src/Image.cpp b/src/libYARP_sig/src/Image.cpp index 096993a573..add9d8c000 100644 --- a/src/libYARP_sig/src/Image.cpp +++ b/src/libYARP_sig/src/Image.cpp @@ -760,7 +760,7 @@ bool Image::write(yarp::os::ConnectionWriter& connection) const { header.listTag = BOTTLE_TAG_LIST; header.listLen = 4; header.paramNameTag = BOTTLE_TAG_VOCAB; - header.paramName = VOCAB3('m','a','t'); + header.paramName = yarp::os::createVocab('m','a','t'); header.paramIdTag = BOTTLE_TAG_VOCAB; header.id = getPixelCode(); header.paramListTag = BOTTLE_TAG_LIST + BOTTLE_TAG_INT32; diff --git a/src/libYARP_sig/src/SoundFile.cpp b/src/libYARP_sig/src/SoundFile.cpp index 6978d113f9..baf9a4ccc1 100644 --- a/src/libYARP_sig/src/SoundFile.cpp +++ b/src/libYARP_sig/src/SoundFile.cpp @@ -85,7 +85,7 @@ bool PcmWavHeader::parse_from_file(FILE *fp) //extra chunks fread(&dummyHeader,sizeof(dummyHeader),1,fp); - while (dummyHeader!=VOCAB4('d','a','t','a')) + while (dummyHeader!=yarp::os::createVocab('d','a','t','a')) { fread(&dummyLength,sizeof(dummyLength),1,fp); dummyData.clear(); @@ -107,10 +107,10 @@ void PcmWavHeader::setup_to_write(const Sound& src, FILE *fp) int bytes = channels*src.getSamples()*2; int align = channels*((bitsPerSample+7)/8); - wavHeader = VOCAB4('R','I','F','F'); + wavHeader = yarp::os::createVocab('R','I','F','F'); wavLength = bytes + sizeof(PcmWavHeader) - 2*sizeof(NetInt32); - formatHeader1 = VOCAB4('W','A','V','E'); - formatHeader2 = VOCAB4('f','m','t',' '); + formatHeader1 = yarp::os::createVocab('W','A','V','E'); + formatHeader2 = yarp::os::createVocab('f','m','t',' '); formatLength = sizeof(pcm); pcm.pcmFormatTag = 1; /* PCM! */ @@ -120,7 +120,7 @@ void PcmWavHeader::setup_to_write(const Sound& src, FILE *fp) pcm.pcmBlockAlign = align; pcm.pcmBitsPerSample = bitsPerSample; - dataHeader = VOCAB4('d','a','t','a'); + dataHeader = yarp::os::createVocab('d','a','t','a'); dataLength = bytes; diff --git a/src/libYARP_sig/src/Vector.cpp b/src/libYARP_sig/src/Vector.cpp index 0f3725ca8a..539576fafa 100644 --- a/src/libYARP_sig/src/Vector.cpp +++ b/src/libYARP_sig/src/Vector.cpp @@ -22,9 +22,10 @@ #include #include +#include #include #include - +#include using namespace yarp::sig; using namespace yarp::os; @@ -41,6 +42,14 @@ class VectorPortContentHeader }; YARP_END_PACK +const std::map tag2FormatStr = { + {BOTTLE_TAG_INT32, PRId32}, + {BOTTLE_TAG_INT64, PRId64}, + {BOTTLE_TAG_VOCAB, "c"}, + {BOTTLE_TAG_STRING, "s"}, + {BOTTLE_TAG_FLOAT64, "lf"}, +}; + bool VectorBase::read(yarp::os::ConnectionReader& connection) { // auto-convert text mode interaction connection.convertTextMode(); @@ -85,146 +94,15 @@ bool VectorBase::write(yarp::os::ConnectionWriter& connection) const { return !connection.isError(); } - -/** -* Quick implementation, space for improvement. -*/ -std::string Vector::toString(int precision, int width) const +std::string VectorBase::getFormatStr(int tag) const { std::string ret = ""; - size_t c; - char tmp[350]; - if(width<0){ - for(c=0;c=1) - return ret.substr(0, ret.length()-1); - return ret; -} - -Vector Vector::subVector(unsigned int first, unsigned int last) const -{ - Vector ret; - if((first<=last)&&((int)last<(int)storage.size())) - { - ret.resize(last-first+1); - for(unsigned int k=first; k<=last; k++) - ret[k-first]=storage[k]; - } - return ret; -} - -bool Vector::setSubvector(int position, const Vector &v) -{ - if(position+v.size() > storage.size()) - return false; - for(size_t i=0;i 0 && - header.listTag == (BOTTLE_TAG_LIST | BOTTLE_TAG_FLOAT64)) { - if (size() != (size_t)(header.listLen)) - resize(header.listLen); - - int k=0; - for (k=0;ksecond; } - return !connection.isError(); -} - -bool Vector::write(yarp::os::ConnectionWriter& connection) const { - VectorPortContentHeader header; - - header.listTag = (BOTTLE_TAG_LIST | BOTTLE_TAG_FLOAT64); - header.listLen = (int)size(); - - connection.appendBlock((char*)&header, sizeof(header)); - - int k=0; - for (k=0;kcomboMode->setItemData( PWM, Pwm, Qt::UserRole); ui->comboMode->setItemData( CURRENT, Current, Qt::UserRole); - QString styleSheet = QString("%1 QComboBox:!editable, QComboBox::drop-down:editable {background-color: rgb(149,221,186);} %2").arg(comboStyle1).arg(comboStyle2); - ui->comboMode->setStyleSheet(styleSheet); + // QString styleSheet = QString("%1 QComboBox:!editable, QComboBox::drop-down:editable {background-color: rgb(149,221,186);} %2").arg(comboStyle1).arg(comboStyle2); + // ui->comboMode->setStyleSheet(styleSheet); setJointInternalState(IDLE); QVariant variant = ui->comboInteraction->itemData(0,Qt::BackgroundRole); - QColor c = variant.value(); +// QColor c = variant.value(); - styleSheet = QString("%1 QComboBox:!editable, QComboBox::drop-down:editable {background-color: rgb(%2,%3,%4);} %5").arg(comboStyle1).arg(c.red()).arg(c.green()).arg(c.blue()).arg(comboStyle2); - ui->comboInteraction->setStyleSheet(styleSheet); + // styleSheet = QString("%1 QComboBox:!editable, QComboBox::drop-down:editable {background-color: rgb(%2,%3,%4);} %5").arg(comboStyle1).arg(c.red()).arg(c.green()).arg(c.blue()).arg(comboStyle2); +// ui->comboInteraction->setStyleSheet(styleSheet); ui->stackedWidget->widget(VELOCITY)->setEnabled(false); @@ -515,9 +492,260 @@ void JointItem::setUnits(yarp::dev::JointTypeEnum t) void JointItem::setMotorPositionVisible(bool visible) { joint_motorPositionVisible = visible; - ui->editPositionMotorPosition->setVisible(visible); - ui->labelPositionMotorPosition->setVisible(visible); - ui->labelPositionMotorPositionUnits->setVisible(visible); + ui->editIdleMotorPos->setVisible(visible); + ui->editPositionMotorPos->setVisible(visible); + ui->editPositionDirMotorPos->setVisible(visible); + ui->editMixedMotorPos->setVisible(visible); + ui->editTorqueMotorPos->setVisible(visible); + ui->editPWMMotorPos->setVisible(visible); + ui->editCurrentMotorPos->setVisible(visible); + ui->editVelocityMotorPos->setVisible(visible); + + ui->labelIdleMotorPos->setVisible(visible); + ui->labelPositionMotorPos->setVisible(visible); + ui->labelPositionDirMotorPos->setVisible(visible); + ui->labelMixedMotorPos->setVisible(visible); + ui->labelTorqueMotorPos->setVisible(visible); + ui->labelPWMMotorPos->setVisible(visible); + ui->labelCurrentMotorPos->setVisible(visible); + ui->labelVelocityMotorPos->setVisible(visible); + + ui->labelIdleMotorPosUnits->setVisible(visible); + ui->labelPositionMotorPosUnits->setVisible(visible); + ui->labelPositionDirMotorPosUnits->setVisible(visible); + ui->labelMixedMotorPosUnits->setVisible(visible); + ui->labelTorqueMotorPosUnits->setVisible(visible); + ui->labelPWMMotorPosUnits->setVisible(visible); + ui->labelCurrentMotorPosUnits->setVisible(visible); + ui->labelVelocityMotorPosUnits->setVisible(visible); + + if (!visible) { + ui->editIdleMotorPos->setMinimumHeight(0); + ui->editPositionMotorPos->setMinimumHeight(0); + ui->editPositionDirMotorPos->setMinimumHeight(0); + ui->editMixedMotorPos->setMinimumHeight(0); + ui->editTorqueMotorPos->setMinimumHeight(0); + ui->editPWMMotorPos->setMinimumHeight(0); + ui->editCurrentMotorPos->setMinimumHeight(0); + ui->editVelocityMotorPos->setMinimumHeight(0); + + ui->labelPositionMotorPos->setMinimumHeight(0); + ui->labelPositionMotorPosUnits->setMinimumHeight(0); + ui->labelPositionDirMotorPos->setMinimumHeight(0); + ui->labelPositionDirMotorPosUnits->setMinimumHeight(0); + ui->labelMixedMotorPos->setMinimumHeight(0); + ui->labelMixedMotorPosUnits->setMinimumHeight(0); + ui->labelTorqueMotorPos->setMinimumHeight(0); + ui->labelTorqueMotorPosUnits->setMinimumHeight(0); + ui->labelPWMMotorPos->setMinimumHeight(0); + ui->labelPWMMotorPosUnits->setMinimumHeight(0); + ui->labelCurrentMotorPos->setMinimumHeight(0); + ui->labelCurrentMotorPosUnits->setMinimumHeight(0); + ui->labelVelocityMotorPos->setMinimumHeight(0); + ui->labelVelocityMotorPosUnits->setMinimumHeight(0); + ui->labelIdleMotorPos->setMinimumHeight(0); + ui->labelIdleMotorPosUnits->setMinimumHeight(0); + } + else { + ui->editIdleMotorPos->setMinimumHeight(20); + ui->editPositionMotorPos->setMinimumHeight(20); + ui->editPositionDirMotorPos->setMinimumHeight(20); + ui->editMixedMotorPos->setMinimumHeight(20); + ui->editTorqueMotorPos->setMinimumHeight(20); + ui->editPWMMotorPos->setMinimumHeight(20); + ui->editCurrentMotorPos->setMinimumHeight(20); + ui->editVelocityMotorPos->setMinimumHeight(20); + + ui->labelPositionMotorPos->setMinimumHeight(20); + ui->labelPositionMotorPosUnits->setMinimumHeight(20); + ui->labelPositionDirMotorPos->setMinimumHeight(20); + ui->labelPositionDirMotorPosUnits->setMinimumHeight(20); + ui->labelMixedMotorPos->setMinimumHeight(20); + ui->labelMixedMotorPosUnits->setMinimumHeight(20); + ui->labelTorqueMotorPos->setMinimumHeight(20); + ui->labelTorqueMotorPosUnits->setMinimumHeight(20); + ui->labelPWMMotorPos->setMinimumHeight(20); + ui->labelPWMMotorPosUnits->setMinimumHeight(20); + ui->labelCurrentMotorPos->setMinimumHeight(20); + ui->labelCurrentMotorPosUnits->setMinimumHeight(20); + ui->labelVelocityMotorPos->setMinimumHeight(20); + ui->labelVelocityMotorPosUnits->setMinimumHeight(20); + ui->labelIdleMotorPos->setMinimumHeight(20); + ui->labelIdleMotorPosUnits->setMinimumHeight(20); + } +} + +void JointItem::setDutyVisible(bool visible) +{ + joint_dutyVisible = visible; + //ui->editIdleDuty->setVisible(visible); + //ui->editPositionDuty->setVisible(visible); + //ui->editPositionDirDuty->setVisible(visible); + // ui->editMixedDuty->setVisible(visible); + ui->editTorqueDuty->setVisible(visible); + ui->editCurrentDuty->setVisible(visible); + //ui->editPWMDuty->setVisible(visible); + //ui->editDutyDuty->setVisible(visible); + //ui->editVelocityDuty->setVisible(visible); + + //ui->labelIdleDuty->setVisible(visible); + //ui->labelIdleDutyUnits->setVisible(visible); + //ui->labelPositionDuty->setVisible(visible); + //ui->labelPositionDutyUnits->setVisible(visible); + //ui->labelPositionDirDuty->setVisible(visible); + //ui->labelPositionDirDutyUnits->setVisible(visible); + //ui->labelMixedDuty->setVisible(visible); + //ui->labelMixedDutyUnits->setVisible(visible); + ui->labelTorqueDuty->setVisible(visible); + ui->labelTorqueDutyUnits->setVisible(visible); + //ui->labelPWMDuty->setVisible(visible); + ui->labelCurrentDuty->setVisible(visible); + //ui->labelPWMDutyUnits->setVisible(visible); + ui->labelCurrentDutyUnits->setVisible(visible); + //ui->labelVelocityDuty->setVisible(visible); + //ui->labelVelocityDutyUnits->setVisible(visible); + + + if (!visible) { + //ui->editIdleDuty->setMinimumHeight(0); + //ui->editPositionDuty->setMinimumHeight(0); + //ui->editPositionDirDuty->setMinimumHeight(0); + //ui->editMixedDuty->setMinimumHeight(0); + //ui->editTorqueDuty->setMinimumHeight(0); + //ui->editPWMDuty->setMinimumHeight(0); + //ui->editDutyDuty->setMinimumHeight(0); + //ui->editVelocityDuty->setMinimumHeight(0); + + //ui->labelPositionDuty->setMinimumHeight(0); + //ui->labelPositionDutyUnits->setMinimumHeight(0); + //ui->labelPositionDirDuty->setMinimumHeight(0); + //ui->labelPositionDirDutyUnits->setMinimumHeight(0); + //ui->labelMixedDuty->setMinimumHeight(0); + //ui->labelMixedDutyUnits->setMinimumHeight(0); + ui->labelTorqueDuty->setMinimumHeight(0); + ui->labelTorqueDutyUnits->setMinimumHeight(0); + //ui->labelPWMDuty->setMinimumHeight(0); + //ui->labelPWMDutyUnits->setMinimumHeight(0); + ui->labelCurrentDuty->setMinimumHeight(0); + ui->labelCurrentDutyUnits->setMinimumHeight(0); + //ui->labelVelocityDuty->setMinimumHeight(0); + //ui->labelVelocityDutyUnits->setMinimumHeight(0); + //ui->labelIdleDuty->setMinimumHeight(0); + //ui->labelIdleDutyUnits->setMinimumHeight(0); + } + else { + //ui->editIdleDuty->setMinimumHeight(20); + //ui->editPositionDuty->setMinimumHeight(20); + //ui->editPositionDirDuty->setMinimumHeight(20); + //ui->editMixedDuty->setMinimumHeight(20); + ui->editTorqueDuty->setMinimumHeight(20); + //ui->editPWMDuty->setMinimumHeight(20); + ui->editCurrentDuty->setMinimumHeight(20); + //ui->editVelocityDuty->setMinimumHeight(20); + + //ui->labelPositionDuty->setMinimumHeight(20); + //ui->labelPositionDutyUnits->setMinimumHeight(20); + //ui->labelPositionDirDuty->setMinimumHeight(20); + //ui->labelPositionDirDutyUnits->setMinimumHeight(20); + //ui->labelMixedDuty->setMinimumHeight(20); + //ui->labelMixedDutyUnits->setMinimumHeight(20); + ui->labelTorqueDuty->setMinimumHeight(20); + ui->labelTorqueDutyUnits->setMinimumHeight(20); + //ui->labelPWMDuty->setMinimumHeight(20); + //ui->labelPWMDutyUnits->setMinimumHeight(20); + ui->labelCurrentDuty->setMinimumHeight(20); + ui->labelCurrentDutyUnits->setMinimumHeight(20); + //ui->labelVelocityDuty->setMinimumHeight(20); + //ui->labelVelocityDutyUnits->setMinimumHeight(20); + //ui->labelIdleDuty->setMinimumHeight(20); + //ui->labelIdleDutyUnits->setMinimumHeight(20); + } +} + +void JointItem::setCurrentsVisible(bool visible) +{ + joint_currentVisible = visible; + ui->editIdleCurrent->setVisible(visible); + ui->editPositionCurrent->setVisible(visible); + ui->editPositionDirCurrent->setVisible(visible); + ui->editMixedCurrent->setVisible(visible); + ui->editTorqueCurrent->setVisible(visible); + ui->editPWMCurrent->setVisible(visible); + //ui->editCurrentCurrent->setVisible(visible); + ui->editVelocityCurrent->setVisible(visible); + + ui->labelIdleCurrent->setVisible(visible); + ui->labelIdleCurrentUnits->setVisible(visible); + ui->labelPositionCurrent->setVisible(visible); + ui->labelPositionCurrentUnits->setVisible(visible); + ui->labelPositionDirCurrent->setVisible(visible); + ui->labelPositionDirCurrentUnits->setVisible(visible); + ui->labelMixedCurrent->setVisible(visible); + ui->labelMixedCurrentUnits->setVisible(visible); + ui->labelTorqueCurrent->setVisible(visible); + ui->labelTorqueCurrentUnits->setVisible(visible); + ui->labelPWMCurrent->setVisible(visible); + //ui->labelCurrentCurrent->setVisible(visible); + ui->labelPWMCurrentUnits->setVisible(visible); + //ui->labelCurrentCurrentUnits->setVisible(visible); + ui->labelVelocityCurrent->setVisible(visible); + ui->labelVelocityCurrentUnits->setVisible(visible); + + + if (!visible) { + ui->editIdleCurrent->setMinimumHeight(0); + ui->editPositionCurrent->setMinimumHeight(0); + ui->editPositionDirCurrent->setMinimumHeight(0); + ui->editMixedCurrent->setMinimumHeight(0); + ui->editTorqueCurrent->setMinimumHeight(0); + ui->editPWMCurrent->setMinimumHeight(0); + //ui->editCurrentCurrent->setMinimumHeight(0); + ui->editVelocityCurrent->setMinimumHeight(0); + + ui->labelPositionCurrent->setMinimumHeight(0); + ui->labelPositionCurrentUnits->setMinimumHeight(0); + ui->labelPositionDirCurrent->setMinimumHeight(0); + ui->labelPositionDirCurrentUnits->setMinimumHeight(0); + ui->labelMixedCurrent->setMinimumHeight(0); + ui->labelMixedCurrentUnits->setMinimumHeight(0); + ui->labelTorqueCurrent->setMinimumHeight(0); + ui->labelTorqueCurrentUnits->setMinimumHeight(0); + ui->labelPWMCurrent->setMinimumHeight(0); + ui->labelPWMCurrentUnits->setMinimumHeight(0); + //ui->labelCurrentCurrent->setMinimumHeight(0); + //ui->labelCurrentCurrentUnits->setMinimumHeight(0); + ui->labelVelocityCurrent->setMinimumHeight(0); + ui->labelVelocityCurrentUnits->setMinimumHeight(0); + ui->labelIdleCurrent->setMinimumHeight(0); + ui->labelIdleCurrentUnits->setMinimumHeight(0); + } + else { + ui->editIdleCurrent->setMinimumHeight(20); + ui->editPositionCurrent->setMinimumHeight(20); + ui->editPositionDirCurrent->setMinimumHeight(20); + ui->editMixedCurrent->setMinimumHeight(20); + ui->editTorqueCurrent->setMinimumHeight(20); + ui->editPWMCurrent->setMinimumHeight(20); + //ui->editCurrentCurrent->setMinimumHeight(20); + ui->editVelocityCurrent->setMinimumHeight(20); + + ui->labelPositionCurrent->setMinimumHeight(20); + ui->labelPositionCurrentUnits->setMinimumHeight(20); + ui->labelPositionDirCurrent->setMinimumHeight(20); + ui->labelPositionDirCurrentUnits->setMinimumHeight(20); + ui->labelMixedCurrent->setMinimumHeight(20); + ui->labelMixedCurrentUnits->setMinimumHeight(20); + ui->labelTorqueCurrent->setMinimumHeight(20); + ui->labelTorqueCurrentUnits->setMinimumHeight(20); + ui->labelPWMCurrent->setMinimumHeight(20); + ui->labelPWMCurrentUnits->setMinimumHeight(20); + //ui->labelCurrentCurrent->setMinimumHeight(20); + //ui->labelCurrentCurrentUnits->setMinimumHeight(20); + ui->labelVelocityCurrent->setMinimumHeight(20); + ui->labelVelocityCurrentUnits->setMinimumHeight(20); + ui->labelIdleCurrent->setMinimumHeight(20); + ui->labelIdleCurrentUnits->setMinimumHeight(20); + } } void JointItem::setSpeedVisible(bool visible) @@ -772,7 +1000,7 @@ void JointItem::enableCurrentSliderDoubleAuto() int v = ui->sliderCurrentOutput->value(); if (v > sliderMax) {} if (v < sliderMin) {} - setCurrent(ref_current); + setRefCurrent(ref_current); } void JointItem::enableCurrentSliderDoubleValue(double value) @@ -786,7 +1014,7 @@ void JointItem::enableCurrentSliderDoubleValue(double value) int v = ui->sliderCurrentOutput->value(); if (v > sliderMax) {} if (v < sliderMin) {} - setCurrent(ref_current); + setRefCurrent(ref_current); } void JointItem::disableCurrentSliderDouble() @@ -805,7 +1033,7 @@ void JointItem::disableCurrentSliderDouble() int v = ui->sliderCurrentOutput->value(); if (v > sliderMax) {} if (v < sliderMin) {} - setCurrent(ref_current); + setRefCurrent(ref_current); } void JointItem::enableTrajectoryVelocitySliderDoubleAuto() @@ -1134,21 +1362,21 @@ void JointItem::updateMotionDone(bool done) int index = ui->stackedWidget->currentIndex(); if (index == POSITION) { if(!done){ - ui->editPositionCurrentPos->setStyleSheet("background-color: rgb(255, 38, 41);"); + ui->editPositionJointPos->setStyleSheet("background-color: rgb(255, 38, 41);"); }else{ - ui->editPositionCurrentPos->setStyleSheet("background-color: rgb(255, 255, 255);"); + ui->editPositionJointPos->setStyleSheet("background-color: rgb(255, 255, 255);"); } } else if (index == POSITION_DIR) { if(!done){ - ui->editPositionDirCurrentPos->setStyleSheet("background-color: rgb(255, 38, 41);"); + ui->editPositionDirJointPos->setStyleSheet("background-color: rgb(255, 38, 41);"); }else{ - ui->editPositionDirCurrentPos->setStyleSheet("background-color: rgb(255, 255, 255);"); + ui->editPositionDirJointPos->setStyleSheet("background-color: rgb(255, 255, 255);"); } } else if (index == MIXED) { if(!done){ - ui->editMixedCurrentPos->setStyleSheet("background-color: rgb(255, 38, 41);"); + ui->editMixedJointPos->setStyleSheet("background-color: rgb(255, 38, 41);"); }else{ - ui->editMixedCurrentPos->setStyleSheet("background-color: rgb(255, 255, 255);"); + ui->editMixedJointPos->setStyleSheet("background-color: rgb(255, 255, 255);"); } } } @@ -1212,7 +1440,7 @@ void JointItem::updateSliderTrajectoryVelocity(double val) ui->sliderTrajectoryVelocity->setValue(val); } -void JointItem::setPWM(double pwmValue) +void JointItem::setRefPWM(double pwmValue) { if(sliderPWMPressed){ return; @@ -1221,11 +1449,11 @@ void JointItem::setPWM(double pwmValue) updateSliderPWM(pwmValue); QString sVal; sVal = QString("%L1").arg(pwmValue, 0, 'f', 3); - ui->editPWMOutput->setText(sVal); + ui->editPWMDuty->setText(sVal); } } -void JointItem::setCurrent(double currentValue) +void JointItem::setRefCurrent(double currentValue) { if (sliderCurrentPressed){ return; @@ -1234,7 +1462,7 @@ void JointItem::setCurrent(double currentValue) updateSliderCurrent(currentValue); QString sVal; sVal = QString("%L1").arg(currentValue, 0, 'f', 3); - ui->editCurrentOutput->setText(sVal); + ui->editCurrentCurrent->setText(sVal); } } @@ -1253,36 +1481,36 @@ void JointItem::setPosition(double val) } if(ui->stackedWidget->currentIndex() == IDLE){ - ui->editIdleCurrentPos->setText(sVal); + ui->editIdleJointPos->setText(sVal); } if(ui->stackedWidget->currentIndex() == POSITION){ - ui->editPositionCurrentPos->setText(sVal); + ui->editPositionJointPos->setText(sVal); updateSliderPosition(ui->sliderTrajectoryPosition, val); } if(ui->stackedWidget->currentIndex() == POSITION_DIR){ - ui->editPositionDirCurrentPos->setText(sVal); + ui->editPositionDirJointPos->setText(sVal); updateSliderPosition(ui->sliderDirectPosition, val); } if(ui->stackedWidget->currentIndex() == MIXED){ - ui->editMixedCurrentPos->setText(sVal); + ui->editMixedJointPos->setText(sVal); updateSliderPosition(ui->sliderMixedPosition, val); } if(ui->stackedWidget->currentIndex() == VELOCITY){ - ui->editVelocityCurrentPos->setText(sVal); + ui->editVelocityJointPos->setText(sVal); } if(ui->stackedWidget->currentIndex() == TORQUE){ - ui->editTorqueCurrentPos->setText(sVal); + ui->editTorqueJointPos->setText(sVal); } if(ui->stackedWidget->currentIndex() == PWM){ - ui->editPWMCurrentPos->setText(sVal); + ui->editPWMJointPos->setText(sVal); } if (ui->stackedWidget->currentIndex() == CURRENT){ - ui->editCurrentCurrentPos->setText(sVal); + ui->editCurrentJointPos->setText(sVal); } } @@ -1373,22 +1601,88 @@ void JointItem::setMotorPosition(double val) QString sVal = QString("%1").arg(mot, 0, 'f', 1); if (ui->stackedWidget->currentIndex() == IDLE){ - ui->editIdleMotorPosition->setText(sVal); + ui->editIdleMotorPos->setText(sVal); } if (ui->stackedWidget->currentIndex() == POSITION){ - ui->editPositionMotorPosition->setText(sVal); + ui->editPositionMotorPos->setText(sVal); + } + + if (ui->stackedWidget->currentIndex() == POSITION_DIR) { + ui->editPositionDirMotorPos->setText(sVal); + } + + if (ui->stackedWidget->currentIndex() == MIXED) { + ui->editMixedMotorPos->setText(sVal); + } + + if (ui->stackedWidget->currentIndex() == VELOCITY) { + ui->editVelocityMotorPos->setText(sVal); + } + + if (ui->stackedWidget->currentIndex() == TORQUE) { + ui->editTorqueMotorPos->setText(sVal); + } + + if (ui->stackedWidget->currentIndex() == PWM) { + ui->editPWMMotorPos->setText(sVal); + } + + if (ui->stackedWidget->currentIndex() == CURRENT) { + ui->editCurrentMotorPos->setText(sVal); + } +} + +void JointItem::setDutyCycles(double val) +{ + if (!joint_dutyVisible) { + return; + } + + double mot = val; + QString sVal = QString("%1").arg(mot, 0, 'f', 1); + + //if (ui->stackedWidget->currentIndex() == IDLE) { + // ui->editIdleDuty->setText(sVal); + //} + + //if (ui->stackedWidget->currentIndex() == POSITION) { + // ui->editPositionDuty->setText(sVal); + //} + + //if (ui->stackedWidget->currentIndex() == POSITION_DIR) { + // ui->editPositionDirDuty->setText(sVal); + //} + + //if (ui->stackedWidget->currentIndex() == MIXED) { + // ui->editMixedDuty->setText(sVal); + //} + + //if (ui->stackedWidget->currentIndex() == VELOCITY) { + // ui->editVelocityDuty->setText(sVal); + //} + + if (ui->stackedWidget->currentIndex() == TORQUE) { + ui->editTorqueDuty->setText(sVal); + } + + if (ui->stackedWidget->currentIndex() == PWM) { + ui->editPWMDuty->setText(sVal); + } + + if (ui->stackedWidget->currentIndex() == CURRENT) { + ui->editCurrentDuty->setText(sVal); } } -void JointItem::setSpeed(double val) +void JointItem::setSpeed(double meas) { if (!joint_speedVisible){ return; } //TODO - double speed = val; + double speed = meas; QString sVal = QString("%1").arg(speed,0,'f',1); if(ui->stackedWidget->currentIndex() == IDLE){ @@ -1426,6 +1720,46 @@ void JointItem::setSpeed(double val) } +void JointItem::setCurrent(double meas) +{ + if (!joint_currentVisible) { + return; + } + //TODO + + double current = meas; + QString sVal = QString("%1").arg(current, 0, 'f', 3); + + if (ui->stackedWidget->currentIndex() == IDLE) { + ui->editIdleCurrent->setText(sVal); + } + + if (ui->stackedWidget->currentIndex() == POSITION) { + ui->editPositionCurrent->setText(sVal); + } + + if (ui->stackedWidget->currentIndex() == POSITION_DIR) { + ui->editPositionDirCurrent->setText(sVal); + } + + if (ui->stackedWidget->currentIndex() == MIXED) { + ui->editMixedCurrent->setText(sVal); + } + + if (ui->stackedWidget->currentIndex() == VELOCITY) { + ui->editVelocityCurrent->setText(sVal); + } + if (ui->stackedWidget->currentIndex() == TORQUE) { + ui->editTorqueCurrent->setText(sVal); + } + if (ui->stackedWidget->currentIndex() == PWM) { + ui->editPWMCurrent->setText(sVal); + } + if (ui->stackedWidget->currentIndex() == CURRENT) { + ui->editCurrentCurrent->setText(sVal); + } + +} @@ -1438,10 +1772,10 @@ void JointItem::setJointInternalInteraction(int interaction) if(ui->stackedWidget->widget(interaction)){ QVariant variant = ui->comboInteraction->itemData(interaction,Qt::BackgroundRole); - QColor c = variant.value(); + // QColor c = variant.value(); - QString styleSheet = QString("%1 QComboBox:!editable, QComboBox::drop-down:editable {background-color: rgb(%2,%3,%4);} %5").arg(comboStyle1).arg(c.red()).arg(c.green()).arg(c.blue()).arg(comboStyle2); - ui->comboInteraction->setStyleSheet(styleSheet); + // QString styleSheet = QString("%1 QComboBox:!editable, QComboBox::drop-down:editable {background-color: rgb(%2,%3,%4);} %5").arg(comboStyle1).arg(c.red()).arg(c.green()).arg(c.blue()).arg(comboStyle2); + // ui->comboInteraction->setStyleSheet(styleSheet); } } @@ -1490,7 +1824,7 @@ void JointItem::setJointInternalState(int mode) setStyleSheet(QString("font: 8pt; background-color: rgb(%1,%2,%3);").arg(c.red()).arg(c.green()).arg(c.blue())); QString styleSheet = QString("%1 QComboBox:!editable, QComboBox::drop-down:editable {background-color: rgb(%2,%3,%4);} %5").arg(comboStyle1).arg(c.red()).arg(c.green()).arg(c.blue()).arg(comboStyle2); - ui->comboMode->setStyleSheet(styleSheet); + // ui->comboMode->setStyleSheet(styleSheet); } } diff --git a/src/yarpmotorgui/jointitem.h b/src/yarpmotorgui/jointitem.h index b3f889c3c3..2c7d617174 100644 --- a/src/yarpmotorgui/jointitem.h +++ b/src/yarpmotorgui/jointitem.h @@ -51,15 +51,17 @@ class JointItem : public QWidget void setJointInteraction(JointInteraction interaction); void setJointState(JointState); void setPosition(double val); - void setTorque(double val); - void setRefTorque(double val); - void setRefVelocitySpeed(double val); - void setRefTrajectorySpeed(double val); - void setRefTrajectoryPosition(double val); + void setTorque(double meas); + void setRefTorque(double ref); + void setRefVelocitySpeed(double ref); + void setRefTrajectorySpeed(double ref); + void setRefTrajectoryPosition(double ref); void setSpeed(double val); - void setMotorPosition(double val); - void setPWM(double val); - void setCurrent(double val); + void setMotorPosition(double meas); + void setDutyCycles(double duty); + void setRefPWM(double ref); + void setCurrent(double meas); + void setRefCurrent(double ref); void updateMotionDone(bool done); void setJointName(QString name); QString getJointName(); @@ -78,6 +80,8 @@ class JointItem : public QWidget void setSpeedVisible(bool); void setMotorPositionVisible(bool); + void setDutyVisible(bool); + void setCurrentsVisible(bool); void setUnits(yarp::dev::JointTypeEnum t); void viewPositionTarget(bool); void enableControlVelocity(bool control); @@ -140,10 +144,11 @@ class JointItem : public QWidget bool sliderPWMPressed; bool sliderCurrentPressed; bool motionDone; - QString movingSliderStyle; bool enableCalib; bool joint_speedVisible; bool joint_motorPositionVisible; + bool joint_currentVisible; + bool joint_dutyVisible; QTimer velocityTimer; double lastVelocity; bool velocityModeEnabled; diff --git a/src/yarpmotorgui/jointitem.ui b/src/yarpmotorgui/jointitem.ui index 28f0513bce..a6734ceeba 100644 --- a/src/yarpmotorgui/jointitem.ui +++ b/src/yarpmotorgui/jointitem.ui @@ -10,19 +10,25 @@ 0 0 329 - 240 + 302 + + + 0 + 0 + + - 290 - 240 + 300 + 302 329 - 240 + 302 @@ -46,6 +52,18 @@ + + + 0 + 0 + + + + + 300 + 300 + + JOINT @@ -67,15 +85,21 @@ + + + 0 + 0 + + - 0 + 100 0 - 16777215 + 100 16777215 @@ -120,6 +144,24 @@ + + + 0 + 0 + + + + + 100 + 0 + + + + + 100 + 16777215 + + false @@ -173,6 +215,24 @@ + + + 0 + 0 + + + + + 100 + 0 + + + + + 100 + 16777215 + + false @@ -190,6 +250,24 @@ + + + 0 + 0 + + + + + 82 + 20 + + + + + 100 + 16777215 + + Home @@ -197,6 +275,24 @@ + + + 0 + 0 + + + + + 82 + 20 + + + + + 100 + 16777215 + + Idle @@ -204,6 +300,24 @@ + + + 0 + 0 + + + + + 82 + 20 + + + + + 100 + 16777215 + + Calib @@ -211,6 +325,24 @@ + + + 0 + 0 + + + + + 82 + 20 + + + + + 100 + 16777215 + + Run @@ -218,6 +350,24 @@ + + + 0 + 0 + + + + + 82 + 20 + + + + + 100 + 16777215 + + PID @@ -228,6 +378,18 @@ + + + 0 + 0 + + + + + 204 + 0 + + QSlider::groove:horizontal:enabled { border: 1px solid #999999; @@ -261,7 +423,7 @@ QSlider::groove:horizontal:disabled { } - 7 + 5 @@ -280,66 +442,6 @@ QSlider::groove:horizontal:disabled { 2 - - - - Position - - - - 2 - - - 2 - - - - - - 2 - - - 0 - - - 0 - - - 0 - - - 0 - - - - - Encoder: - - - - - - - background-color: rgb(255, 255, 255); - - - true - - - - - - - deg - - - - - - - - - @@ -349,15 +451,39 @@ QSlider::groove:horizontal:disabled { 2 - + + + + A + + + + + + + background-color: rgb(255, 255, 255); + + + true + + + + + + + Current: + + + + Torque: - - + + 16777215 @@ -372,22 +498,46 @@ QSlider::groove:horizontal:disabled { - - + + - Nm + deg - + Speed: + + + + background-color: rgb(255, 255, 255); + + + true + + + + + + + deg/s + + + + + + + Nm + + + - + 16777215 @@ -402,29 +552,32 @@ QSlider::groove:horizontal:disabled { - - - - deg/s - - - - - + + MotorPos: - - + + + + background-color: rgb(255, 255, 255); + true - - + + + + Encoder: + + + + + deg @@ -454,6 +607,12 @@ QSlider::groove:horizontal:disabled { + + + 0 + 0 + + 0 @@ -481,6 +640,12 @@ QSlider::groove:horizontal:disabled { + + + 0 + 0 + + 0 @@ -527,31 +692,6 @@ QSlider::groove:horizontal:disabled { 0 - - - - Encoder: - - - - - - - background-color: rgb(255, 255, 255); - - - - true - - - - - - - deg - - - @@ -560,6 +700,12 @@ QSlider::groove:horizontal:disabled { + + + 0 + 0 + + Velocity @@ -581,6 +727,12 @@ QSlider::groove:horizontal:disabled { + + + 0 + 0 + + 0 @@ -630,15 +782,53 @@ QSlider::groove:horizontal:disabled { 6 + + + + Current: + + + - + - Nm + deg - - + + + + deg/s + + + + + + + Encoder: + + + + + + + A + + + + + + + background-color: rgb(255, 255, 255); + + + true + + + + + 16777215 @@ -653,29 +843,36 @@ QSlider::groove:horizontal:disabled { - - + + - Torque: + Nm - + + + + deg + + + + Speed: - - + + - deg/s + MotorPos: - - + + 16777215 @@ -690,24 +887,31 @@ QSlider::groove:horizontal:disabled { - - - - MotorPos: + + + + background-color: rgb(255, 255, 255); + + + true - - + + + + background-color: rgb(255, 255, 255); + + true - - + + - deg + Torque: @@ -796,30 +1000,6 @@ QSlider::groove:horizontal:disabled { 0 - - - - Encoder: - - - - - - - background-color: rgb(255, 255, 255); - - - true - - - - - - - deg - - - @@ -835,21 +1015,8 @@ QSlider::groove:horizontal:disabled { 2 - - - - Torque: - - - - - - - 16777215 - 16777215 - - + background-color: rgb(255, 255, 255); @@ -858,21 +1025,35 @@ QSlider::groove:horizontal:disabled { + + + + Torque: + + + + + + + Current + + + - + - Nm + deg - - + + - Speed: + Nm - + @@ -888,13 +1069,102 @@ QSlider::groove:horizontal:disabled { - + + + + A + + + + + + + Encoder: + + + + + + + Speed: + + + + deg/s + + + + background-color: rgb(255, 255, 255); + + + true + + + + + + + + 16777215 + 16777215 + + + + background-color: rgb(255, 255, 255); + + + true + + + + + + + MotorPos: + + + + + + + background-color: rgb(255, 255, 255); + + + + + + + deg + + + + + + + background-color: rgb(255, 255, 255); + + + + + + + % + + + + + + + DutyCycle: + + + @@ -921,6 +1191,12 @@ QSlider::groove:horizontal:disabled { + + + 0 + 0 + + 0 @@ -991,31 +1267,6 @@ QSlider::groove:horizontal:disabled { 0 - - - - Encoder: - - - - - - - background-color: rgb(255, 255, 255); - - - - true - - - - - - - deg - - - @@ -1024,6 +1275,12 @@ QSlider::groove:horizontal:disabled { + + + 0 + 0 + + Velocity @@ -1094,8 +1351,22 @@ QSlider::groove:horizontal:disabled { 6 + + + + deg/s + + + + + + + Encoder: + + + - + 16777215 @@ -1110,29 +1381,26 @@ QSlider::groove:horizontal:disabled { - + Speed: - - - - deg/s + + + + background-color: rgb(255, 255, 255); + - - - - - - Nm + + true - - + + 16777215 @@ -1147,13 +1415,72 @@ QSlider::groove:horizontal:disabled { - + Torque: + + + + Nm + + + + + + + Current: + + + + + + + background-color: rgb(255, 255, 255); + + + true + + + + + + + A + + + + + + + deg + + + + + + + MotorPos: + + + + + + + deg + + + + + + + background-color: rgb(255, 255, 255); + + + @@ -1230,72 +1557,6 @@ QSlider::groove:horizontal:disabled { - - - - Position - - - - 2 - - - 0 - - - 2 - - - 0 - - - - - - 2 - - - 0 - - - 0 - - - 0 - - - 0 - - - - - Encoder: - - - - - - - background-color: rgb(255, 255, 255); - - - true - - - - - - - deg - - - - - - - - - @@ -1305,15 +1566,22 @@ QSlider::groove:horizontal:disabled { 2 - - + + - Torque: + Nm - - + + + + Speed: + + + + + 16777215 @@ -1328,22 +1596,15 @@ QSlider::groove:horizontal:disabled { - - + + - Nm - - - - - - - Speed: + Current: - + 16777215 @@ -1358,13 +1619,82 @@ QSlider::groove:horizontal:disabled { - + + + + A + + + + + + + Torque: + + + + + + + background-color: rgb(255, 255, 255); + + + true + + + + deg/s + + + + background-color: rgb(255, 255, 255); + + + true + + + + + + + deg + + + + + + + Encoder: + + + + + + + background-color: rgb(255, 255, 255); + + + + + + + deg + + + + + + + MotorPos: + + + @@ -1413,92 +1743,26 @@ QSlider::groove:horizontal:disabled { 0 - 40 - - - - false - - - margin-top:10px; - - - - - -50 - - - 50 - - - Qt::Horizontal - - - - - - - - - - Position - - - - 2 - - - 0 - - - 2 - - - 2 - - - - - - 2 - - - 0 - - - 0 - - - 0 - - - 0 - - - - - Encoder: - - - - - - - background-color: rgb(255, 255, 255); - - - true - - - - - - - deg - - - - + 40 + + + + false + + + margin-top:10px; + + + + + -50 + + + 50 + + + Qt::Horizontal + @@ -1513,24 +1777,46 @@ QSlider::groove:horizontal:disabled { 2 + + + + Nm + + + + + + + A + + + - false + true Torque: - - - - - 16777215 - 16777215 - + + + + Speed: + + + + + + + deg/s + + + + background-color: rgb(255, 255, 255); @@ -1539,21 +1825,24 @@ QSlider::groove:horizontal:disabled { - - - - Nm + + + + background-color: rgb(255, 255, 255); + + + true - - + + - Speed: + Current: - + @@ -1569,10 +1858,75 @@ QSlider::groove:horizontal:disabled { + + + + + 16777215 + 16777215 + + + + background-color: rgb(255, 255, 255); + + + true + + + + + + + deg + + + + + + + Encoder: + + + + + + + MotorPos: + + + + + + + deg + + + + + + + background-color: rgb(255, 255, 255); + + + + + + + DutyCycle: + + + + + + + background-color: rgb(255, 255, 255); + + + - + - deg/s + % @@ -1664,10 +2018,10 @@ QSlider::groove:horizontal:disabled { 2 - - + + - deg/s + Nm @@ -1710,10 +2064,10 @@ QSlider::groove:horizontal:disabled { - - + + - Nm + deg/s @@ -1725,14 +2079,38 @@ QSlider::groove:horizontal:disabled { - + Encoder: + + + + % + + + + + + + background-color: rgb(255, 255, 255); + + + true + + + + + + + DutyCycle: + + + - + background-color: rgb(255, 255, 255); @@ -1748,20 +2126,45 @@ QSlider::groove:horizontal:disabled { - - + + + + background-color: rgb(255, 255, 255); + + + + + - DutyCycle + A - - + + + + Current: + + - - + + - % + deg + + + + + + + MotorPos: + + + + + + + background-color: rgb(255, 255, 255); @@ -1853,15 +2256,22 @@ QSlider::groove:horizontal:disabled { 2 - + deg/s + + + + Torque: + + + - + 16777215 @@ -1876,15 +2286,15 @@ QSlider::groove:horizontal:disabled { - - + + - Torque: + deg - - + + 16777215 @@ -1899,58 +2309,97 @@ QSlider::groove:horizontal:disabled { - + + + + background-color: rgb(255, 255, 255); + + + + + + + A + + + + + + + Current: + + + + + + + background-color: rgb(255, 255, 255); + + + true + + + + Nm - - + + - Speed: + MotorPos: - + Encoder: - - + + background-color: rgb(255, 255, 255); - - true + + + + + + Speed: - - + + deg - - + + - Output: + DutyCycle: - - - - - + + - mA + % + + + + + + + background-color: rgb(255, 255, 255); diff --git a/src/yarpmotorgui/mainwindow.cpp b/src/yarpmotorgui/mainwindow.cpp index 0991b2eff1..9c874b231c 100644 --- a/src/yarpmotorgui/mainwindow.cpp +++ b/src/yarpmotorgui/mainwindow.cpp @@ -255,7 +255,9 @@ MainWindow::MainWindow(QWidget *parent) : QAction *viewGlobalToolbar = windows->addAction("Global Commands Toolbar"); QAction *viewPartToolbar = windows->addAction("Part Commands Toolbar"); QAction *viewSpeedValues = windows->addAction("View Speed Values"); + QAction *viewCurrentValues = windows->addAction("View Current Values"); QAction *viewMotorPosition = windows->addAction("View Motor Position"); + QAction *viewDutyCycles = windows->addAction("View Duty Cycles"); QAction *viewPositionTarget = windows->addAction("View Position Target"); QAction *enableControlVelocity = windows->addAction("Enable Velocity Control"); QAction *enableControlMixed = windows->addAction("Enable Mixed Control"); @@ -267,7 +269,9 @@ MainWindow::MainWindow(QWidget *parent) : viewGlobalToolbar->setCheckable(true); viewPartToolbar->setCheckable(true); viewSpeedValues->setCheckable(true); + viewCurrentValues->setCheckable(true); viewMotorPosition->setCheckable(true); + viewDutyCycles->setCheckable(true); enableControlVelocity->setCheckable(true); enableControlMixed->setCheckable(true); enableControlPositionDirect->setCheckable(true); @@ -281,11 +285,15 @@ MainWindow::MainWindow(QWidget *parent) : bool bSpeedValues = settings.value("SpeedValuesVisible",false).toBool(); bool bViewPositionTarget = settings.value("ViewPositionTarget", true).toBool(); bool bviewMotorPosition = settings.value("MotorPositionVisible", false).toBool(); + bool bviewDutyCycles = settings.value("DutyCycleVisible", false).toBool(); + bool bCurrentValues = settings.value("CurrentsVisible", false).toBool(); viewGlobalToolbar->setChecked(bViewGlobalToolbar); viewPartToolbar->setChecked(bViewPartToolbar); viewSpeedValues->setChecked(bSpeedValues); + viewCurrentValues->setChecked(bCurrentValues); viewMotorPosition->setChecked(bviewMotorPosition); + viewDutyCycles->setChecked(bviewDutyCycles); viewPositionTarget->setChecked(bViewPositionTarget); enableControlVelocity->setChecked(false); enableControlMixed->setChecked(false); @@ -299,7 +307,9 @@ MainWindow::MainWindow(QWidget *parent) : connect(viewGlobalToolbar,SIGNAL(triggered(bool)),this,SLOT(onViewGlobalToolbar(bool))); connect(viewPartToolbar,SIGNAL(triggered(bool)),this,SLOT(onViewPartToolbar(bool))); connect(viewSpeedValues,SIGNAL(triggered(bool)),this,SLOT(onViewSpeeds(bool))); + connect(viewCurrentValues, SIGNAL(triggered(bool)), this, SLOT(onViewCurrents(bool))); connect(viewMotorPosition, SIGNAL(triggered(bool)), this, SLOT(onViewMotorPositions(bool))); + connect(viewDutyCycles, SIGNAL(triggered(bool)), this, SLOT(onViewDutyCycles(bool))); connect(viewPositionTarget, SIGNAL(triggered(bool)), this, SLOT(onViewPositionTarget(bool))); connect(enableControlVelocity, SIGNAL(triggered(bool)), this, SLOT(onEnableControlVelocity(bool))); connect(enableControlMixed, SIGNAL(triggered(bool)), this, SLOT(onEnableControlMixed(bool))); @@ -451,6 +461,14 @@ void MainWindow::onViewSpeeds(bool val) emit sig_viewSpeedValues(val); } +void MainWindow::onViewCurrents(bool val) +{ + QSettings settings("YARP", "yarpmotorgui"); + settings.setValue("CurrentValuesVisible", val); + + emit sig_viewCurrentValues(val); +} + void MainWindow::onViewMotorPositions(bool val) { QSettings settings("YARP", "yarpmotorgui"); @@ -459,6 +477,14 @@ void MainWindow::onViewMotorPositions(bool val) emit sig_viewMotorPositions(val); } +void MainWindow::onViewDutyCycles(bool val) +{ + QSettings settings("YARP", "yarpmotorgui"); + settings.setValue("DutyCyclesVisible", val); + + emit sig_viewDutyCycles(val); +} + void MainWindow::onViewPositionTarget(bool val) { QSettings settings("YARP", "yarpmotorgui"); @@ -605,7 +631,9 @@ bool MainWindow::init(QStringList enabledParts, connect(part,SIGNAL(sequenceActivated()),this,SLOT(onSequenceActivated())); connect(part,SIGNAL(sequenceStopped()),this,SLOT(onSequenceStopped())); connect(this,SIGNAL(sig_viewSpeedValues(bool)),part,SLOT(onViewSpeedValues(bool))); + connect(this, SIGNAL(sig_viewCurrentValues(bool)), part, SLOT(onViewCurrentValues(bool))); connect(this, SIGNAL(sig_viewMotorPositions(bool)), part, SLOT(onViewMotorPositions(bool))); + connect(this, SIGNAL(sig_viewDutyCycles(bool)), part, SLOT(onViewDutyCycles(bool))); connect(this, SIGNAL(sig_setPosSliderOptionMW(int, double)), part, SLOT(onSetPosSliderOptionPI(int, double))); connect(this, SIGNAL(sig_setVelSliderOptionMW(int, double)), part, SLOT(onSetVelSliderOptionPI(int, double))); connect(this, SIGNAL(sig_setTrqSliderOptionMW(int, double)), part, SLOT(onSetTrqSliderOptionPI(int, double))); @@ -664,9 +692,13 @@ bool MainWindow::init(QStringList enabledParts, QSettings settings("YARP","yarpmotorgui"); bool speedVisible = settings.value("SpeedValuesVisible",false).toBool(); bool motorPosVisible = settings.value("MotorPositionVisible", false).toBool(); + bool currentVisible = settings.value("CurrentsVisible", false).toBool(); + bool dutyVisible = settings.value("DutyCyclesVisible", false).toBool(); onViewSpeeds(speedVisible); + onViewCurrents(currentVisible); onViewMotorPositions(motorPosVisible); + onViewDutyCycles(dutyVisible); return true; } diff --git a/src/yarpmotorgui/mainwindow.h b/src/yarpmotorgui/mainwindow.h index 16759de653..1d5d930d32 100644 --- a/src/yarpmotorgui/mainwindow.h +++ b/src/yarpmotorgui/mainwindow.h @@ -126,7 +126,9 @@ private slots: void onViewGlobalToolbar(bool); void onViewPartToolbar(bool); void onViewSpeeds(bool); + void onViewCurrents(bool); void onViewMotorPositions(bool); + void onViewDutyCycles(bool); void onViewPositionTarget(bool); void onEnableControlVelocity(bool val); void onEnableControlMixed(bool val); @@ -145,7 +147,9 @@ private slots: void sig_enableControlPWM(bool); void sig_enableControlCurrent(bool); void sig_viewSpeedValues(bool); + void sig_viewCurrentValues(bool); void sig_viewMotorPositions(bool); + void sig_viewDutyCycles(bool); void sig_setPosSliderOptionMW(int, double); void sig_setVelSliderOptionMW(int, double); void sig_setTrqSliderOptionMW(int, double); diff --git a/src/yarpmotorgui/partitem.cpp b/src/yarpmotorgui/partitem.cpp index 388b8121e7..6ad38a4079 100644 --- a/src/yarpmotorgui/partitem.cpp +++ b/src/yarpmotorgui/partitem.cpp @@ -56,10 +56,14 @@ PartItem::PartItem(QString robotName, int id, QString partName, ResourceFinder& m_torques(nullptr), m_positions(nullptr), m_speeds(nullptr), + m_currents(nullptr), m_motorPositions(nullptr), + m_dutyCycles(nullptr), m_done(nullptr), m_part_speedVisible(false), m_part_motorPositionVisible(false), + m_part_dutyVisible(false), + m_part_currentVisible(false), m_interactionModes(nullptr), m_finder(&_finder), m_iMot(nullptr), @@ -138,19 +142,22 @@ PartItem::PartItem(QString robotName, int id, QString partName, ResourceFinder& if (m_interfaceError == false) { + int i = 0; std::string jointname; int number_of_joints; m_iPos->getAxes(&number_of_joints); - m_controlModes = new int[number_of_joints]; - m_refTrajectorySpeeds = new double[number_of_joints]; - m_refTrajectoryPositions = new double[number_of_joints]; - m_refTorques = new double[number_of_joints]; - m_refVelocitySpeeds = new double[number_of_joints]; - m_torques = new double[number_of_joints]; - m_positions = new double[number_of_joints]; - m_speeds = new double[number_of_joints]; - m_motorPositions = new double[number_of_joints]; + m_controlModes = new int[number_of_joints]; //for (i = 0; i < number_of_joints; i++) m_controlModes = 0; + m_refTrajectorySpeeds = new double[number_of_joints]; for (i = 0; i < number_of_joints; i++) m_refTrajectorySpeeds[i] = std::nan(""); + m_refTrajectoryPositions = new double[number_of_joints]; for (i = 0; i < number_of_joints; i++) m_refTrajectoryPositions[i] = std::nan(""); + m_refTorques = new double[number_of_joints]; for (i = 0; i < number_of_joints; i++) m_refTorques[i] = std::nan(""); + m_refVelocitySpeeds = new double[number_of_joints]; for (i = 0; i < number_of_joints; i++) m_refVelocitySpeeds[i] = std::nan(""); + m_torques = new double[number_of_joints]; for (i = 0; i < number_of_joints; i++) m_torques[i] = std::nan(""); + m_positions = new double[number_of_joints]; for (i = 0; i < number_of_joints; i++) m_positions[i] = std::nan(""); + m_speeds = new double[number_of_joints]; for (i = 0; i < number_of_joints; i++) m_speeds[i] = std::nan(""); + m_currents = new double[number_of_joints]; for (i = 0; i < number_of_joints; i++) m_currents[i] = std::nan(""); + m_motorPositions = new double[number_of_joints]; for (i = 0; i < number_of_joints; i++) m_motorPositions[i] = std::nan(""); + m_dutyCycles = new double[number_of_joints]; for (i = 0; i < number_of_joints; i++) m_dutyCycles[i] = std::nan(""); m_done = new bool[number_of_joints]; m_interactionModes = new yarp::dev::InteractionModeEnum[number_of_joints]; @@ -196,6 +203,7 @@ PartItem::PartItem(QString robotName, int id, QString partName, ResourceFinder& m_iinfo->getAxisName(k, jointname); yarp::dev::JointTypeEnum jtype = yarp::dev::VOCAB_JOINTTYPE_REVOLUTE; + m_iinfo->getJointType(k, jtype); Pid myPid(0,0,0,0,0,0); yarp::os::SystemClock::delaySystem(0.005); @@ -316,7 +324,9 @@ PartItem::~PartItem() if (m_torques) { delete[] m_torques; m_torques = nullptr; } if (m_positions) { delete[] m_positions; m_positions = nullptr; } if (m_speeds) { delete[] m_speeds; m_speeds = nullptr; } + if (m_currents) { delete[] m_currents; m_currents = nullptr; } if (m_motorPositions) { delete[] m_motorPositions; m_motorPositions = nullptr; } + if (m_dutyCycles) { delete[] m_dutyCycles; m_dutyCycles = nullptr; } if (m_done) { delete[] m_done; m_done = nullptr; } } @@ -529,8 +539,8 @@ void PartItem::onDumpAllRemoteVariables() if (m_iVar->getRemoteVariablesList(&keys)) { std::string s = keys.toString(); - int keys_size = keys.size(); - for (int i = 0; i < keys_size; i++) + size_t keys_size = keys.size(); + for (size_t i = 0; i < keys_size; i++) { std::string key_name; if (keys.get(i).isString()) @@ -622,9 +632,9 @@ void PartItem::onSendPWM(int jointIndex, double pwmVal) m_iPWM->setRefDutyCycle(jointIndex, pwmVal); yarp::os::SystemClock::delaySystem(0.010); - m_iPWM->getRefDutyCycle(jointIndex, &pwm_reference); //This is the reference reference + m_iPWM->getRefDutyCycle(jointIndex, &pwm_reference); //This is the reference yarp::os::SystemClock::delaySystem(0.010); - m_iPWM->getDutyCycle(jointIndex, ¤t_pwm); //This is the reak PWM output + m_iPWM->getDutyCycle(jointIndex, ¤t_pwm); //This is the real PWM output if (m_currentPidDlg){ m_currentPidDlg->initPWM(pwm_reference, current_pwm); @@ -1334,7 +1344,7 @@ void PartItem::onOpenSequence() if(desiredExtension != extension){ QMessageBox::critical(this,"Error Loading The Sequence", - QString("Wrong format (check estensions) of the file associated with: ").arg(m_partName)); + QString("Wrong format (check extensions) of the file associated with: ").arg(m_partName)); return; } @@ -1904,6 +1914,26 @@ void PartItem::onViewMotorPositions(bool view) } } +void PartItem::onViewDutyCycles(bool view) +{ + m_part_dutyVisible = view; + for (int i = 0; icount(); i++) + { + JointItem *joint = (JointItem*)m_layout->itemAt(i)->widget(); + joint->setDutyVisible(view); + } +} + +void PartItem::onViewCurrentValues(bool view) +{ + m_part_currentVisible = view; + for (int i = 0; icount(); i++) + { + JointItem *joint = (JointItem*)m_layout->itemAt(i)->widget(); + joint->setCurrentsVisible(view); + } +} + void PartItem::onViewPositionTarget(bool ena) { for (int i = 0; icount(); i++) @@ -2108,13 +2138,40 @@ bool PartItem::updatePart() } // *** update measured encoders, velocity, torques *** - if (!m_iencs->getEncoders(m_positions)) { yWarning("Unable to update encoders"); return false; } - if (!m_iTrq->getTorques(m_torques)) { yWarning("Unable to update torques"); } - if (this->m_part_speedVisible && !m_iencs->getEncoderSpeeds(m_speeds)) { yWarning("Unable to update speeds"); } - if (this->m_part_motorPositionVisible && !m_iMot->getMotorEncoders(m_motorPositions)) { yWarning("Unable to update motorPositions"); } - + bool b = true; + if (1) + { + b = m_iencs->getEncoders(m_positions); + if (!b) { yWarning("Unable to update encoders"); return false; } + } + if (1) + { + b = m_iTrq->getTorques(m_torques); + if (!b) { yWarning("Unable to update torques"); } + } + if (this->m_part_currentVisible) + { + b = m_iCur->getCurrents(m_currents); + if (!b) { yWarning("Unable to update currents"); } + } + if (this->m_part_speedVisible) + { + b = m_iencs->getEncoderSpeeds(m_speeds); + if (!b) { yWarning("Unable to update speeds"); } + } + if (this->m_part_motorPositionVisible) + { + b = m_iMot->getMotorEncoders(m_motorPositions); + if (!b) { yWarning("Unable to update motorPositions"); } + } + if (this->m_part_dutyVisible) + { + b = m_iPWM->getDutyCycles(m_dutyCycles); + if (!b) { yWarning("Unable to update dutyCycles"); } + } + // *** update checkMotionDone, refTorque, refTrajectorySpeed, refSpeed *** - // (only one at a time in order to save badwidth) + // (only one at a time in order to save bandwidth) bool b_motdone = m_iPos->checkMotionDone(m_slow_k, &m_done[m_slow_k]); //using k to save bandwidth bool b_refTrq = m_iTrq->getRefTorque(m_slow_k, &m_refTorques[m_slow_k]); //using k to save bandwidth bool b_refPosSpeed = m_iPos->getRefSpeed(m_slow_k, &m_refTrajectorySpeeds[m_slow_k]); //using k to save bandwidth @@ -2152,8 +2209,12 @@ bool PartItem::updatePart() else {} if (1) { joint->setSpeed(m_speeds[jk]); } else {} + if (1) { joint->setCurrent(m_currents[jk]); } + else {} if (1) { joint->setMotorPosition(m_motorPositions[jk]); } else {} + if (1) { joint->setDutyCycles(m_dutyCycles[jk]); } + else {} } // *** update the widget NOT every cycle *** @@ -2209,17 +2270,17 @@ bool PartItem::updatePart() case VOCAB_CM_CURRENT: { joint->setJointState(JointItem::Current); - double tmp = 0; - m_iCur->getRefCurrent(k, &tmp); //? - joint->setCurrent(tmp); + double ref_current = 0; + m_iCur->getRefCurrent(k, &ref_current); + joint->setRefCurrent(ref_current); break; } case VOCAB_CM_PWM: { joint->setJointState(JointItem::Pwm); - double tmp = 0; - m_iPWM->getRefDutyCycle(k, &tmp); - joint->setPWM(tmp); + double ref_duty = 0; + m_iPWM->getRefDutyCycle(k, &ref_duty); + joint->setRefPWM(ref_duty); break; } case VOCAB_CM_HW_FAULT: diff --git a/src/yarpmotorgui/partitem.h b/src/yarpmotorgui/partitem.h index 5ae93c6a02..880ecc8b9e 100644 --- a/src/yarpmotorgui/partitem.h +++ b/src/yarpmotorgui/partitem.h @@ -46,9 +46,7 @@ using namespace yarp::dev; using namespace yarp::sig; using namespace yarp::os; -#define MAX_WIDTH_JOINT 240 - - +#define MAX_WIDTH_JOINT 320 class PartItem : public QWidget { @@ -130,10 +128,14 @@ class PartItem : public QWidget double* m_torques; double* m_positions; double* m_speeds; + double* m_currents; double* m_motorPositions; + double* m_dutyCycles; bool* m_done; bool m_part_speedVisible; bool m_part_motorPositionVisible; + bool m_part_dutyVisible; + bool m_part_currentVisible; yarp::dev::InteractionModeEnum* m_interactionModes; ResourceFinder* m_finder; @@ -178,6 +180,8 @@ public slots: bool updatePart(); void onViewSpeedValues(bool); void onViewMotorPositions(bool); + void onViewDutyCycles(bool); + void onViewCurrentValues(bool); void onSetPosSliderOptionPI(int mode, double step); void onSetVelSliderOptionPI(int mode, double step); void onSetTrqSliderOptionPI(int mode, double step); diff --git a/tests/libYARP_OS/BottleTest.cpp b/tests/libYARP_OS/BottleTest.cpp index 0314b58d42..485a60e916 100644 --- a/tests/libYARP_OS/BottleTest.cpp +++ b/tests/libYARP_OS/BottleTest.cpp @@ -423,7 +423,7 @@ class BottleTest : public UnitTest { Bottle bot("[send] 10 20"); checkEqual(bot.size(),(size_t) 3,"plausible parse"); checkTrue(bot.get(0).isVocab(),"vocab present"); - checkEqual(bot.get(0).asInt32(),VOCAB('s','e','n','d'), + checkEqual(bot.get(0).asInt32(),yarp::os::createVocab('s','e','n','d'), "vocab match"); } diff --git a/tests/libYARP_OS/VocabTest.cpp b/tests/libYARP_OS/VocabTest.cpp index fa2934c5df..12f2b6f79f 100644 --- a/tests/libYARP_OS/VocabTest.cpp +++ b/tests/libYARP_OS/VocabTest.cpp @@ -22,17 +22,17 @@ class VocabTest : public UnitTest { void checkConvert() { report(0,"checking vocabulary conversions"); - checkEqual(VOCAB2('h','i'),Vocab::encode("hi"),"encoding"); + checkEqual(yarp::os::createVocab('h','i'),Vocab::encode("hi"),"encoding"); checkEqual(Vocab::decode(Vocab::encode("hi")).c_str(),"hi","decoding"); - checkEqual(VOCAB4('h','i','g','h'),Vocab::encode("high"),"encoding"); + checkEqual(yarp::os::createVocab('h','i','g','h'),Vocab::encode("high"),"encoding"); checkEqual(Vocab::decode(Vocab::encode("high")).c_str(),"high","decoding"); report(0,"checking compile-time functions"); NetInt32 code = Vocab::encode("stop"); switch(code) { - case VOCAB3('s','e','t'): + case yarp::os::createVocab('s','e','t'): report(1,"very strange error switching"); break; - case VOCAB4('s','t','o','p'): + case yarp::os::createVocab('s','t','o','p'): report(0,"good switch"); break; default: diff --git a/tests/libYARP_sig/PointCloudTest.cpp b/tests/libYARP_sig/PointCloudTest.cpp index da47ba6bfa..6406d4d9ad 100644 --- a/tests/libYARP_sig/PointCloudTest.cpp +++ b/tests/libYARP_sig/PointCloudTest.cpp @@ -767,6 +767,160 @@ class PointCloudTest : public yarp::os::impl::UnitTest } + void readWritetoFromBottle() + { + { + report(0,"Testing readWriteFromBottle(toBottle) XYZ_NORMAL_RGBA"); + PointCloud testPC; + Port outPort; + Port inPort; + checkTrue(outPort.open("/test/pointcloud/out"),"Opening output port"); + checkTrue(inPort.open("/test/pointcloud/in"),"Opening input port"); + checkTrue(NetworkBase::connect(outPort.getName(), inPort.getName()),"Checking connection"); + size_t width = 21; size_t height = 32; + testPC.resize(width, height); + for (size_t i=0; i(i); + testPC(i).y = static_cast(i + 1); + testPC(i).z = static_cast(i + 2); + testPC(i).normal_x = static_cast(i*2); + testPC(i).normal_y = static_cast(i*3); + testPC(i).normal_z = static_cast(i*4); + testPC(i).curvature = static_cast(i*5); + testPC(i).r = 'r'; + testPC(i).g = 'g'; + testPC(i).b = 'b'; + testPC(i).a = 'a'; + } + + Bottle outBt = testPC.toBottle(); + outPort.enableBackgroundWrite(true); + checkTrue(outPort.write(outBt),"Checking write"); + yarp::os::Time::delay(0.2); + Bottle inBt; + checkTrue(inPort.read(inBt), "Checking read"); + PointCloud testPC2; + testPC2.fromBottle(inBt); + checkEqual(testPC.width(), testPC2.width(),"Checking width"); + checkEqual(testPC.height(), testPC2.height(),"Checking height"); + + bool ok = true; + + for (size_t i=0; i testPCfail; + checkFalse(testPCfail.fromBottle(inBt),"from bottle correctly failing... type mismatch"); + } + { + report(0,"Testing readWriteFromBottle(toBottle) XYZ_NORMAL"); + PointCloud testPC; + Port outPort; + Port inPort; + checkTrue(outPort.open("/test/pointcloud/out"),"Opening output port"); + checkTrue(inPort.open("/test/pointcloud/in"),"Opening input port"); + checkTrue(NetworkBase::connect(outPort.getName(), inPort.getName()),"Checking connection"); + size_t width = 21; size_t height = 32; + testPC.resize(width, height); + for (size_t i=0; i(i); + testPC(i).y = static_cast(i + 1); + testPC(i).z = static_cast(i + 2); + testPC(i).normal_x = static_cast(i*2); + testPC(i).normal_y = static_cast(i*3); + testPC(i).normal_z = static_cast(i*4); + testPC(i).curvature = static_cast(i*5); + } + + Bottle outBt = testPC.toBottle(); + outPort.enableBackgroundWrite(true); + checkTrue(outPort.write(outBt),"Checking write"); + yarp::os::Time::delay(0.2); + Bottle inBt; + checkTrue(inPort.read(inBt), "Checking read"); + PointCloud testPC2; + testPC2.fromBottle(inBt); + checkEqual(testPC.width(), testPC2.width(),"Checking width"); + checkEqual(testPC.height(), testPC2.height(),"Checking height"); + + bool ok = true; + + for (size_t i=0; i testPCfail; + checkFalse(testPCfail.fromBottle(inBt),"from bottle correctly failing... type mismatch"); + } + + + { + report(0,"Testing readWriteFromBottle(toBottle) XYZ"); + PointCloud testPC; + Port outPort; + Port inPort; + checkTrue(outPort.open("/test/pointcloud/out"),"Opening output port"); + checkTrue(inPort.open("/test/pointcloud/in"),"Opening input port"); + checkTrue(NetworkBase::connect(outPort.getName(), inPort.getName()),"Checking connection"); + size_t width = 21; size_t height = 32; + testPC.resize(width, height); + for (size_t i=0; i(i); + testPC(i).y = static_cast(i + 1); + testPC(i).z = static_cast(i + 2); + } + + Bottle outBt = testPC.toBottle(); + outPort.enableBackgroundWrite(true); + checkTrue(outPort.write(outBt),"Checking write"); + yarp::os::Time::delay(0.2); + Bottle inBt; + checkTrue(inPort.read(inBt), "Checking read"); + PointCloud testPC2; + testPC2.fromBottle(inBt); + checkEqual(testPC.width(), testPC2.width(),"Checking width"); + checkEqual(testPC.height(), testPC2.height(),"Checking height"); + + bool ok = true; + + for (size_t i=0; i testPCfail; + checkFalse(testPCfail.fromBottle(inBt),"from bottle correctly failing... type mismatch"); + } + + } + virtual void runTests() override { readWriteMatchTest(); @@ -776,6 +930,7 @@ class PointCloudTest : public yarp::os::impl::UnitTest fromExternalTest(); concatenationTest(); toFromBottle(); + readWritetoFromBottle(); } }; diff --git a/tests/libYARP_sig/TestList.h b/tests/libYARP_sig/TestList.h index cfe1d728d6..67e395c1a0 100644 --- a/tests/libYARP_sig/TestList.h +++ b/tests/libYARP_sig/TestList.h @@ -24,6 +24,7 @@ namespace yarp { // method. extern yarp::os::impl::UnitTest& getImageTest(); extern yarp::os::impl::UnitTest& getVectorTest(); +extern yarp::os::impl::UnitTest& getVectorOfTest(); extern yarp::os::impl::UnitTest& getSoundTest(); extern yarp::os::impl::UnitTest& getMatrixTest(); extern yarp::os::impl::UnitTest& getPointCloudTest(); @@ -34,6 +35,7 @@ class yarp::sig::impl::TestList { yarp::os::impl::UnitTest& root = yarp::os::impl::UnitTest::getRoot(); root.add(getImageTest()); root.add(getVectorTest()); + root.add(getVectorOfTest()); root.add(getMatrixTest()); root.add(getSoundTest()); root.add(getPointCloudTest()); diff --git a/tests/libYARP_sig/VectorOfTest.cpp b/tests/libYARP_sig/VectorOfTest.cpp index 521e92c549..224409f7cc 100644 --- a/tests/libYARP_sig/VectorOfTest.cpp +++ b/tests/libYARP_sig/VectorOfTest.cpp @@ -6,12 +6,13 @@ * BSD-3-Clause license. See the accompanying LICENSE file for details. */ -#include #include #include -#include #include +#include +#include #include +#include //#include @@ -23,247 +24,149 @@ using namespace yarp::os::impl; using namespace yarp::os; using namespace yarp::sig; -template -class SenderThread:public Thread -{ -public: - SenderThread(Port *p) - { - portOut=p; - } - bool threadInit() override - { - success=false; - return true; - } +class VectorOfTest : public UnitTest { - void run() override +public: + virtual std::string getName() const override { return "VectorOfTest"; } + void checkSendReceiveInt() { - VectorOf v; - - int times=10; + report(0, "check VectorO send receive"); - while(times--) { - v.clear(); - int k=0; - for(k=0;k<10;k++) - v.push_back(42); - - portOut->write(v); - Time::delay(0.1); - v.clear(); - - for(k=0;k<42;k++) - v.push_back(k); + Port portIn; + Port portOut; - portOut->write(v); - } - - success=true; - } - - Port *portOut; - bool success; -}; + portOut.open("/harness_sig/vtest/o"); + portIn.open("/harness_sig/vtest/i"); -class Thread2b:public Thread -{ - int value; -public: - Thread2b(Port *p) - { - portIn=p; - } + Network::connect("/harness_sig/vtest/o", "/harness_sig/vtest/i"); - void init(int val) - { - value = val; - } + portOut.enableBackgroundWrite(true); - bool threadInit() override - { - success=false; - return true; - } + VectorOf vector; + vector.resize(10); + for (unsigned int k = 0; k < vector.size(); k++) + { + vector[k] = k; + } - void run() override - { - VectorOf v; + bool success = true; + portOut.write(vector); - int times=10; - bool ok=true; - while(times--) - { - portIn->read(v); + VectorOf tmp; + portIn.read(tmp); - int k; - int s=(int)v.size(); - if (s!=10) - ok=false; - for(k=0;kread(v); - s=(int)v.size(); - if (s!=42) - ok=false; - for(k=0;k was sent and received correctly"); + portOut.interrupt(); + portOut.close(); + portIn.interrupt(); + portIn.close(); } - success=ok; - } - - Port *portIn; - bool success; -}; + report(0, "check VectorOf bottle compatibility"); + { + //write the same vector again and receive it as a bottle + Port portIn; + Port portOut; + bool success = true; -class Thread3b :public Thread -{ -public: - Thread3b(Port *p) - { - portIn = p; - } + portOut.open("/harness_sig/vtest/o"); + portIn.open("/harness_sig/vtest/i"); - bool threadInit() override - { - success = false; - return true; - } + Network::connect("/harness_sig/vtest/o", "/harness_sig/vtest/i"); - void run() override - { - Bottle v; + portOut.enableBackgroundWrite(true); - int times = 10; - bool ok = true; - while (times--) - { - portIn->read(v); - int k; - int s = (int)v.size(); - if (s != 10) - ok = false; - for (k = 0; k vector; + vector.resize(10); + for (unsigned int k = 0; k < vector.size(); k++) { - if (v.get(k).asInt32() != 42) - { - - ok = false; - } + vector[k] = k; } - - portIn->read(v); - - s = (int)v.size(); - if (s != 42) - ok = false; - for (k = 0; k < s; k++) + portOut.write(vector); + Bottle tmp2; + success = portIn.read(tmp2); + checkTrue(success,"correctly read from the port"); + + //compare vector and tmp + success = true; + if (tmp2.size() != (int)vector.size()) + { + success = false; + } + else { - if (v.get(k).asInt32() != k) + for (unsigned int k = 0; k < vector.size(); k++) { - - ok = false; + if (tmp2.get(k).asInt32() != vector[k]) + success = false; } } + checkTrue(success, "VectorOf was received correctly in a Bottle"); + portOut.interrupt(); + portOut.close(); + portIn.interrupt(); + portIn.close(); } - - success = ok; } - Port *portIn; - bool success; -}; - -class VectorOfTest : public UnitTest { - -public: - virtual std::string getName() const override { return "VectorOfTest"; } - void checkSendReceiveInt() + void testToString() { - report(0, "check VectorO send receive"); - - Port portIn; - Port portOut; - - portOut.open("/harness_sig/vtest/o"); - portIn.open("/harness_sig/vtest/i"); - - Network::connect("/harness_sig/vtest/o", "/harness_sig/vtest/i"); - - portOut.enableBackgroundWrite(true); - - - VectorOf vector; - vector.resize(10); - for (unsigned int k = 0; k < vector.size(); k++) - { - vector[k] = k; - } - - portOut.write(vector); - - VectorOf tmp; - portIn.read(tmp); - - //compare vector and tmp - bool success = true; - if (tmp.size() != vector.size()) { - success = false; - } - else - { - for (unsigned int k = 0; k < vector.size(); k++) + report(0, "testing toString int"); + bool ok = true; + VectorOf vec; + std::string strToCheck = "0 1 2 3 4 5 6 7 8 9"; + for (size_t i=0; i<10; i++) { - if (tmp[k] != vector[k]) - success = false; + vec.push_back(i); } - } - - checkTrue(success, "VectorOf was sent and received correctly"); - - report(0, "check VectorO bottle compatibility"); - //write the same vector again and receive it as a bottle - portOut.write(vector); - Bottle tmp2; - portIn.read(tmp2); - //compare vector and tmp - success = true; - if (tmp2.size() != vector.size()) - { - success = false; + ok = vec.toString() == strToCheck; + checkTrue(ok, "string correctly formatted"); } - else + { - for (unsigned int k = 0; k < vector.size(); k++) + report(0, "testing toString double"); + bool ok = true; + VectorOf vec; + std::string strToCheck = " 0.000000\t 1.000000\t 2.000000\t 3.000000\t 4.000000\t" + " 5.000000\t 6.000000\t 7.000000\t 8.000000\t 9.000000"; + for (size_t i=0; i<10; i++) { - if (tmp2.get(k).asInt32() != vector[k]) - success = false; + vec.push_back(i); } + + ok = vec.toString() == strToCheck; + checkTrue(ok, "string correctly formatted"); } - checkTrue(success, "VectorOf was received correctly in a Bottle"); } + virtual void runTests() override { Network::setLocalMode(true); checkSendReceiveInt(); + testToString(); Network::setLocalMode(false); } }; diff --git a/tests/yarpidl_thrift/demo/main.cpp b/tests/yarpidl_thrift/demo/main.cpp index 52473014a7..6a7b2f5746 100644 --- a/tests/yarpidl_thrift/demo/main.cpp +++ b/tests/yarpidl_thrift/demo/main.cpp @@ -407,25 +407,25 @@ bool test_partial() { msg.fromString("add pair 4"); client_port.write(msg,reply); printf("(incomplete) %s -> %s\n", msg.toString().c_str(), reply.toString().c_str()); - if (reply.get(0).asVocab() != VOCAB4('f','a','i','l')) return false; + if (reply.get(0).asVocab() != yarp::os::createVocab('f','a','i','l')) return false; msg.fromString("add pair"); reply.fromString("0"); client_port.write(msg,reply); printf("(incomplete) %s -> %s\n", msg.toString().c_str(), reply.toString().c_str()); - if (reply.get(0).asVocab() != VOCAB4('f','a','i','l')) return false; + if (reply.get(0).asVocab() != yarp::os::createVocab('f','a','i','l')) return false; msg.fromString("add"); reply.fromString("0"); client_port.write(msg,reply); printf("(incomplete) %s -> %s\n", msg.toString().c_str(), reply.toString().c_str()); - if (reply.get(0).asVocab() != VOCAB4('f','a','i','l')) return false; + if (reply.get(0).asVocab() != yarp::os::createVocab('f','a','i','l')) return false; msg.fromString(""); reply.fromString("0"); client_port.write(msg,reply); printf("(incomplete) %s -> %s\n", msg.toString().c_str(), reply.toString().c_str()); - if (reply.get(0).asVocab() != VOCAB4('f','a','i','l')) return false; + if (reply.get(0).asVocab() != yarp::os::createVocab('f','a','i','l')) return false; msg.fromString("add pair 10 20"); reply.fromString("0"); @@ -443,7 +443,7 @@ bool test_partial() { reply.fromString("0"); client_port.write(msg,reply); printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str()); - if (reply.get(0).asVocab() != VOCAB4('f','a','i','l')) return false; + if (reply.get(0).asVocab() != yarp::os::createVocab('f','a','i','l')) return false; return true; } @@ -477,7 +477,7 @@ bool test_defaults_with_rpc() { msg.fromString("test longer tail defaults"); client_port.write(msg,reply); printf("%s -> %s\n", msg.toString().c_str(), reply.toString().c_str()); - if (reply.get(0).asVocab() != VOCAB4('f','a','i','l')) return false; + if (reply.get(0).asVocab() != yarp::os::createVocab('f','a','i','l')) return false; msg.fromString("test longer tail defaults 888"); client_port.write(msg,reply);