From 994722a3ea573c880b7645329ca6dc2028d823cf Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Wed, 30 Aug 2023 17:58:48 +0200 Subject: [PATCH 1/4] the individual implementations of parameter `subdevice` have been removed from all yarp wrapper devices. It will be reintroduced at a different level shared by all devices. --- .../ControlBoardRemapper.cpp | 6 +- .../robotinterface_xml/ros.xml | 4 +- .../robotinterface_xml/ros2.xml | 8 +- .../robotinterface_xml/ros2b.xml | 8 +- .../robotinterface_xml/yarp.xml | 4 +- .../frameGrabberCropper.cpp | 26 ----- .../frameGrabberCropper/frameGrabberCropper.h | 3 - .../AnalogWrapper/AnalogWrapper.cpp | 88 ----------------- .../AnalogWrapper/AnalogWrapper.h | 10 -- .../JoypadControlServer.cpp | 87 +---------------- .../JoypadControlServer/JoypadControlServer.h | 3 - .../RgbdSensor_nws_yarp.cpp | 83 +--------------- .../RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.h | 11 --- .../Rangefinder2D_nws_yarp.cpp | 22 +---- .../Rangefinder2D_nws_yarp.h | 3 - .../audioPlayerWrapper/AudioPlayerWrapper.cpp | 28 ------ .../audioPlayerWrapper/AudioPlayerWrapper.h | 3 - .../AudioRecorderWrapper.cpp | 21 ----- .../AudioRecorderWrapper.h | 2 - .../ControlBoard_nws_yarp.cpp | 94 +++---------------- .../ControlBoard_nws_yarp.h | 3 - .../FrameGrabber_nws_yarp.cpp | 40 +------- .../FrameGrabber_nws_yarp.h | 5 - .../MobileBaseVelocityControl_nws_yarp.cpp | 27 +----- .../MobileBaseVelocityControl_nws_yarp.h | 2 - .../MultipleAnalogSensorsServer.cpp | 38 -------- .../MultipleAnalogSensorsServer.h | 4 - 27 files changed, 37 insertions(+), 596 deletions(-) diff --git a/src/devices/controlBoardRemapper/ControlBoardRemapper.cpp b/src/devices/controlBoardRemapper/ControlBoardRemapper.cpp index 6d717437514..dd310dc0b20 100644 --- a/src/devices/controlBoardRemapper/ControlBoardRemapper.cpp +++ b/src/devices/controlBoardRemapper/ControlBoardRemapper.cpp @@ -308,7 +308,7 @@ bool ControlBoardRemapper::attachAllUsingAxesNames(const PolyDriverList& polylis if( !iencs || !iaxinfos ) { - yCError(CONTROLBOARDREMAPPER) << "subdevice" << deviceKey << "does not implemented the required IAxisInfo or IEncoders interfaces"; + yCError(CONTROLBOARDREMAPPER) << "sub-device" << deviceKey << "does not implemented the required IAxisInfo or IEncoders interfaces"; return false; } @@ -317,7 +317,7 @@ bool ControlBoardRemapper::attachAllUsingAxesNames(const PolyDriverList& polylis if( !ok ) { - yCError(CONTROLBOARDREMAPPER) << "subdevice" << deviceKey << "does not implemented the required getAxes method"; + yCError(CONTROLBOARDREMAPPER) << "sub-device" << deviceKey << "does not implemented the required getAxes method"; return false; } @@ -330,7 +330,7 @@ bool ControlBoardRemapper::attachAllUsingAxesNames(const PolyDriverList& polylis if( !ok ) { - yCError(CONTROLBOARDREMAPPER) << "subdevice" << deviceKey << "does not implemented the required getAxisName method"; + yCError(CONTROLBOARDREMAPPER) << "sub-device" << deviceKey << "does not implemented the required getAxisName method"; return false; } diff --git a/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/ros.xml b/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/ros.xml index e40b026ccf1..0ebde68b60e 100644 --- a/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/ros.xml +++ b/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/ros.xml @@ -16,7 +16,7 @@ 0.01 - lidarmotor + lidarmotor @@ -28,7 +28,7 @@ 0.01 - lidarmotor + lidarmotor diff --git a/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/ros2.xml b/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/ros2.xml index 4775272ee47..72d8132d398 100644 --- a/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/ros2.xml +++ b/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/ros2.xml @@ -15,7 +15,7 @@ 0.01 - lidarmotor + lidarmotor @@ -26,7 +26,7 @@ 0.01 - lidarmotor + lidarmotor @@ -39,7 +39,7 @@ 0.01 - lidarmotor + lidarmotor @@ -51,7 +51,7 @@ 0.01 - lidarmotor + lidarmotor diff --git a/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/ros2b.xml b/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/ros2b.xml index 1e3f8f1ad4b..179db51c1a1 100644 --- a/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/ros2b.xml +++ b/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/ros2b.xml @@ -15,7 +15,7 @@ 0.01 - lidarmotor + lidarmotor @@ -26,7 +26,7 @@ 0.01 - lidarmotor + lidarmotor @@ -51,7 +51,7 @@ 0.01 - lidarmotor + lidarmotor @@ -65,7 +65,7 @@ 0.01 - lidarmotor + lidarmotor diff --git a/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/yarp.xml b/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/yarp.xml index 102e224922e..5a6d3d1a4ac 100644 --- a/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/yarp.xml +++ b/src/devices/fake/fakeLaserWithMotor/robotinterface_xml/yarp.xml @@ -15,7 +15,7 @@ 0.01 - lidarmotor + lidarmotor @@ -26,7 +26,7 @@ 0.01 - lidarmotor + lidarmotor diff --git a/src/devices/frameGrabberCropper/frameGrabberCropper.cpp b/src/devices/frameGrabberCropper/frameGrabberCropper.cpp index c9873034568..d9d4da0417f 100644 --- a/src/devices/frameGrabberCropper/frameGrabberCropper.cpp +++ b/src/devices/frameGrabberCropper/frameGrabberCropper.cpp @@ -59,38 +59,12 @@ bool FrameGrabberCropper::open(yarp::os::Searchable& config) forwardRgbVisualParams = true; } - if (config.check("subdevice")) { - yarp::os::Property p; - subdevice = new yarp::dev::PolyDriver; - p.fromString(config.toString()); - p.unput("device"); - p.put("device", config.find("subdevice").asString()); // subdevice was already checked before - - // if errors occurred during open, quit here. - if (!subdevice->open(p) || !(subdevice->isValid())) { - yCError(FRAMEGRABBERCROPPER, "Could not open subdevice"); - return false; - } - - if (!attach(subdevice)) { - yCError(FRAMEGRABBERCROPPER, "Could not attach subdevice"); - subdevice->close(); - return false; - } - subdeviceOwned = true; - } - return true; } bool FrameGrabberCropper::close() { - if (subdeviceOwned) { - subdevice->close(); - delete subdevice; - subdevice = nullptr; - } return true; } diff --git a/src/devices/frameGrabberCropper/frameGrabberCropper.h b/src/devices/frameGrabberCropper/frameGrabberCropper.h index 8e550aa6979..52ff2f4616d 100644 --- a/src/devices/frameGrabberCropper/frameGrabberCropper.h +++ b/src/devices/frameGrabberCropper/frameGrabberCropper.h @@ -66,9 +66,6 @@ class FrameGrabberCropper : public yarp::dev::IRgbVisualParams, public yarp::dev::IPreciselyTimed { - yarp::dev::PolyDriver* subdevice{nullptr}; - bool subdeviceOwned{false}; - yarp::dev::IFrameGrabberControls* iFrameGrabberControls{nullptr}; yarp::dev::IFrameGrabberControlsDC1394* iFrameGrabberControlsDC1394{nullptr}; yarp::dev::IRgbVisualParams* iRgbVisualParams{nullptr}; diff --git a/src/devices/networkWrappers/AnalogWrapper/AnalogWrapper.cpp b/src/devices/networkWrappers/AnalogWrapper/AnalogWrapper.cpp index 9d44cfbbbfa..d7184225e2e 100644 --- a/src/devices/networkWrappers/AnalogWrapper/AnalogWrapper.cpp +++ b/src/devices/networkWrappers/AnalogWrapper/AnalogWrapper.cpp @@ -247,69 +247,12 @@ void AnalogWrapper::removeHandlers() handlers.clear(); } -bool AnalogWrapper::openAndAttachSubDevice(Searchable &prop) -{ - Property p; - subDeviceOwned = new PolyDriver; - p.fromString(prop.toString()); - -// p.setMonitor(prop.getMonitor(), "subdevice"); // pass on any monitoring - p.unput("device"); - p.put("device", prop.find("subdevice").asString()); // subdevice was already checked before - - // if errors occurred during open, quit here. - yCDebug(ANALOGWRAPPER, "opening AnalogWrapper subdevice..."); - subDeviceOwned->open(p); - - if (!subDeviceOwned->isValid()) - { - yCError(ANALOGWRAPPER, "opening AnalogWrapper subdevice... FAILED\n"); - return false; - } - - subDeviceOwned->view(analogSensor_p); - - if (analogSensor_p == nullptr) - { - yCError(ANALOGWRAPPER, "Opening IAnalogSensor interface of AnalogWrapper subdevice... FAILED\n"); - return false; - } - - int chNum = analogSensor_p->getChannels(); - - if (chNum <= 0) - { - yCError(ANALOGWRAPPER, "Calling analog sensor has invalid channels number %d.\n", chNum); - return false; - } - - attach(analogSensor_p); - PeriodicThread::setPeriod(_rate / 1000.0); - return PeriodicThread::start(); -} - - -bool AnalogWrapper::openDeferredAttach(yarp::os::Searchable &prop) -{ - // nothing to do here? - if ((subDeviceOwned != nullptr) || (ownDevices == true)) { - yCError(ANALOGWRAPPER) << "AnalogWrapper: something wrong with the initialization."; - } - return true; -} - - /** * Specify which analog sensor this thread has to read from. */ bool AnalogWrapper::attachAll(const PolyDriverList &analog2attach) { - //check if we already instantiated a subdevice previously - if (ownDevices) { - return false; - } - if (analog2attach.size() != 1) { yCError(ANALOGWRAPPER, "AnalogWrapper: cannot attach more than one device"); @@ -335,11 +278,6 @@ bool AnalogWrapper::attachAll(const PolyDriverList &analog2attach) bool AnalogWrapper::detachAll() { - //check if we already instantiated a subdevice previously - if (ownDevices) { - return false; - } - analogSensor_p = nullptr; for(unsigned int i=0; iclose(); - delete subDeviceOwned; - subDeviceOwned = nullptr; - } - return true; } diff --git a/src/devices/networkWrappers/AnalogWrapper/AnalogWrapper.h b/src/devices/networkWrappers/AnalogWrapper/AnalogWrapper.h index 88436e665c0..beb1e2f4962 100644 --- a/src/devices/networkWrappers/AnalogWrapper/AnalogWrapper.h +++ b/src/devices/networkWrappers/AnalogWrapper/AnalogWrapper.h @@ -53,7 +53,6 @@ class AnalogPortEntry; * |:--------------:|:--------------:|:-------:|:--------------:|:-------------:|:--------------------------: |:-----------------------------------------------------------------:|:-----:| * | name | - | string | - | - | Yes | full name of the port opened by the device, like /robotName/part/ | MUST start with a '/' character | * | period | - | int | ms | 20 | No | refresh period of the broadcasted values in ms | optional, default 20ms | - * | subdevice | - | string | - | - | alternative to netwok group | name of the subdevice to instantiate | when used, parameters for the subdevice must be provided as well | * | ports | - | group | - | - | alternative to subdevice | this is expected to be a group parameter in xml format, a list in .ini file format. SubParameter are mandatory if this is used| - | * | - | portName_1 | 4 * int | channel number | - | if ports is used | describe how to match subdevice_1 channels with the wrapper channels. First 2 numbers indicate first/last wrapper channel, last 2 numbers are subdevice first/last channel | The channels are intended to be consequent | * | - | ... | 4 * int | channel number | - | if ports is used | same as above | The channels are intended to be consequent | @@ -162,15 +161,6 @@ class AnalogWrapper : int _rate{DEFAULT_THREAD_PERIOD}; std::string sensorId; - bool ownDevices{false}; - // Open the wrapper only, the attach method needs to be called before using it - bool openDeferredAttach(yarp::os::Searchable &prop); - - // For the simulator, if a subdevice parameter is given to the wrapper, it will - // open it and attach to it immediately. - yarp::dev::PolyDriver *subDeviceOwned{nullptr}; - bool openAndAttachSubDevice(yarp::os::Searchable &prop); - bool initialize_YARP(yarp::os::Searchable &config); void setHandlers(); void removeHandlers(); diff --git a/src/devices/networkWrappers/JoypadControlServer/JoypadControlServer.cpp b/src/devices/networkWrappers/JoypadControlServer/JoypadControlServer.cpp index 48455531008..dca6f94071c 100644 --- a/src/devices/networkWrappers/JoypadControlServer/JoypadControlServer.cpp +++ b/src/devices/networkWrappers/JoypadControlServer/JoypadControlServer.cpp @@ -224,8 +224,6 @@ bool JoypadCtrlParser::respond(const yarp::os::Bottle& cmd, yarp::os::Bottle& re JoypadControlServer::JoypadControlServer() : PeriodicThread(DEFAULT_THREAD_PERIOD), m_period(DEFAULT_THREAD_PERIOD), m_IJoypad(nullptr), - m_subDeviceOwned(nullptr), - m_isSubdeviceOwned(false), m_separatePorts(false), m_profile(false), m_coordsMode(yarp::dev::IJoypadController::JoypadCtrl_coordinateMode::JypCtrlcoord_POLAR) @@ -235,11 +233,6 @@ JoypadControlServer::JoypadControlServer() : PeriodicThread(DEFAULT_THREAD_PERIO JoypadControlServer::~JoypadControlServer() { - if(m_subDeviceOwned) - { - delete m_subDeviceOwned; - } - m_subDeviceOwned = nullptr; m_IJoypad = nullptr; } @@ -301,60 +294,10 @@ bool JoypadControlServer::open(yarp::os::Searchable& params) m_portTrackball.name = rootName + "/trackball:o"; m_portHats.name = rootName + "/hat:o"; - - // check if we need to create subdevice or if they are - // passed later on thorugh attachAll() - if(params.check("subdevice")) - { - m_isSubdeviceOwned=true; - if(!openAndAttachSubDevice(params)) - { - yCError(JOYPADCONTROLSERVER) << "Error while opening subdevice"; - return false; - } - } - else - { - m_isSubdeviceOwned=false; - } - + yCInfo(JOYPADCONTROLSERVER) << "Running, waiting for attach..."; return true; } -bool JoypadControlServer::openAndAttachSubDevice(Searchable& prop) -{ - Property p; - - m_subDeviceOwned = new PolyDriver; - - p.fromString(prop.toString()); - p.unput("device"); - p.put("device",prop.find("subdevice").asString()); // subdevice was already checked before - - // if errors occurred during open, quit here. - m_subDeviceOwned->open(p); - - if (!m_subDeviceOwned->isValid()) - { - yCError(JOYPADCONTROLSERVER) << "Opening subdevice... FAILED"; - return false; - } - m_isSubdeviceOwned = true; - if (!attach(m_subDeviceOwned)) { - return false; - } - - if(!m_parser.configure(m_IJoypad) ) - { - yCError(JOYPADCONTROLSERVER) << "Error configuring interfaces for parsers"; - return false; - } - - openPorts(); - PeriodicThread::setPeriod(m_period); - return PeriodicThread::start(); -} - bool JoypadControlServer::attach(PolyDriver* poly) { if (poly) { @@ -382,28 +325,12 @@ bool JoypadControlServer::attach(PolyDriver* poly) return true; } -bool JoypadControlServer::attach(yarp::dev::IJoypadController *s) -{ - if(s == nullptr) - { - yCError(JOYPADCONTROLSERVER) << "Attached device has no valid IJoystickController interface."; - return false; - } - m_IJoypad = s; - return true; -} - bool JoypadControlServer::detach() { if (yarp::os::PeriodicThread::isRunning()) { yarp::os::PeriodicThread::stop(); } - //check if we already instantiated a subdevice previously - if (m_isSubdeviceOwned) { - return false; - } - m_IJoypad = nullptr; return true; } @@ -822,18 +749,6 @@ bool JoypadControlServer::close() { detachAll(); - // close subdevice if it was created inside the open (--subdevice option) - if(m_isSubdeviceOwned) - { - if (m_subDeviceOwned) { - m_subDeviceOwned->close(); - } - - m_subDeviceOwned = nullptr; - m_IJoypad = nullptr; - m_isSubdeviceOwned = false; - } - // Closing port std::vector portv; portv.push_back(&m_portButtons); diff --git a/src/devices/networkWrappers/JoypadControlServer/JoypadControlServer.h b/src/devices/networkWrappers/JoypadControlServer/JoypadControlServer.h index fa03b23f2f0..2259132d673 100644 --- a/src/devices/networkWrappers/JoypadControlServer/JoypadControlServer.h +++ b/src/devices/networkWrappers/JoypadControlServer/JoypadControlServer.h @@ -55,8 +55,6 @@ class JoypadControlServer : JoypadCtrlParser m_parser; yarp::dev::IJoypadController* m_IJoypad; yarp::os::Port m_rpcPort; - yarp::dev::PolyDriver* m_subDeviceOwned; - bool m_isSubdeviceOwned; bool m_separatePorts; bool m_profile; std::string m_rpcPortName; @@ -87,7 +85,6 @@ class JoypadControlServer : bool fromConfig(yarp::os::Searchable& params); bool close() override; bool attach(yarp::dev::PolyDriver* poly) override; - bool attach(yarp::dev::IJoypadController* s); bool detach() override; bool threadInit() override; void threadRelease() override; diff --git a/src/devices/networkWrappers/RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.cpp b/src/devices/networkWrappers/RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.cpp index 8530cc5182a..cc1e48ceb00 100644 --- a/src/devices/networkWrappers/RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.cpp +++ b/src/devices/networkWrappers/RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.cpp @@ -172,9 +172,7 @@ RgbdSensor_nws_yarp::RgbdSensor_nws_yarp() : sensor_p(nullptr), fgCtrl(nullptr), sensorStatus(IRGBDSensor::RGBD_SENSOR_NOT_READY), - verbose(4), - isSubdeviceOwned(false), - subDeviceOwned(nullptr) + verbose(4) {} RgbdSensor_nws_yarp::~RgbdSensor_nws_yarp() @@ -210,23 +208,6 @@ bool RgbdSensor_nws_yarp::open(yarp::os::Searchable &config) return false; } - // check if we need to create subdevice or if they are - // passed later on through attachAll() - if(isSubdeviceOwned) - { - if(! openAndAttachSubDevice(config)) - { - yCError(RGBDSENSORNWSYARP, "Error while opening subdevice"); - return false; - } - } - else - { - if (!openDeferredAttach(config)) { - return false; - } - } - return true; } @@ -255,45 +236,6 @@ bool RgbdSensor_nws_yarp::fromConfig(yarp::os::Searchable &config) colorFrame_StreamingPort_Name = rootName + "/rgbImage:o"; depthFrame_StreamingPort_Name = rootName + "/depthImage:o"; - if(config.check("subdevice")) { - isSubdeviceOwned=true; - } else { - isSubdeviceOwned=false; - } - - return true; -} - -bool RgbdSensor_nws_yarp::openDeferredAttach(Searchable& prop) -{ - // I dunno what to do here now... - isSubdeviceOwned = false; - return true; -} - -bool RgbdSensor_nws_yarp::openAndAttachSubDevice(Searchable& prop) -{ - Property p; - subDeviceOwned = new PolyDriver; - p.fromString(prop.toString()); - - p.unput("device"); - p.put("device",prop.find("subdevice").asString()); // subdevice was already checked before - - // if errors occurred during open, quit here. - yCDebug(RGBDSENSORNWSYARP, "Opening IRGBDSensor subdevice"); - subDeviceOwned->open(p); - - if (!subDeviceOwned->isValid()) - { - yCError(RGBDSENSORNWSYARP, "Opening IRGBDSensor subdevice... FAILED"); - return false; - } - isSubdeviceOwned = true; - if(!attach(subDeviceOwned)) { - return false; - } - return true; } @@ -302,21 +244,7 @@ bool RgbdSensor_nws_yarp::close() yCTrace(RGBDSENSORNWSYARP, "Close"); detach(); - // close subdevice if it was created inside the open (--subdevice option) - if(isSubdeviceOwned) - { - if(subDeviceOwned) - { - delete subDeviceOwned; - subDeviceOwned=nullptr; - } - sensor_p = nullptr; - fgCtrl = nullptr; - isSubdeviceOwned = false; - } - // Closing port - rpcPort.interrupt(); colorFrame_StreamingPort.interrupt(); depthFrame_StreamingPort.interrupt(); @@ -366,11 +294,6 @@ bool RgbdSensor_nws_yarp::detach() yarp::os::PeriodicThread::stop(); } - //check if we already instantiated a subdevice previously - if (isSubdeviceOwned) { - return false; - } - sensor_p = nullptr; return true; } @@ -528,7 +451,7 @@ bool RgbdSensor_nws_yarp::writeData() else { oldDepthStamp = depthStamp; } // TBD: We should check here somehow if the timestamp was correctly updated and, if not, update it ourselves. - if (rgb_data_ok && colorFrame_StreamingPort.getOutputCount() > 0) + if (rgb_data_ok) { FlexImage& yColorImage = colorFrame_StreamingPort.prepare(); yColorImage.setPixelCode(colorImage.getPixelCode()); @@ -537,7 +460,7 @@ bool RgbdSensor_nws_yarp::writeData() colorFrame_StreamingPort.setEnvelope(colorStamp); colorFrame_StreamingPort.write(); } - if (depth_data_ok && depthFrame_StreamingPort.getOutputCount() > 0) + if (depth_data_ok) { ImageOf& yDepthImage = depthFrame_StreamingPort.prepare(); yDepthImage.setQuantum(depthImage.getQuantum()); diff --git a/src/devices/networkWrappers/RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.h b/src/devices/networkWrappers/RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.h index 08352108d2d..ad43081736b 100644 --- a/src/devices/networkWrappers/RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.h +++ b/src/devices/networkWrappers/RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.h @@ -79,7 +79,6 @@ class RGBDSensorParser : * |:--------------:|:-----------------------:|:-------:|:--------------:|:-------------:|:-----------------------------: |:---------------------------------------------------------------------------------------------------:|:-----:| * | period | - | double | s | 0.02 | No | refresh period of the broadcasted values in s | default 0.02s | * | name | - | string | - | - | Yes | Prefix name of the ports opened by the RGBD wrapper, e.g. /robotName/RGBD | Required suffix like '/rpc' will be added by the device | - * | subdevice | - | string | - | - | alternative to 'attach' action | name of the subdevice to use as a data source | when used, parameters for the subdevice must be provided as well | * * Some example of configuration files: * @@ -148,16 +147,6 @@ class RgbdSensor_nws_yarp : int verbose; bool initialize_YARP(yarp::os::Searchable& config); - // Open the wrapper only, the attach method needs to be called before using it - // Typical usage: yarprobotinterface - bool openDeferredAttach(yarp::os::Searchable& prop); - - // If a subdevice parameter is given, the wrapper will open it and attach to immediately. - // Typical usage: simulator or command line - bool isSubdeviceOwned; - yarp::dev::PolyDriver* subDeviceOwned; - bool openAndAttachSubDevice(yarp::os::Searchable& prop); - // Synch yarp::os::Stamp colorStamp; yarp::os::Stamp depthStamp; diff --git a/src/devices/networkWrappers/Rangefinder2D_nws_yarp/Rangefinder2D_nws_yarp.cpp b/src/devices/networkWrappers/Rangefinder2D_nws_yarp/Rangefinder2D_nws_yarp.cpp index 9d33325168d..a3a30987445 100644 --- a/src/devices/networkWrappers/Rangefinder2D_nws_yarp/Rangefinder2D_nws_yarp.cpp +++ b/src/devices/networkWrappers/Rangefinder2D_nws_yarp/Rangefinder2D_nws_yarp.cpp @@ -33,8 +33,7 @@ YARP_LOG_COMPONENT(RANGEFINDER2D_NWS_YARP, "yarp.devices.Rangefinder2D_nws_yarp" maxAngle(0), minDistance(0), maxDistance(0), - resolution(0), - isDeviceOwned(false) + resolution(0) {} Rangefinder2D_nws_yarp::~Rangefinder2D_nws_yarp() @@ -325,25 +324,6 @@ bool Rangefinder2D_nws_yarp::open(yarp::os::Searchable &config) return false; } - if(config.check("subdevice")) - { - Property p; - p.fromString(config.toString(), false); - p.put("device", config.find("subdevice").asString()); - - if(!m_driver.open(p) || !m_driver.isValid()) - { - yCError(RANGEFINDER2D_NWS_YARP) << "failed to open subdevice.. check params"; - return false; - } - - if(!attach(&m_driver)) - { - yCError(RANGEFINDER2D_NWS_YARP) << "failed to open subdevice.. check params"; - return false; - } - isDeviceOwned = true; - } return true; } diff --git a/src/devices/networkWrappers/Rangefinder2D_nws_yarp/Rangefinder2D_nws_yarp.h b/src/devices/networkWrappers/Rangefinder2D_nws_yarp/Rangefinder2D_nws_yarp.h index 7d8f3b74c39..a67e975d126 100644 --- a/src/devices/networkWrappers/Rangefinder2D_nws_yarp/Rangefinder2D_nws_yarp.h +++ b/src/devices/networkWrappers/Rangefinder2D_nws_yarp/Rangefinder2D_nws_yarp.h @@ -48,7 +48,6 @@ * |:--------------:|:-----------------------:|:-------:|:--------------:|:-------------:|:-----------------------------: |:---------------------------------------------------------------------------------------------------:|:-----:| * | period | - | double | s | 0.02 | No | refresh period of the broadcasted values in s | default 0.02s | * | name | - | string | - | - | Yes | Prefix name of the ports opened by the wrapper, e.g. /robotName/Rangefinder2DSensor | Required suffix like '/rpc' will be added by the device | - * | subdevice | - | string | - | - | alternative to 'attach' action | name of the subdevice to use as a data source | when used, parameters for the subdevice must be provided as well | * | frame_id | - | string | - | - | No | name of the attached frame | Currently not used, reserved for future use | * * Example of configuration file using .ini format. @@ -91,7 +90,6 @@ class Rangefinder2D_nws_yarp : yarp::os::BufferedPort streamingPort; //interfaces - yarp::dev::PolyDriver m_driver; yarp::dev::IRangefinder2D *sens_p; //device data @@ -100,7 +98,6 @@ class Rangefinder2D_nws_yarp : double minAngle, maxAngle; double minDistance, maxDistance; double resolution; - bool isDeviceOwned; //private methods bool initialize_YARP(yarp::os::Searchable &config); diff --git a/src/devices/networkWrappers/audioPlayerWrapper/AudioPlayerWrapper.cpp b/src/devices/networkWrappers/audioPlayerWrapper/AudioPlayerWrapper.cpp index 1d79873dd55..73ebef932ce 100644 --- a/src/devices/networkWrappers/audioPlayerWrapper/AudioPlayerWrapper.cpp +++ b/src/devices/networkWrappers/audioPlayerWrapper/AudioPlayerWrapper.cpp @@ -194,30 +194,6 @@ bool AudioPlayerWrapper::open(yarp::os::Searchable &config) yCInfo(AUDIOPLAYERWRAPPER) << "Using a 'playback_network_buffer_size' of" << m_buffer_delay << "s"; yCInfo(AUDIOPLAYERWRAPPER) << "Increase this value to robustify the real-time audio stream (it will increase latency too)"; - if(config.check("subdevice")) - { - Property p; - p.fromString(config.toString(), false); - p.put("device", config.find("subdevice").asString()); - - if(!m_driver.open(p) || !m_driver.isValid()) - { - yCError(AUDIOPLAYERWRAPPER) << "Failed to open subdevice.. check params"; - return false; - } - if(!attach(&m_driver)) - { - yCError(AUDIOPLAYERWRAPPER) << "Failed to open subdevice.. check params"; - return false; - } - m_isDeviceOwned = true; - if (m_irender == nullptr) - { - yCError(AUDIOPLAYERWRAPPER, "m_irender is null\n"); - return false; - } - } - return true; } @@ -325,9 +301,5 @@ bool AudioPlayerWrapper::close() PeriodicThread::stop(); } - if(m_config.check("subdevice")) - { - detach(); - } return true; } diff --git a/src/devices/networkWrappers/audioPlayerWrapper/AudioPlayerWrapper.h b/src/devices/networkWrappers/audioPlayerWrapper/AudioPlayerWrapper.h index cfa784b4ede..53a9fc05e49 100644 --- a/src/devices/networkWrappers/audioPlayerWrapper/AudioPlayerWrapper.h +++ b/src/devices/networkWrappers/audioPlayerWrapper/AudioPlayerWrapper.h @@ -86,8 +86,6 @@ class AudioPlayerWrapper : void run() override; private: - yarp::dev::PolyDriver m_driver; - std::string m_rpcPortName; yarp::os::Port m_rpcPort; std::string m_audioInPortName; @@ -103,7 +101,6 @@ class AudioPlayerWrapper : std::queue m_sound_buffer; double m_period; double m_buffer_delay; - bool m_isDeviceOwned = false; bool m_debug_enabled = false; bool m_isPlaying = false; diff --git a/src/devices/networkWrappers/audioRecorderWrapper/AudioRecorderWrapper.cpp b/src/devices/networkWrappers/audioRecorderWrapper/AudioRecorderWrapper.cpp index 17286ddf79f..3ae0e61e1e7 100644 --- a/src/devices/networkWrappers/audioRecorderWrapper/AudioRecorderWrapper.cpp +++ b/src/devices/networkWrappers/audioRecorderWrapper/AudioRecorderWrapper.cpp @@ -90,27 +90,6 @@ bool AudioRecorderWrapper::open(yarp::os::Searchable& config) } m_rpcPort.setReader(*this); - // Subdevice check and initialization - if (config.check("subdevice")) - { - yarp::os::Property p; - p.fromString(config.toString(), false); - p.put("device", config.find("subdevice").asString()); - - if (!m_driver.open(p) || !m_driver.isValid()) - { - yCError(AUDIORECORDERWRAPPER) << "Failed to open subdevice.. check params"; - return false; - } - - if (!attach(&m_driver)) - { - yCError(AUDIORECORDERWRAPPER) << "Failed to open subdevice.. check params"; - return false; - } - m_isDeviceOwned = true; - } - return true; } diff --git a/src/devices/networkWrappers/audioRecorderWrapper/AudioRecorderWrapper.h b/src/devices/networkWrappers/audioRecorderWrapper/AudioRecorderWrapper.h index 9c56336b37a..52e930aeb74 100644 --- a/src/devices/networkWrappers/audioRecorderWrapper/AudioRecorderWrapper.h +++ b/src/devices/networkWrappers/audioRecorderWrapper/AudioRecorderWrapper.h @@ -49,7 +49,6 @@ class AudioRecorderWrapper : public yarp::os::PortReader { private: - yarp::dev::PolyDriver m_driver; yarp::dev::IAudioGrabberSound* m_mic = nullptr; //The microphone device yarp::os::Property m_config; double m_period; @@ -60,7 +59,6 @@ class AudioRecorderWrapper : size_t m_min_number_of_samples_over_network; size_t m_max_number_of_samples_over_network; double m_getSound_timeout; - bool m_isDeviceOwned =false; bool m_isRecording=false; AudioRecorderStatusThread* m_statusThread = nullptr; AudioRecorderDataThread* m_dataThread =nullptr; diff --git a/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.cpp b/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.cpp index d98440dcf11..5f373cfa067 100644 --- a/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.cpp +++ b/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.cpp @@ -111,16 +111,6 @@ bool ControlBoard_nws_yarp::open(Searchable& config) period = default_period; } - // Check if we need to create subdevice or if they are - // passed later on through attachAll() - if (prop.check("subdevice")) { - if (!openAndAttachSubDevice(prop)) { - yCError(CONTROLBOARD, "Error while opening subdevice"); - return false; - } - subdevice_ready = true; - } - rootName = prop.check("rootName", Value("/"), "starting '/' if needed.").asString(); partName = prop.check("name", Value("controlboard"), "prefix for port names").asString(); rootName += (partName); @@ -173,37 +163,10 @@ bool ControlBoard_nws_yarp::open(Searchable& config) return true; } - -// For the simulator, if a subdevice parameter is given to the wrapper, it will -// open it and attach to immediately. -bool ControlBoard_nws_yarp::openAndAttachSubDevice(Property& prop) -{ - Property p; - auto* subDeviceOwned = new PolyDriver; - p.fromString(prop.toString()); - - std::string subdevice = prop.find("subdevice").asString(); - p.unput("device"); - p.put("device", subdevice); // subdevice was already checked before - - // if errors occurred during open, quit here. - yCDebug(CONTROLBOARD, "opening subdevice"); - subDeviceOwned->open(p); - - if (!subDeviceOwned->isValid()) { - yCError(CONTROLBOARD, "opening subdevice... FAILED"); - return false; - } - - return setDevice(subDeviceOwned, true); -} - - bool ControlBoard_nws_yarp::setDevice(yarp::dev::DeviceDriver* driver, bool owned) { // Save the pointer and subDeviceOwned - subdevice_ptr = driver; - subdevice_owned = owned; + yarp::dev::DeviceDriver* subdevice_ptr = driver; // yarp::dev::IJointFault* iJointFault{nullptr}; subdevice_ptr->view(iJointFault); @@ -220,7 +183,8 @@ bool ControlBoard_nws_yarp::setDevice(yarp::dev::DeviceDriver* driver, bool owne // yarp::dev::IPositionControl* iPositionControl{nullptr}; subdevice_ptr->view(iPositionControl); if (!iPositionControl) { - yCWarning(CONTROLBOARD, "Part <%s>: IPositionControl interface was not found in subdevice.", partName.c_str()); + yCError(CONTROLBOARD, "Part <%s>: IPositionControl interface was not found in subdevice. Quitting", partName.c_str()); + return false; } // yarp::dev::IPositionDirect* iPositionDirect{nullptr}; @@ -232,13 +196,15 @@ bool ControlBoard_nws_yarp::setDevice(yarp::dev::DeviceDriver* driver, bool owne // yarp::dev::IVelocityControl* iVelocityControl{nullptr}; subdevice_ptr->view(iVelocityControl); if (!iVelocityControl) { - yCWarning(CONTROLBOARD, "Part <%s>: IVelocityControl interface was not found in subdevice.", partName.c_str()); + yCError(CONTROLBOARD, "Part <%s>: IVelocityControl interface was not found in subdevice. Quitting", partName.c_str()); + return false; } // yarp::dev::IEncodersTimed* iEncodersTimed{nullptr}; subdevice_ptr->view(iEncodersTimed); if (!iEncodersTimed) { - yCWarning(CONTROLBOARD, "Part <%s>: IEncodersTimed interface was not found in subdevice.", partName.c_str()); + yCError(CONTROLBOARD, "Part <%s>: IEncodersTimed interface was not found in subdevice. Quitting", partName.c_str()); + return false; } // yarp::dev::IMotor* iMotor{nullptr}; @@ -327,32 +293,11 @@ bool ControlBoard_nws_yarp::setDevice(yarp::dev::DeviceDriver* driver, bool owne // Get the number of controlled joints - int tmp_axes = 0; - if (iAxisInfo) - { - if (!iAxisInfo->getAxes(&tmp_axes)) { - yCError(CONTROLBOARD) << "Part <%s>: iAxisInfo->getAxes() failed for subdevice " << partName.c_str(); - return false;} - } - else if (iEncodersTimed) - { - if (!iEncodersTimed->getAxes(&tmp_axes)) { - yCError(CONTROLBOARD) << "Part <%s>: iEncodersTimed->getAxes() failed for subdevice " << partName.c_str(); - return false;} - } - else if (iPositionControl) - { - if (!iPositionControl->getAxes(&tmp_axes)) { - yCError(CONTROLBOARD) << "Part <%s>: iPositionControl->getAxes() failed for subdevice " << partName.c_str(); - return false;} - } - else if(iVelocityControl) - { - if (!iVelocityControl->getAxes(&tmp_axes)) { - yCError(CONTROLBOARD) << "Part <%s>: iVelocityControl->getAxes() failed for subdevice " << partName.c_str(); - return false;} + int tmp_axes; + if (!iPositionControl->getAxes(&tmp_axes)) { + yCError(CONTROLBOARD) << "Part <%s>: Failed to get axes number for subdevice " << partName.c_str(); + return false; } - if (tmp_axes <= 0) { yCError(CONTROLBOARD, "Part <%s>: attached device has an invalid number of joints (%d)", partName.c_str(), tmp_axes); return false; @@ -376,13 +321,6 @@ void ControlBoard_nws_yarp::closeDevice() streaming_parser.reset(); RPC_parser.reset(); - // If the subdevice is owned, close and delete the device - if (subdevice_owned) { - subdevice_ptr->close(); - delete subdevice_ptr; - } - subdevice_ptr = nullptr; - subdevice_owned = false; subdevice_joints = 0; subdevice_ready = false; @@ -433,11 +371,6 @@ bool ControlBoard_nws_yarp::attach(yarp::dev::PolyDriver* poly) bool ControlBoard_nws_yarp::detach() { - //check if we already instantiated a subdevice previously - if (subdevice_owned) { - return false; - } - // Ensure that the device is not running if (isRunning()) { stop(); @@ -453,7 +386,7 @@ void ControlBoard_nws_yarp::run() // check we are not overflowing with input messages constexpr int reads_for_warning = 20; if (inputStreamingPort.getPendingReads() >= reads_for_warning) { - yCIWarning(CONTROLBOARD, id()) << "Number of streaming input messages to be read is" << inputStreamingPort.getPendingReads() << "and can overflow"; + yCWarning(CONTROLBOARD) << "Number of streaming input messages to be read is " << inputStreamingPort.getPendingReads() << " and can overflow"; } // handle stateExt first @@ -529,7 +462,8 @@ void ControlBoard_nws_yarp::run() { if (std::abs(times[0] - tt) > 1.0) { - yCIErrorThrottle(CONTROLBOARD, id(), 1.0) << "Encoder timestamps are not consistent! Data will not be published."; + yCError(CONTROLBOARD, "Encoder Timestamps are not consistent! Data will not be published."); + yarp::sig::Vector _data(subdevice_joints); return; } } diff --git a/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.h b/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.h index 72b59d0fc73..20f5ded90da 100644 --- a/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.h +++ b/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.h @@ -56,7 +56,6 @@ * |:--------------:|:--------------:|:-------:|:--------------:|:-------------:|:--------------------------: |:-----------------------------------------------------------------:|:-----:| * | name | - | string | - | - | Yes | full name of the port opened by the device, like /robotName/part/ | MUST start with a '/' character | * | period | - | double | s | 0.02 | No | refresh period of the broadcasted values in s | optional, default 0.02s period| - * | subdevice | - | string | - | - | No | name of the subdevice to instantiate | when used, parameters for the subdevice must be provided as well | */ class ControlBoard_nws_yarp : @@ -87,8 +86,6 @@ class ControlBoard_nws_yarp : yarp::sig::Vector times; // time for each joint yarp::os::Stamp time; // envelope to attach to the state port - yarp::dev::DeviceDriver* subdevice_ptr{nullptr}; - bool subdevice_owned = false; size_t subdevice_joints {0}; bool subdevice_ready = false; diff --git a/src/devices/networkWrappers/frameGrabber_nws_yarp/FrameGrabber_nws_yarp.cpp b/src/devices/networkWrappers/frameGrabber_nws_yarp/FrameGrabber_nws_yarp.cpp index 21ad0b124b6..7b83246b0b0 100644 --- a/src/devices/networkWrappers/frameGrabber_nws_yarp/FrameGrabber_nws_yarp.cpp +++ b/src/devices/networkWrappers/frameGrabber_nws_yarp/FrameGrabber_nws_yarp.cpp @@ -51,14 +51,6 @@ bool FrameGrabber_nws_yarp::close() delete img_Raw; img_Raw = nullptr; - if (subdevice) { - subdevice->close(); - delete subdevice; - subdevice = nullptr; - } - - isSubdeviceOwned = false; - return true; } @@ -118,37 +110,7 @@ bool FrameGrabber_nws_yarp::open(yarp::os::Searchable& config) } pImg.setReader(*this); - - // Check "subdevice" option and eventually open the device - isSubdeviceOwned = config.check("subdevice"); - if (isSubdeviceOwned) { - yarp::os::Property p; - subdevice = new yarp::dev::PolyDriver; - p.fromString(config.toString()); - if (cap == COLOR) { - p.put("pixelType", VOCAB_PIXEL_RGB); - } else { - p.put("pixelType", VOCAB_PIXEL_MONO); - } - - p.unput("device"); - p.put("device", config.find("subdevice").asString()); // subdevice was already checked before - - // if errors occurred during open, quit here. - subdevice->open(p); - - if (!(subdevice->isValid())) { - yCError(FRAMEGRABBER_NWS_YARP, "Unable to open subdevice"); - return false; - } - if (!attach(subdevice)) { - yCError(FRAMEGRABBER_NWS_YARP, "Unable to attach subdevice"); - return false; - } - } else { - yCInfo(FRAMEGRABBER_NWS_YARP) << "Running, waiting for attach..."; - } - + yCInfo(FRAMEGRABBER_NWS_YARP) << "Running, waiting for attach..."; active = true; return true; diff --git a/src/devices/networkWrappers/frameGrabber_nws_yarp/FrameGrabber_nws_yarp.h b/src/devices/networkWrappers/frameGrabber_nws_yarp/FrameGrabber_nws_yarp.h index d2d16e0d0b4..98b9c606ced 100644 --- a/src/devices/networkWrappers/frameGrabber_nws_yarp/FrameGrabber_nws_yarp.h +++ b/src/devices/networkWrappers/frameGrabber_nws_yarp/FrameGrabber_nws_yarp.h @@ -45,7 +45,6 @@ * | period | - | double | s | 0.03 | No | refresh period (in s) of the broadcasted values through yarp ports | default 0.03s | * | name | - | string | - | /grabber | No | Prefix name of the ports opened by the FrameGrabber_nws_yarp | Required suffix like '/rpc' will be added by the device | * | capabilities | - | string | - | COLOR | No | two capabilities supported, COLOR and RAW respectively for rgb and raw streaming | - | - * | subdevice | - | string | - | - | No | name of the subdevice to use as a data source | when used, parameters for the subdevice must be provided as well; when not used, the device should be 'attached' | // FIXME DRDANZ no_drop? * * Some example of configuration files: @@ -92,10 +91,6 @@ class FrameGrabber_nws_yarp : yarp::os::RpcServer rpcPort; yarp::os::BufferedPort pImg; - // Subdevice - yarp::dev::PolyDriver* subdevice {nullptr}; - bool isSubdeviceOwned {false}; - // Interfaces handled yarp::dev::IRgbVisualParams* iRgbVisualParams {nullptr}; yarp::dev::IFrameGrabberImage* iFrameGrabberImage {nullptr}; diff --git a/src/devices/networkWrappers/mobileBaseVelocityControl_nws_yarp/MobileBaseVelocityControl_nws_yarp.cpp b/src/devices/networkWrappers/mobileBaseVelocityControl_nws_yarp/MobileBaseVelocityControl_nws_yarp.cpp index 11fefc5bd4f..cd5a7b6f3a1 100644 --- a/src/devices/networkWrappers/mobileBaseVelocityControl_nws_yarp/MobileBaseVelocityControl_nws_yarp.cpp +++ b/src/devices/networkWrappers/mobileBaseVelocityControl_nws_yarp/MobileBaseVelocityControl_nws_yarp.cpp @@ -82,30 +82,7 @@ bool MobileBaseVelocityControl_nws_yarp::open(yarp::os::Searchable &config) m_StreamingInput.useCallback(); } - //attach subdevice if required - if (config.check("subdevice")) - { - Property p; - p.fromString(config.toString(), false); - p.put("device", config.find("subdevice").asString()); - - if (!m_subdev.open(p) || !m_subdev.isValid()) - { - yCError(MOBVEL_NWS_YARP) << "Failed to open subdevice.. check params"; - return false; - } - - if (!attach(&m_subdev)) - { - yCError(MOBVEL_NWS_YARP) << "Failed to attach subdevice.. check params"; - return false; - } - } - else - { - yCInfo(MOBVEL_NWS_YARP) << "Waiting for device to attach"; - } - + yCInfo(MOBVEL_NWS_YARP) << "Waiting for device to attach"; return true; } @@ -113,7 +90,6 @@ bool MobileBaseVelocityControl_nws_yarp::close() { m_rpc_port_navigation_server.close(); m_StreamingInput.close(); - if (m_subdev.isValid()) { m_subdev.close(); } return true; } @@ -137,6 +113,7 @@ bool MobileBaseVelocityControl_nws_yarp::attach(PolyDriver* driver) } m_StreamingInput.m_iVel = m_iNavVel; + yCInfo(MOBVEL_NWS_YARP) << "Attach complete"; return true; } diff --git a/src/devices/networkWrappers/mobileBaseVelocityControl_nws_yarp/MobileBaseVelocityControl_nws_yarp.h b/src/devices/networkWrappers/mobileBaseVelocityControl_nws_yarp/MobileBaseVelocityControl_nws_yarp.h index d1c21851b57..ade4f6c3c76 100644 --- a/src/devices/networkWrappers/mobileBaseVelocityControl_nws_yarp/MobileBaseVelocityControl_nws_yarp.h +++ b/src/devices/networkWrappers/mobileBaseVelocityControl_nws_yarp/MobileBaseVelocityControl_nws_yarp.h @@ -33,7 +33,6 @@ * | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | Notes | * |:--------------:|:--------------:|:-------:|:-----:|:-------------:|:-------: |:-----------------------------------------------------------------:|:-----:| * | local | - | string | - | - | Yes | Full name of the port opened by the device. For both ports (i.e. /rpc:i, /data:i) the corresponding suffix is automatically added | | - * | subdevice | - | string | - | - | No | name of the subdevice to instantiate | when used, parameters for the subdevice must be provided as well | * * Example usage: * yarpdev --device mobileBaseVelocityControl_nws_yarp --subdevice velocityInputHandler --local /input1 @@ -61,7 +60,6 @@ class MobileBaseVelocityControl_nws_yarp: VelocityInputPortProcessor m_StreamingInput; std::string m_local_name; - yarp::dev::PolyDriver m_subdev; yarp::dev::Nav2D::INavigation2DVelocityActions* m_iNavVel = nullptr; public: diff --git a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp index 8c31feff95d..fa9af984a63 100644 --- a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp +++ b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp @@ -152,36 +152,8 @@ bool MultipleAnalogSensorsServer::open(yarp::os::Searchable& config) m_RPCPortName = name + "/rpc:o"; m_streamingPortName = name + "/measures:o"; - if (config.check("subdevice")) - { - std::string subdeviceName = config.find("subdevice").asString(); - - yarp::os::Property driverConfig; - driverConfig.fromString(config.toString()); - driverConfig.put("device", subdeviceName); - if (!m_subdevice.open(driverConfig)) - { - yCError(MULTIPLEANALOGSENSORSSERVER, "Opening subdevice failed."); - return false; - } - yarp::dev::PolyDriverList driverList; - driverList.push(&m_subdevice, subdeviceName.c_str()); - - if (!attachAll(driverList)) - { - yCError(MULTIPLEANALOGSENSORSSERVER, "Attaching subdevice failed."); - return false; - } - - yCInfo(MULTIPLEANALOGSENSORSSERVER, - "Subdevice \"%s\" successfully configured and attached.", - subdeviceName.c_str()); - m_isDeviceOwned = true; - } - - yCDebug(MULTIPLEANALOGSENSORSSERVER, "Open complete"); return true; } @@ -189,13 +161,6 @@ bool MultipleAnalogSensorsServer::close() { bool ok = this->detachAll(); - if (m_isDeviceOwned) - { - ok &= m_subdevice.close(); - m_isDeviceOwned = false; - } - - yCDebug(MULTIPLEANALOGSENSORSSERVER, "Close complete"); return ok; } @@ -449,7 +414,6 @@ bool MultipleAnalogSensorsServer::attachAll(const yarp::dev::PolyDriverList& p) if(!ok) { - yCError(MULTIPLEANALOGSENSORSSERVER, "Failure in populateAllSensorsMetadata()"); close(); return false; } @@ -489,7 +453,6 @@ bool MultipleAnalogSensorsServer::attachAll(const yarp::dev::PolyDriverList& p) return false; } - yCDebug(MULTIPLEANALOGSENSORSSERVER, "Attach complete"); return true; } @@ -504,7 +467,6 @@ bool MultipleAnalogSensorsServer::detachAll() m_rpcPort.close(); m_streamingPort.close(); - yCDebug(MULTIPLEANALOGSENSORSSERVER, "Detach complete"); return true; } diff --git a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h index cf885c167db..36e6f5dbc22 100644 --- a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h +++ b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.h @@ -52,10 +52,6 @@ class MultipleAnalogSensorsServer : // Generic vector buffer yarp::sig::Vector m_buffer; - // Wrapped subdevices, if any - yarp::dev::PolyDriver m_subdevice; - bool m_isDeviceOwned{false}; - // Interface of the wrapped device yarp::dev::IThreeAxisGyroscopes* m_iThreeAxisGyroscopes{nullptr}; yarp::dev::IThreeAxisLinearAccelerometers* m_iThreeAxisLinearAccelerometers{nullptr}; From 64b80661887429e0f053d7244765c215b22797e4 Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Tue, 6 Feb 2024 17:13:26 +0100 Subject: [PATCH 2/4] fixed regression issue in controlBoard_nws_yarp --- .../fakeMotionControl/fakeMotionControl.h | 6 +-- .../fakeMotionControlMicro.h | 10 +--- .../ControlBoard_nws_yarp.cpp | 49 +++++++++++++------ 3 files changed, 38 insertions(+), 27 deletions(-) diff --git a/src/devices/fake/fakeMotionControl/fakeMotionControl.h b/src/devices/fake/fakeMotionControl/fakeMotionControl.h index aedc457e273..3d14ba93808 100644 --- a/src/devices/fake/fakeMotionControl/fakeMotionControl.h +++ b/src/devices/fake/fakeMotionControl/fakeMotionControl.h @@ -73,15 +73,13 @@ struct ImpedanceParameters /** * @ingroup dev_impl_fake dev_impl_motor * - * \brief `fakeMotionControl`: Documentation to be added - * - * The aim of this device is to mimic the expected behavior of a + * \brief `fakeMotionControl`: The aim of this device is to mimic the expected behavior of a * real motion control device to help testing the high level software. * * This device is implementing last version of interfaces and it is compatible * with controlBoard_nws_yarp device. * - * WIP - it is very basic now, not all interfaces are implemented yet. + * WIP - Some interfaces could be not implemented. */ class FakeMotionControl : public yarp::dev::DeviceDriver, diff --git a/src/devices/fake/fakeMotionControlMicro/fakeMotionControlMicro.h b/src/devices/fake/fakeMotionControlMicro/fakeMotionControlMicro.h index 117b58c6383..f7021aefb3b 100644 --- a/src/devices/fake/fakeMotionControlMicro/fakeMotionControlMicro.h +++ b/src/devices/fake/fakeMotionControlMicro/fakeMotionControlMicro.h @@ -20,15 +20,9 @@ /** * @ingroup dev_impl_fake dev_impl_motor * - * \brief `fakeMotionControlMicro`: Documentation to be added + * \brief `fakeMotionControlMicro`: This device implements a minimal subset of mandatory interfaces + * to run with controlBoard_nws_yarp. It is thus a smaller, limited version than fakeMotionControl. * - * The aim of this device is to mimic the expected behavior of a - * real motion control device to help testing the high level software. - * - * This device is implementing last version of interfaces and it is compatible - * with controlBoard_nws_yarp device. - * - * WIP - it is very basic now, not all interfaces are implemented yet. */ class FakeMotionControlMicro : public yarp::os::PeriodicThread, diff --git a/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.cpp b/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.cpp index 5f373cfa067..f4fe2330d82 100644 --- a/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.cpp +++ b/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.cpp @@ -183,8 +183,7 @@ bool ControlBoard_nws_yarp::setDevice(yarp::dev::DeviceDriver* driver, bool owne // yarp::dev::IPositionControl* iPositionControl{nullptr}; subdevice_ptr->view(iPositionControl); if (!iPositionControl) { - yCError(CONTROLBOARD, "Part <%s>: IPositionControl interface was not found in subdevice. Quitting", partName.c_str()); - return false; + yCWarning(CONTROLBOARD, "Part <%s>: IPositionControl interface was not found in subdevice.", partName.c_str()); } // yarp::dev::IPositionDirect* iPositionDirect{nullptr}; @@ -196,15 +195,13 @@ bool ControlBoard_nws_yarp::setDevice(yarp::dev::DeviceDriver* driver, bool owne // yarp::dev::IVelocityControl* iVelocityControl{nullptr}; subdevice_ptr->view(iVelocityControl); if (!iVelocityControl) { - yCError(CONTROLBOARD, "Part <%s>: IVelocityControl interface was not found in subdevice. Quitting", partName.c_str()); - return false; + yCWarning(CONTROLBOARD, "Part <%s>: IVelocityControl interface was not found in subdevice.", partName.c_str()); } // yarp::dev::IEncodersTimed* iEncodersTimed{nullptr}; subdevice_ptr->view(iEncodersTimed); if (!iEncodersTimed) { - yCError(CONTROLBOARD, "Part <%s>: IEncodersTimed interface was not found in subdevice. Quitting", partName.c_str()); - return false; + yCWarning(CONTROLBOARD, "Part <%s>: IEncodersTimed interface was not found in subdevice.", partName.c_str()); } // yarp::dev::IMotor* iMotor{nullptr}; @@ -291,13 +288,37 @@ bool ControlBoard_nws_yarp::setDevice(yarp::dev::DeviceDriver* driver, bool owne yCWarning(CONTROLBOARD, "Part <%s>: ICurrentControl interface was not found in subdevice.", partName.c_str()); } - // Get the number of controlled joints - int tmp_axes; - if (!iPositionControl->getAxes(&tmp_axes)) { - yCError(CONTROLBOARD) << "Part <%s>: Failed to get axes number for subdevice " << partName.c_str(); - return false; + int tmp_axes = 0; + if (iAxisInfo) + { + if (!iAxisInfo->getAxes(&tmp_axes)) { + yCError(CONTROLBOARD) << "Part <%s>: iAxisInfo->getAxes() failed for subdevice " << partName.c_str(); + return false; + } + } + else if (iEncodersTimed) + { + if (!iEncodersTimed->getAxes(&tmp_axes)) { + yCError(CONTROLBOARD) << "Part <%s>: iEncodersTimed->getAxes() failed for subdevice " << partName.c_str(); + return false; + } + } + else if (iPositionControl) + { + if (!iPositionControl->getAxes(&tmp_axes)) { + yCError(CONTROLBOARD) << "Part <%s>: iPositionControl->getAxes() failed for subdevice " << partName.c_str(); + return false; + } } + else if (iVelocityControl) + { + if (!iVelocityControl->getAxes(&tmp_axes)) { + yCError(CONTROLBOARD) << "Part <%s>: iVelocityControl->getAxes() failed for subdevice " << partName.c_str(); + return false; + } + } + if (tmp_axes <= 0) { yCError(CONTROLBOARD, "Part <%s>: attached device has an invalid number of joints (%d)", partName.c_str(), tmp_axes); return false; @@ -386,9 +407,8 @@ void ControlBoard_nws_yarp::run() // check we are not overflowing with input messages constexpr int reads_for_warning = 20; if (inputStreamingPort.getPendingReads() >= reads_for_warning) { - yCWarning(CONTROLBOARD) << "Number of streaming input messages to be read is " << inputStreamingPort.getPendingReads() << " and can overflow"; + yCIWarning(CONTROLBOARD, id()) << "Number of streaming input messages to be read is " << inputStreamingPort.getPendingReads() << " and can overflow"; } - // handle stateExt first jointData& data = extendedOutputState_buffer.get(); @@ -462,8 +482,7 @@ void ControlBoard_nws_yarp::run() { if (std::abs(times[0] - tt) > 1.0) { - yCError(CONTROLBOARD, "Encoder Timestamps are not consistent! Data will not be published."); - yarp::sig::Vector _data(subdevice_joints); + yCIErrorThrottle(CONTROLBOARD, id(), 1.0) << "Encoder timestamps are not consistent! Data will not be published."; return; } } From 6f96e3479518cd8dc27af6daa3dd023ed54f4838 Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Tue, 6 Feb 2024 17:23:15 +0100 Subject: [PATCH 3/4] fixed regression for RgbdSensor_nws_yarp --- .../RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/devices/networkWrappers/RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.cpp b/src/devices/networkWrappers/RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.cpp index cc1e48ceb00..1d5f4e92ea9 100644 --- a/src/devices/networkWrappers/RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.cpp +++ b/src/devices/networkWrappers/RGBDSensor_nws_yarp/RgbdSensor_nws_yarp.cpp @@ -295,6 +295,7 @@ bool RgbdSensor_nws_yarp::detach() } sensor_p = nullptr; + fgCtrl = nullptr; return true; } @@ -451,7 +452,7 @@ bool RgbdSensor_nws_yarp::writeData() else { oldDepthStamp = depthStamp; } // TBD: We should check here somehow if the timestamp was correctly updated and, if not, update it ourselves. - if (rgb_data_ok) + if (rgb_data_ok && colorFrame_StreamingPort.getOutputCount() > 0) { FlexImage& yColorImage = colorFrame_StreamingPort.prepare(); yColorImage.setPixelCode(colorImage.getPixelCode()); @@ -460,7 +461,7 @@ bool RgbdSensor_nws_yarp::writeData() colorFrame_StreamingPort.setEnvelope(colorStamp); colorFrame_StreamingPort.write(); } - if (depth_data_ok) + if (depth_data_ok && depthFrame_StreamingPort.getOutputCount() > 0) { ImageOf& yDepthImage = depthFrame_StreamingPort.prepare(); yDepthImage.setQuantum(depthImage.getQuantum()); From e3aa409dcd5d70d721d6e56a4cc9a81fb4231acc Mon Sep 17 00:00:00 2001 From: Marco Randazzo Date: Wed, 7 Feb 2024 14:38:14 +0100 Subject: [PATCH 4/4] fixed regression for MultipleAnalogSensorsServer --- .../MultipleAnalogSensorsServer.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp index fa9af984a63..bb5e29e2ad8 100644 --- a/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp +++ b/src/devices/networkWrappers/multipleanalogsensorsserver/MultipleAnalogSensorsServer.cpp @@ -414,6 +414,7 @@ bool MultipleAnalogSensorsServer::attachAll(const yarp::dev::PolyDriverList& p) if(!ok) { + yCError(MULTIPLEANALOGSENSORSSERVER, "Failure in populateAllSensorsMetadata()"); close(); return false; } @@ -453,6 +454,7 @@ bool MultipleAnalogSensorsServer::attachAll(const yarp::dev::PolyDriverList& p) return false; } + yCDebug(MULTIPLEANALOGSENSORSSERVER, "Attach complete"); return true; } @@ -467,6 +469,7 @@ bool MultipleAnalogSensorsServer::detachAll() m_rpcPort.close(); m_streamingPort.close(); + yCDebug(MULTIPLEANALOGSENSORSSERVER, "Detach complete"); return true; }