From 238d6c00cacc347140aea8788e4c734b8d30a9aa Mon Sep 17 00:00:00 2001 From: elandini84 <62991576+elandini84@users.noreply.github.com> Date: Mon, 7 Feb 2022 14:52:47 +0100 Subject: [PATCH 1/5] Decimal places limit for depth Camera (#608) The possibility to limit the decimal places of the pixels values has been added to the DepthCameraDriver class. Co-authored-by: Silvio Traversaro --- CHANGELOG.md | 3 ++ .../depthCamera/include/gazebo/DepthCamera.hh | 1 + .../include/yarp/dev/DepthCameraDriver.h | 5 +++ plugins/depthCamera/src/DepthCameraDriver.cpp | 40 +++++++++++++++++-- tutorial/model/depthcamera/depthcamera.ini | 3 ++ 5 files changed, 49 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index c850fa2ad..2d3a3b351 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,9 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo ## [Unreleased] +### Added +- The possibility to limit the decimal places of the pixels values has been added to the `gazebo_yarp_depthcamera` plugin, via the `QUANT_PARAM::depth_quant` parameter. The data modification is performed in the `GazeboYarpDepthCameraDriver::getDepthImage` function (https://github.com/robotology/gazebo-yarp-plugins/pull/608). + ### Fixed - Removed `getLastInputStamp` method from `LaserSensorDriver` class in `gazebo_yarp_lasersensor`. Furthermore, fix bug in `gazebo_yarp_lasersensor`, by adding the update to the variable `m_timestamp` inherited from `yarp::dev::Lidar2DDeviceBase` (https://github.com/robotology/gazebo-yarp-plugins/pull/604). diff --git a/plugins/depthCamera/include/gazebo/DepthCamera.hh b/plugins/depthCamera/include/gazebo/DepthCamera.hh index 557b16afd..ad7efeaad 100644 --- a/plugins/depthCamera/include/gazebo/DepthCamera.hh +++ b/plugins/depthCamera/include/gazebo/DepthCamera.hh @@ -45,6 +45,7 @@ namespace gazebo /// /// /// + /// ///
name Port name to assign to the wrapper to this device.
period Update period (in s) of yarp port that publish the measure.
QUANT_PARAM::depth_quant The number of decimals for the values of the depth image pixels
/// If the required parameters are not specified, their value will be the /// default one assigned by the yarp::dev::ServerFrameGrabbers wrapper. diff --git a/plugins/depthCamera/include/yarp/dev/DepthCameraDriver.h b/plugins/depthCamera/include/yarp/dev/DepthCameraDriver.h index ce5567a97..e672aee20 100644 --- a/plugins/depthCamera/include/yarp/dev/DepthCameraDriver.h +++ b/plugins/depthCamera/include/yarp/dev/DepthCameraDriver.h @@ -17,6 +17,7 @@ #include #include #include +#include #include @@ -130,6 +131,10 @@ class yarp::dev::GazeboYarpDepthCameraDriver: public yarp::dev::DeviceDriver, gazebo::event::ConnectionPtr m_updateDepthFrame_Connection; gazebo::event::ConnectionPtr m_updateRGBPointCloud_Connection; gazebo::event::ConnectionPtr m_updateImageFrame_Connection; + + // Data quantization related parameters + bool m_depthQuantizationEnabled{false}; + int m_depthDecimalNum{0}; }; #endif // GAZEBOYARP_DEPTHCAMERADRIVER_H diff --git a/plugins/depthCamera/src/DepthCameraDriver.cpp b/plugins/depthCamera/src/DepthCameraDriver.cpp index ff8fabe9a..a06a4c3a6 100644 --- a/plugins/depthCamera/src/DepthCameraDriver.cpp +++ b/plugins/depthCamera/src/DepthCameraDriver.cpp @@ -28,6 +28,7 @@ using GazeboYarpPlugins::GAZEBODEPTH; const string YarpScopedName = "sensorScopedName"; + GazeboYarpDepthCameraDriver::GazeboYarpDepthCameraDriver() { m_imageFormat = VOCAB_PIXEL_RGB; @@ -76,6 +77,16 @@ bool GazeboYarpDepthCameraDriver::open(yarp::os::Searchable &config) { string sensorScopedName((config.find(YarpScopedName.c_str()).asString().c_str())); + //Manage depth quantization parameter + if(config.check("QUANT_PARAM")) { + yarp::os::Property quantCfg; + quantCfg.fromString(config.findGroup("QUANT_PARAM").toString()); + m_depthQuantizationEnabled = true; + if (quantCfg.check("depth_quant")) { + m_depthDecimalNum = quantCfg.find("depth_quant").asInt32(); + } + } + //Get gazebo pointers m_conf.fromString(config.toString()); m_depthCameraSensorPtr = dynamic_cast (GazeboYarpPlugins::Handler::getHandler()->getSensor(sensorScopedName)); @@ -159,7 +170,7 @@ void GazeboYarpDepthCameraDriver::OnNewImageFrame(const unsigned char* _image, U { //possible image format (hardcoded (sigh..) in osrf/gazebo/source/gazebo/rendering/Camera.cc ) /* data type is string. why they didn't use a enum? mystery... - * + * * L8 = INT8 = 1 * R8G8B8 = RGB_INT8 = BGR_INT8 = B8G8R8 = 3 * BAYER_RGGB8 = BAYER_BGGR8 = BAYER_GBRG8 = BAYER_GRBG8 = 1 @@ -406,7 +417,30 @@ bool GazeboYarpDepthCameraDriver::getDepthImage(depthImageType& depthImage, Stam depthImage.resize(m_width, m_height); //depthImage.setPixelCode(m_depthFormat); - memcpy(depthImage.getRawImage(), m_depthFrame_Buffer, m_width * m_height * sizeof(float)); + if(!m_depthQuantizationEnabled) { + memcpy(depthImage.getRawImage(), m_depthFrame_Buffer, m_width * m_height * sizeof(float)); + } + else { + double nearPlane = m_depthCameraSensorPtr->DepthCamera()->NearClip(); + double farPlane = m_depthCameraSensorPtr->DepthCamera()->FarClip(); + + int intTemp; + float value; + + auto pxPtr = reinterpret_cast(depthImage.getRawImage()); + for(int i=0; i farPlane) { value = farPlane; } + + *pxPtr = value; + pxPtr++; + } + } timeStamp->update(this->m_depthCameraSensorPtr->LastUpdateTime().Double()); @@ -432,7 +466,7 @@ std::string GazeboYarpDepthCameraDriver::getLastErrorMsg(Stamp* timeStamp) { if(timeStamp) { - timeStamp->update(this->m_depthCameraSensorPtr->LastUpdateTime().Double()); + timeStamp->update(this->m_depthCameraSensorPtr->LastUpdateTime().Double()); } return m_error; diff --git a/tutorial/model/depthcamera/depthcamera.ini b/tutorial/model/depthcamera/depthcamera.ini index 472484ff7..50bd733f7 100644 --- a/tutorial/model/depthcamera/depthcamera.ini +++ b/tutorial/model/depthcamera/depthcamera.ini @@ -3,3 +3,6 @@ period 30 imagePort /camera depthPort /depthcamera name /depthcamera + +[QUANT_PARAM] +depth_quant 2 From b47656b16bc009047e851453f59f19a7c8c66fd9 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Fri, 25 Feb 2022 11:11:16 +0100 Subject: [PATCH 2/5] Fix use of yarpConfigurationString parameter in gazebo_yarp_worldinterface plugin (#609) --- CHANGELOG.md | 1 + plugins/worldinterface/src/worldinterface.cpp | 14 ++------------ 2 files changed, 3 insertions(+), 12 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2d3a3b351..68b8557a3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,7 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo ### Fixed - Removed `getLastInputStamp` method from `LaserSensorDriver` class in `gazebo_yarp_lasersensor`. Furthermore, fix bug in `gazebo_yarp_lasersensor`, by adding the update to the variable `m_timestamp` inherited from `yarp::dev::Lidar2DDeviceBase` (https://github.com/robotology/gazebo-yarp-plugins/pull/604). +- Fix use of yarpConfigurationString parameter in `gazebo_yarp_worldinterface` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/609). ## [4.1.2] - 2022-01-19 diff --git a/plugins/worldinterface/src/worldinterface.cpp b/plugins/worldinterface/src/worldinterface.cpp index b1979c782..c250c562c 100644 --- a/plugins/worldinterface/src/worldinterface.cpp +++ b/plugins/worldinterface/src/worldinterface.cpp @@ -12,6 +12,7 @@ #include #include #include +#include using namespace gazebo; using namespace std; @@ -54,18 +55,7 @@ void WorldInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) //pass reference to server m_wi_server.attachWorldProxy(&m_proxy); - //Getting .ini configuration file from sdf - bool configuration_loaded = false; - - if (_sdf->HasElement("yarpConfigurationFile")) { - std::string ini_file_name = _sdf->Get("yarpConfigurationFile"); - std::string ini_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(ini_file_name); - - if (ini_file_path != "" && m_parameters.fromConfigFile(ini_file_path.c_str())) { - yInfo() << "Found yarpConfigurationFile: loading from " << ini_file_path ; - configuration_loaded = true; - } - } + bool configuration_loaded = GazeboYarpPlugins::loadConfigModelPlugin(_model, _sdf, m_parameters); if (!configuration_loaded) { yError() << "WorldInterface::Load error could not load configuration file"; From ea7d469de8a000a86f05587a994f5375b43cbe11 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Mon, 28 Feb 2022 10:04:41 +0100 Subject: [PATCH 3/5] Add disableImplicitNetworkWrapper to gazebo_yarp_forcetorque, gazebo_yarp_depthcamera, gazebo_yarp_lasersensor, gazebo_yarp_imu and gazebo_yarp_controlboard (#586) --- CHANGELOG.md | 7 +- CMakeLists.txt | 4 +- .../include/gazebo/ControlBoard.hh | 3 +- plugins/controlboard/src/ControlBoard.cc | 291 ++++++++++-------- .../controlboard/src/ControlBoardDriver.cpp | 30 +- plugins/depthCamera/src/DepthCamera.cc | 50 +-- plugins/forcetorque/src/ForceTorque.cc | 62 ++-- plugins/imu/src/IMU.cc | 99 +++--- plugins/lasersensor/src/LaserSensor.cc | 43 ++- 9 files changed, 349 insertions(+), 240 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 68b8557a3..b37ba93bf 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,9 +5,14 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo ## [Unreleased] -### Added +## [4.2.0] - 2022-02-25 + +### Added +- Added `disableImplicitNetworkWrapper` option to `gazebo_yarp_forcetorque`, `gazebo_yarp_depthcamera`, `gazebo_yarp_lasersensor`, `gazebo_yarp_imu` and `gazebo_yarp_controlboard` plugins. If present, this option +disable any network wrapper server that the plugin created, by just creating the device itself. This option is meant to be used in conjuction with the `gazebo_yarp_robotinterface` plugin, that can be used to launch and attach any networ wrapper server. This option is equivalent to the behavior that can be obtained by setting the CMake option `GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS` to `OFF` (https://github.com/robotology/gazebo-yarp-plugins/pull/586). - The possibility to limit the decimal places of the pixels values has been added to the `gazebo_yarp_depthcamera` plugin, via the `QUANT_PARAM::depth_quant` parameter. The data modification is performed in the `GazeboYarpDepthCameraDriver::getDepthImage` function (https://github.com/robotology/gazebo-yarp-plugins/pull/608). + ### Fixed - Removed `getLastInputStamp` method from `LaserSensorDriver` class in `gazebo_yarp_lasersensor`. Furthermore, fix bug in `gazebo_yarp_lasersensor`, by adding the update to the variable `m_timestamp` inherited from `yarp::dev::Lidar2DDeviceBase` (https://github.com/robotology/gazebo-yarp-plugins/pull/604). - Fix use of yarpConfigurationString parameter in `gazebo_yarp_worldinterface` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/609). diff --git a/CMakeLists.txt b/CMakeLists.txt index 4ad9bf3a8..2dc03881c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,8 +8,8 @@ option(GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS "if enabled removes # Project version set(${PROJECT_NAME}_MAJOR_VERSION 4) -set(${PROJECT_NAME}_MINOR_VERSION 1) -set(${PROJECT_NAME}_PATCH_VERSION 1) +set(${PROJECT_NAME}_MINOR_VERSION 2) +set(${PROJECT_NAME}_PATCH_VERSION 0) set(${PROJECT_NAME}_VERSION ${${PROJECT_NAME}_MAJOR_VERSION}.${${PROJECT_NAME}_MINOR_VERSION}.${${PROJECT_NAME}_PATCH_VERSION}) diff --git a/plugins/controlboard/include/gazebo/ControlBoard.hh b/plugins/controlboard/include/gazebo/ControlBoard.hh index e80595bb5..6c403bf49 100644 --- a/plugins/controlboard/include/gazebo/ControlBoard.hh +++ b/plugins/controlboard/include/gazebo/ControlBoard.hh @@ -57,11 +57,10 @@ private: yarp::dev::IMultipleWrapper* m_iVirtAnalogSensorWrap; yarp::dev::PolyDriverList m_controlBoards; bool m_useVirtAnalogSensor = false; - #else + #endif yarp::dev::PolyDriver m_controlboardDriver; std::string m_scopedDeviceName; std::string m_yarpDeviceName; - #endif yarp::os::Property m_parameters; diff --git a/plugins/controlboard/src/ControlBoard.cc b/plugins/controlboard/src/ControlBoard.cc index 07deeb9ae..379a0adff 100644 --- a/plugins/controlboard/src/ControlBoard.cc +++ b/plugins/controlboard/src/ControlBoard.cc @@ -101,170 +101,221 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpControlBoard) return; } #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS - - yarp::os::Bottle wrapper_group = m_parameters.findGroup("WRAPPER"); - if(wrapper_group.isNull()) - { - yCDebug(GAZEBOCONTROLBOARD) <<"[WRAPPER] group not found in config file"; - } - - if(m_parameters.check("ROS")) + bool disable_wrapper = m_parameters.check("disableImplicitNetworkWrapper"); + + if (disable_wrapper && !m_parameters.check("yarpDeviceName")) { - std::string ROS; - ROS = std::string ("(") + m_parameters.findGroup("ROS").toString() + std::string (")"); - wrapper_group.append(yarp::os::Bottle(ROS)); - } - - m_wrapper.open(wrapper_group); - - if (!m_wrapper.isValid()) { - yCError(GAZEBOCONTROLBOARD) <<"wrapper did not open, load failed."; - m_wrapper.close(); - return; - } - - if (!m_wrapper.view(m_iWrap)) { - yCError(GAZEBOCONTROLBOARD) <<"wrapper interface not found, load failed."; - } - - yarp::os::Bottle *netList = wrapper_group.find("networks").asList(); - - if (netList->isNull()) { - yCError(GAZEBOCONTROLBOARD) <<"net list to attach to was not found, load failed."; - m_wrapper.close(); + yError() << "GazeboYarpControlBoard : missing yarpDeviceName parameter for one device in robot " << m_robotName; return; } - - yarp::os::Bottle driver_group; - yarp::os::Bottle virt_group; - - m_useVirtAnalogSensor = m_parameters.check("useVirtualAnalogSensor", yarp::os::Value(false)).asBool(); - if (m_useVirtAnalogSensor) + + + if (!disable_wrapper) { - virt_group = m_parameters.findGroup("VIRTUAL_ANALOG_SERVER"); - if (virt_group.isNull()) + yarp::os::Bottle wrapper_group = m_parameters.findGroup("WRAPPER"); + if(wrapper_group.isNull()) { - yCError(GAZEBOCONTROLBOARD) <<"[VIRTUAL_ANALOG_SERVER] group not found in config file"; - return; + yCDebug(GAZEBOCONTROLBOARD) <<"[WRAPPER] group not found in config file"; } - yarp::os::Bottle& robotName_config = virt_group.addList(); - robotName_config.addString("robotName"); - robotName_config.addString(m_robotName.c_str()); + if(m_parameters.check("ROS")) + { + std::string ROS; + ROS = std::string ("(") + m_parameters.findGroup("ROS").toString() + std::string (")"); + wrapper_group.append(yarp::os::Bottle(ROS)); + } - std::string networks = std::string("(") + wrapper_group.findGroup("networks").toString() + std::string(")"); - virt_group.append(yarp::os::Bottle(networks)); - } + m_wrapper.open(wrapper_group); - for (int n = 0; n < netList->size(); n++) - { - yarp::dev::PolyDriverDescriptor newPoly; + if (!m_wrapper.isValid()) { + yCError(GAZEBOCONTROLBOARD) <<"wrapper did not open, load failed."; + m_wrapper.close(); + return; + } - newPoly.key = netList->get(n).asString(); + if (!m_wrapper.view(m_iWrap)) { + yCError(GAZEBOCONTROLBOARD) <<"wrapper interface not found, load failed."; + } + + yarp::os::Bottle driver_group; + yarp::os::Bottle virt_group; - // initially deal with virtual analog stuff + m_useVirtAnalogSensor = m_parameters.check("useVirtualAnalogSensor", yarp::os::Value(false)).asBool(); if (m_useVirtAnalogSensor) { - std::string net = std::string("(") + wrapper_group.findGroup(newPoly.key.c_str()).toString() + std::string(")"); - virt_group.append(yarp::os::Bottle(net)); - } + virt_group = m_parameters.findGroup("VIRTUAL_ANALOG_SERVER"); + if (virt_group.isNull()) + { + yCError(GAZEBOCONTROLBOARD) <<"[VIRTUAL_ANALOG_SERVER] group not found in config file"; + return; + } - std::string scopedDeviceName; - if(!m_parameters.check("yarpDeviceName")) - { - scopedDeviceName = m_robotName + "::" + newPoly.key.c_str(); - } - else - { - scopedDeviceName = m_robotName + "::" + m_parameters.find("yarpDeviceName").asString(); + yarp::os::Bottle& robotName_config = virt_group.addList(); + robotName_config.addString("robotName"); + robotName_config.addString(m_robotName.c_str()); + + std::string networks = std::string("(") + wrapper_group.findGroup("networks").toString() + std::string(")"); + virt_group.append(yarp::os::Bottle(networks)); } - newPoly.poly = GazeboYarpPlugins::Handler::getHandler()->getDevice(scopedDeviceName); - if( newPoly.poly != NULL) - { - // device already exists, use it, setting it againg to increment the usage counter. - yCWarning(GAZEBOCONTROLBOARD) << newPoly.key.c_str() << "already opened."; + + + + yarp::os::Bottle *netList = wrapper_group.find("networks").asList(); + + if (netList->isNull()) { + yCError(GAZEBOCONTROLBOARD) <<"net list to attach to was not found, load failed."; + m_wrapper.close(); + return; } - else + + + + for (int n = 0; n < netList->size(); n++) { - driver_group = m_parameters.findGroup(newPoly.key.c_str()); - if (driver_group.isNull()) { - yCError(GAZEBOCONTROLBOARD) <<"group not found in config file. Closing wrapper." << newPoly.key.c_str(); - m_wrapper.close(); - return; - } + yarp::dev::PolyDriverDescriptor newPoly; - m_parameters.put("name", newPoly.key.c_str()); - m_parameters.fromString(driver_group.toString(), false); - m_parameters.put("robotScopedName", m_robotName); + newPoly.key = netList->get(n).asString(); - if (_sdf->HasElement("initialConfiguration")) { - //yCDebug(GAZEBOCONTROLBOARD)<<"Found initial Configuration: "; - std::string configuration_s = _sdf->Get("initialConfiguration"); - m_parameters.put("initialConfiguration", configuration_s.c_str()); - //yCDebug(GAZEBOCONTROLBOARD)<open(m_parameters) || ! newPoly.poly->isValid()) + std::string scopedDeviceName; + if(!m_parameters.check("yarpDeviceName")) { - yCError(GAZEBOCONTROLBOARD) << newPoly.key << "> did not open."; - for(int idx=0; idxgetDevice(scopedDeviceName); + + if( newPoly.poly != NULL) + { + // device already exists, use it, setting it againg to increment the usage counter. + yCWarning(GAZEBOCONTROLBOARD) << newPoly.key.c_str() << "already opened."; + } + else + { + driver_group = m_parameters.findGroup(newPoly.key.c_str()); + if (driver_group.isNull()) { + yCError(GAZEBOCONTROLBOARD) <<"group not found in config file. Closing wrapper." << newPoly.key.c_str(); + m_wrapper.close(); + return; + } + + m_parameters.put("name", newPoly.key.c_str()); + m_parameters.fromString(driver_group.toString(), false); + m_parameters.put("robotScopedName", m_robotName); + + if (_sdf->HasElement("initialConfiguration")) { + //yCDebug(GAZEBOCONTROLBOARD)<<"Found initial Configuration: "; + std::string configuration_s = _sdf->Get("initialConfiguration"); + m_parameters.put("initialConfiguration", configuration_s.c_str()); + //yCDebug(GAZEBOCONTROLBOARD)<open(m_parameters) || ! newPoly.poly->isValid()) { - m_controlBoards[idx]->poly->close(); + yCError(GAZEBOCONTROLBOARD) << newPoly.key << "> did not open."; + for(int idx=0; idxpoly->close(); + } + m_wrapper.close(); + return; } - m_wrapper.close(); + } + + //Register the device with the given name + if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, newPoly.poly)) + { + yCError(GAZEBOCONTROLBOARD) << "failed setting scopedDeviceName(=" << scopedDeviceName << ")"; return; } + yCInfo(GAZEBOCONTROLBOARD) << "Registered YARP device with instance name:" << scopedDeviceName; + + m_controlBoards.push(newPoly); } - //Register the device with the given name - if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, newPoly.poly)) + if (m_useVirtAnalogSensor) { - yCError(GAZEBOCONTROLBOARD) << "failed setting scopedDeviceName(=" << scopedDeviceName << ")"; - return; - } - yCInfo(GAZEBOCONTROLBOARD) << "Registered YARP device with instance name:" << scopedDeviceName; + m_virtAnalogSensorWrapper.open(virt_group); - m_controlBoards.push(newPoly); - } + if (!m_virtAnalogSensorWrapper.isValid()) + { + yCError(GAZEBOCONTROLBOARD) << "Virtual analog sensor wrapper did not open, load failed."; + m_virtAnalogSensorWrapper.close(); + return; + } - if (m_useVirtAnalogSensor) - { - m_virtAnalogSensorWrapper.open(virt_group); + if (!m_virtAnalogSensorWrapper.view(m_iVirtAnalogSensorWrap)) + { + yCError(GAZEBOCONTROLBOARD) << "Could not view the IVirtualAnalogSensor interface"; + return; + } - if (!m_virtAnalogSensorWrapper.isValid()) + if (!m_iVirtAnalogSensorWrap->attachAll(m_controlBoards)) + { + yCError(GAZEBOCONTROLBOARD) << "Could not attach VirtualAnalogSensor interface to controlboards"; + return; + } + } + + if (!m_iWrap || !m_iWrap->attachAll(m_controlBoards)) { - yCError(GAZEBOCONTROLBOARD) << "Virtual analog sensor wrapper did not open, load failed."; - m_virtAnalogSensorWrapper.close(); + yCError(GAZEBOCONTROLBOARD) << "error while attaching wrapper to device."; + m_wrapper.close(); + if (m_useVirtAnalogSensor) + { + m_virtAnalogSensorWrapper.close(); + } + for (int n = 0; n < netList->size(); n++) { + std::string scopedDeviceName = m_robotName + "::" + m_controlBoards[n]->key.c_str(); + GazeboYarpPlugins::Handler::getHandler()->removeDevice(scopedDeviceName); + } return; } - - if (!m_virtAnalogSensorWrapper.view(m_iVirtAnalogSensorWrap)) + } else { + if(!m_parameters.check("yarpDeviceName")) { - yCError(GAZEBOCONTROLBOARD) << "Could not view the IVirtualAnalogSensor interface"; + yCError(GAZEBOCONTROLBOARD) << "missing parameter yarpDeviceName"; return; } + m_yarpDeviceName = m_parameters.find("yarpDeviceName").asString(); + m_scopedDeviceName = m_robotName + "::" + m_yarpDeviceName; + + m_parameters.put("device","gazebo_controlboard"); + m_parameters.put("name", m_scopedDeviceName); + m_parameters.put("robotScopedName", m_robotName); + yCDebug(GAZEBOCONTROLBOARD) << "m_parameters:"<HasElement("initialConfiguration")) { + //yCDebug(GAZEBOCONTROLBOARD)<<"Found initial Configuration: "; + std::string configuration_s = _sdf->Get("initialConfiguration"); + m_parameters.put("initialConfiguration", configuration_s.c_str()); + //yCDebug(GAZEBOCONTROLBOARD)<attachAll(m_controlBoards)) + if(! m_controlboardDriver.open(m_parameters) || ! m_controlboardDriver.isValid()) { - yCError(GAZEBOCONTROLBOARD) << "Could not attach VirtualAnalogSensor interface to controlboards"; + yCError(GAZEBOCONTROLBOARD) << m_yarpDeviceName.c_str() << "> did not open."; + m_controlboardDriver.close(); return; } - } - - if (!m_iWrap || !m_iWrap->attachAll(m_controlBoards)) - { - yCError(GAZEBOCONTROLBOARD) << "error while attaching wrapper to device."; - m_wrapper.close(); - if (m_useVirtAnalogSensor) + //Register the device with the given name + if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(m_scopedDeviceName, &m_controlboardDriver)) { - m_virtAnalogSensorWrapper.close(); - } - for (int n = 0; n < netList->size(); n++) { - std::string scopedDeviceName = m_robotName + "::" + m_controlBoards[n]->key.c_str(); - GazeboYarpPlugins::Handler::getHandler()->removeDevice(scopedDeviceName); + yCError(GAZEBOCONTROLBOARD) << "failed setting scopedDeviceName(=" << m_scopedDeviceName << ")"; + return; } - return; + yCInfo(GAZEBOCONTROLBOARD) << "Registered YARP device with instance name:" << m_scopedDeviceName; } #else if(!m_parameters.check("yarpDeviceName")) diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp index 62acef69c..05f824030 100644 --- a/plugins/controlboard/src/ControlBoardDriver.cpp +++ b/plugins/controlboard/src/ControlBoardDriver.cpp @@ -1300,21 +1300,27 @@ bool GazeboYarpControlBoardDriver::setPIDsForGroup_IMPEDANCE(std::vector. \ + /////////////////////////// + //Open the wrapper, forcing it to be a "RGBDSensorWrapper" + wrapper_properties.put("device","RGBDSensorWrapper"); + if(wrapper_properties.check("subdevice")) + { + yCError(GAZEBODEPTH) << "RGBDSensorWrapper: Do not use 'subdevice' keyword here since the only supported subdevice is . \ Please remove the line 'subdevice " << wrapper_properties.find("subdevice").asString().c_str() << "' from your config file before proceeding"; - return; + return; + } + + if(!m_cameraWrapper.open(wrapper_properties) ) + { + yCError(GAZEBODEPTH)<<"GazeboYarpDepthCamera Plugin failed: error in opening yarp wrapper"; + return; + } } - - if(!m_cameraWrapper.open(wrapper_properties) ) + if (disable_wrapper && !m_driverParameters.check("yarpDeviceName")) { - yCError(GAZEBODEPTH)<<"GazeboYarpDepthCamera Plugin failed: error in opening yarp wrapper"; + yCError(GAZEBODEPTH) << "GazeboYarpDepthCamera : missing yarpDeviceName parameter for device" << m_sensorName; return; } #endif @@ -119,20 +128,23 @@ void GazeboYarpDepthCamera::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd } #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS - //Attach the driver to the wrapper ::yarp::dev::PolyDriverList driver_list; - - if(!m_cameraWrapper.view(m_iWrap) ) + if (!disable_wrapper) { - yCError(GAZEBODEPTH) << "GazeboYarpDepthCamera : error in loading wrapper"; - return; - } + //Attach the driver to the wrapper - driver_list.push(&m_cameraDriver, "depthcamera"); + if(!m_cameraWrapper.view(m_iWrap) ) + { + yCError(GAZEBODEPTH) << "GazeboYarpDepthCamera : error in loading wrapper"; + return; + } - if(!m_iWrap->attachAll(driver_list) ) - { - yCError(GAZEBODEPTH) << "GazeboYarpDepthCamera : error in connecting wrapper and device "; + driver_list.push(&m_cameraDriver, "depthcamera"); + + if(!m_iWrap->attachAll(driver_list) ) + { + yCError(GAZEBODEPTH) << "GazeboYarpDepthCamera : error in connecting wrapper and device "; + } } #endif diff --git a/plugins/forcetorque/src/ForceTorque.cc b/plugins/forcetorque/src/ForceTorque.cc index e45acd266..53a0b77a8 100644 --- a/plugins/forcetorque/src/ForceTorque.cc +++ b/plugins/forcetorque/src/ForceTorque.cc @@ -82,11 +82,19 @@ void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd driver_properties.put(YarpForceTorqueScopedName.c_str(), m_sensorName.c_str()); #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS - //Open the wrapper - //Force the wrapper to be of type "analogServer" (it make sense? probably no) - wrapper_properties.put("device","analogServer"); - if( !m_forcetorqueWrapper.open(wrapper_properties) ) { - yError()<<"GazeboYarpForceTorque Plugin failed: error in opening yarp driver wrapper"; + bool disable_wrapper = driver_properties.check("disableImplicitNetworkWrapper"); + if (!disable_wrapper) { + //Open the wrapper + //Force the wrapper to be of type "analogServer" (it make sense? probably no) + wrapper_properties.put("device","analogServer"); + if( !m_forcetorqueWrapper.open(wrapper_properties) ) { + yError()<<"GazeboYarpForceTorque Plugin failed: error in opening yarp driver wrapper"; + return; + } + } + if (disable_wrapper && !driver_properties.check("yarpDeviceName")) + { + yError() << "GazeboYarpForceTorque : missing yarpDeviceName parameter for device" << m_sensorName; return; } #endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS @@ -101,28 +109,34 @@ void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd std::string scopedDeviceName; #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS - //Attach the driver to the wrapper - ::yarp::dev::PolyDriverList driver_list; - - if( !m_forcetorqueWrapper.view(m_iWrap) ) { - yError() << "GazeboYarpForceTorque : error in loading wrapper"; - return; + if (!disable_wrapper) { + //Attach the driver to the wrapper + ::yarp::dev::PolyDriverList driver_list; + + if( !m_forcetorqueWrapper.view(m_iWrap) ) { + yError() << "GazeboYarpForceTorque : error in loading wrapper"; + return; + } + + driver_list.push(&m_forceTorqueDriver,"dummy"); + + if( !m_iWrap->attachAll(driver_list) ) { + yError() << "GazeboYarpForceTorque : error in connecting wrapper and device "; + } + + if(!driver_properties.check("yarpDeviceName")) + { + scopedDeviceName = m_sensorName + "::" + driver_list[0]->key; + } + else + { + scopedDeviceName = m_sensorName + "::" + driver_properties.find("yarpDeviceName").asString(); + } + } else { + scopedDeviceName = m_sensorName + "::" + driver_properties.find("yarpDeviceName").asString(); } - driver_list.push(&m_forceTorqueDriver,"dummy"); - - if( !m_iWrap->attachAll(driver_list) ) { - yError() << "GazeboYarpForceTorque : error in connecting wrapper and device "; - } - if(!driver_properties.check("yarpDeviceName")) - { - scopedDeviceName = m_sensorName + "::" + driver_list[0]->key; - } - else - { - scopedDeviceName = m_sensorName + "::" + driver_properties.find("yarpDeviceName").asString(); - } #else if(!driver_properties.check("yarpDeviceName")) { diff --git a/plugins/imu/src/IMU.cc b/plugins/imu/src/IMU.cc index e825d1516..1cb459b23 100644 --- a/plugins/imu/src/IMU.cc +++ b/plugins/imu/src/IMU.cc @@ -84,50 +84,58 @@ void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) GazeboYarpPlugins::Handler::getHandler()->setSensor(_sensor.get()); #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS - /* - * Open the driver wrapper - */ - //Retrieve wrapper properties - ::yarp::os::Bottle MASwrapperProp = m_parameters.findGroup("WRAPPER"); - if(MASwrapperProp.isNull()) + bool disable_wrapper = m_parameters.check("disableImplicitNetworkWrapper"); + if (!disable_wrapper) { - yWarning("GazeboYarpIMU : [WRAPPER] group not found in config file, maybe you are using the old version of the ini file, please update icub-gazebo\n"); - yWarning("GazeboYarpIMU : trying to open it with the legacy behaviour device-subdevice"); - legacy_behaviour = true; - } + /* + * Open the driver wrapper + */ + //Retrieve wrapper properties + ::yarp::os::Bottle MASwrapperProp = m_parameters.findGroup("WRAPPER"); + if(MASwrapperProp.isNull()) + { + yWarning("GazeboYarpIMU : [WRAPPER] group not found in config file, maybe you are using the old version of the ini file, please update icub-gazebo\n"); + yWarning("GazeboYarpIMU : trying to open it with the legacy behaviour device-subdevice"); + legacy_behaviour = true; + } - if (legacy_behaviour) { - m_parameters.put(YarpIMUScopedName.c_str(), m_scopedSensorName.c_str()); - m_parameters.put("sensor_name",_sensor->Name()); - if (!m_imuDriver.open(m_parameters)) { - yError() << "GazeboYarpIMU Plugin Load failed: error in opening yarp driver"; + if (legacy_behaviour) { + m_parameters.put(YarpIMUScopedName.c_str(), m_scopedSensorName.c_str()); + m_parameters.put("sensor_name",_sensor->Name()); + if (!m_imuDriver.open(m_parameters)) { + yError() << "GazeboYarpIMU Plugin Load failed: error in opening yarp driver"; + } + return; } - return; - } - //Open the driver wrapper - if (!m_MASWrapper.open(MASwrapperProp)) - { - yError() << "GazeboYarpIMU Plugin Load failed: error in opening the yarp wrapper"; - } + //Open the driver wrapper + if (!m_MASWrapper.open(MASwrapperProp)) + { + yError() << "GazeboYarpIMU Plugin Load failed: error in opening the yarp wrapper"; + } - /* - * Open the old wrapper - */ - //Retrieve wrapper properties - ::yarp::os::Bottle AdditionalWrapperProp = m_parameters.findGroup("ADDITIONAL_WRAPPER"); - if(AdditionalWrapperProp.isNull()) - { - yError("GazeboYarpIMU : [ADDITIONAL_WRAPPER] group not found in config file\n"); - return; - } + /* + * Open the old wrapper + */ + //Retrieve wrapper properties + ::yarp::os::Bottle AdditionalWrapperProp = m_parameters.findGroup("ADDITIONAL_WRAPPER"); + if(AdditionalWrapperProp.isNull()) + { + yError("GazeboYarpIMU : [ADDITIONAL_WRAPPER] group not found in config file\n"); + return; + } - //Open the driver wrapper - if (!m_AdditionalWrapper.open(AdditionalWrapperProp)) + //Open the driver wrapper + if (!m_AdditionalWrapper.open(AdditionalWrapperProp)) + { + yError() << "GazeboYarpIMU Plugin Load failed: error in opening the yarp wrapper"; + } + } + if (disable_wrapper && !m_parameters.check("yarpDeviceName")) { - yError() << "GazeboYarpIMU Plugin Load failed: error in opening the yarp wrapper"; + yError() << "GazeboYarpIMU : missing yarpDeviceName parameter for device" << m_scopedSensorName; + return; } - #endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS /* @@ -149,16 +157,19 @@ void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) //Register the device with the given name std::string scopedDeviceName; #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS - //Attach the part driver to the wrapper - if(!m_MASWrapper.view(m_iWrap) || (!m_AdditionalWrapper.view(m_iWrapAdditional))){ - yError()<< "GazeboYarpIMU Plugin Load failed: unable to view iMultipleWrapper interfaces"; - return; - } ::yarp::dev::PolyDriverList driverList; - driverList.push(&m_imuDriver,"dummy"); - if(!m_iWrap->attachAll(driverList) || ! m_iWrapAdditional->attachAll(driverList)) + if (!disable_wrapper) { - yError() << "GazeboYarpIMU: error in connecting wrapper and device "; + //Attach the part driver to the wrapper + if(!m_MASWrapper.view(m_iWrap) || (!m_AdditionalWrapper.view(m_iWrapAdditional))){ + yError()<< "GazeboYarpIMU Plugin Load failed: unable to view iMultipleWrapper interfaces"; + return; + } + driverList.push(&m_imuDriver,"dummy"); + if(!m_iWrap->attachAll(driverList) || ! m_iWrapAdditional->attachAll(driverList)) + { + yError() << "GazeboYarpIMU: error in connecting wrapper and device "; + } } if(!m_parameters.check("yarpDeviceName")) diff --git a/plugins/lasersensor/src/LaserSensor.cc b/plugins/lasersensor/src/LaserSensor.cc index bd316ce4f..1b7fa56f8 100644 --- a/plugins/lasersensor/src/LaserSensor.cc +++ b/plugins/lasersensor/src/LaserSensor.cc @@ -97,12 +97,20 @@ void GazeboYarpLaserSensor::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd driver_properties.put(YarpLaserSensorScopedName.c_str(), m_sensorName.c_str()); #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS - //Open the wrapper - wrapper_properties.put("device","Rangefinder2DWrapper"); - if( m_laserWrapper.open(wrapper_properties) ) { - } else + bool disable_wrapper = driver_properties.check("disableImplicitNetworkWrapper"); + if (!disable_wrapper) { + //Open the wrapper + wrapper_properties.put("device","Rangefinder2DWrapper"); + if( m_laserWrapper.open(wrapper_properties) ) { + } else + { + yCError(GAZEBOLASER)<<"Plugin failed: error in opening yarp driver wrapper"; + return; + } + } + if (disable_wrapper && !driver_properties.check("yarpDeviceName")) { - yCError(GAZEBOLASER)<<"Plugin failed: error in opening yarp driver wrapper"; + yCError(GAZEBOLASER) << "GazeboYarpLaserSensor : missing yarpDeviceName parameter for device" << m_sensorName; return; } #endif @@ -118,21 +126,24 @@ void GazeboYarpLaserSensor::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd } #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS - //Attach the driver to the wrapper ::yarp::dev::PolyDriverList driver_list; - - if( !m_laserWrapper.view(m_iWrap) ) + if (!disable_wrapper) { - yCError(GAZEBOLASER) << "GazeboYarpLaserSensor : error in loading wrapper" ; - return; - } + //Attach the driver to the wrapper - driver_list.push(&m_laserDriver, "lasersensor"); + if( !m_laserWrapper.view(m_iWrap) ) + { + yCError(GAZEBOLASER) << "GazeboYarpLaserSensor : error in loading wrapper" ; + return; + } - if( m_iWrap->attachAll(driver_list) ) { - } else - { - yCError(GAZEBOLASER) << "GazeboYarpLaserSensor : error in connecting wrapper and device " ; + driver_list.push(&m_laserDriver, "lasersensor"); + + if( m_iWrap->attachAll(driver_list) ) { + } else + { + yCError(GAZEBOLASER) << "GazeboYarpLaserSensor : error in connecting wrapper and device " ; + } } #endif From fbe000a2ee3ff946609e78ae29f000f55a271332 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Mon, 28 Feb 2022 10:08:49 +0100 Subject: [PATCH 4/5] Fix Windows Conda CI jon (#610) --- .github/workflows/conda-ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/conda-ci.yml b/.github/workflows/conda-ci.yml index 3ddf8f838..adebef9d9 100644 --- a/.github/workflows/conda-ci.yml +++ b/.github/workflows/conda-ci.yml @@ -15,7 +15,7 @@ jobs: strategy: matrix: build_type: [Release] - os: [ubuntu-latest, windows-latest, macos-latest] + os: [ubuntu-latest, windows-2019, macos-latest] fail-fast: false steps: From 4831bb4939cd73d046fa7afaf6b5f2e37d8cd0fb Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Mon, 28 Feb 2022 10:28:48 +0100 Subject: [PATCH 5/5] Fix release date of 4.2.0 --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index b37ba93bf..70dfab5bc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,7 +5,7 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo ## [Unreleased] -## [4.2.0] - 2022-02-25 +## [4.2.0] - 2022-02-28 ### Added - Added `disableImplicitNetworkWrapper` option to `gazebo_yarp_forcetorque`, `gazebo_yarp_depthcamera`, `gazebo_yarp_lasersensor`, `gazebo_yarp_imu` and `gazebo_yarp_controlboard` plugins. If present, this option