Skip to content

Commit

Permalink
Merge pull request #611 from robotology/devel
Browse files Browse the repository at this point in the history 
Merge devel in master and release v4.2.0
  • Loading branch information
@traversaro
traversaro authored Feb 28, 2022
2 parents bb5afed + 4831bb4 commit c395265
Show file tree
Hide file tree
Showing 15 changed files with 401 additions and 255 deletions.
2 changes: 1 addition & 1 deletion .github/workflows/conda-ci.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
9 changes: 9 additions & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,17 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo

## [Unreleased]

## [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
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).

## [4.1.2] - 2022-01-19

Expand Down
4 changes: 2 additions & 2 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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})
Expand Down
3 changes: 1 addition & 2 deletions plugins/controlboard/include/gazebo/ControlBoard.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
291 changes: 171 additions & 120 deletions plugins/controlboard/src/ControlBoard.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string>("initialConfiguration");
m_parameters.put("initialConfiguration", configuration_s.c_str());
//yCDebug(GAZEBOCONTROLBOARD)<<configuration_s;
// initially deal with virtual analog stuff
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));
}

newPoly.poly = new yarp::dev::PolyDriver;
if(! newPoly.poly->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; idx<m_controlBoards.size(); idx++)
scopedDeviceName = m_robotName + "::" + newPoly.key.c_str();
}
else
{
scopedDeviceName = m_robotName + "::" + m_parameters.find("yarpDeviceName").asString();
}

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.";
}
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<std::string>("initialConfiguration");
m_parameters.put("initialConfiguration", configuration_s.c_str());
//yCDebug(GAZEBOCONTROLBOARD)<<configuration_s;
}

newPoly.poly = new yarp::dev::PolyDriver;
if(! newPoly.poly->open(m_parameters) || ! newPoly.poly->isValid())
{
m_controlBoards[idx]->poly->close();
yCError(GAZEBOCONTROLBOARD) << newPoly.key << "> did not open.";
for(int idx=0; idx<m_controlBoards.size(); idx++)
{
m_controlBoards[idx]->poly->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:"<<m_parameters.toString();

if (_sdf->HasElement("initialConfiguration")) {
//yCDebug(GAZEBOCONTROLBOARD)<<"Found initial Configuration: ";
std::string configuration_s = _sdf->Get<std::string>("initialConfiguration");
m_parameters.put("initialConfiguration", configuration_s.c_str());
//yCDebug(GAZEBOCONTROLBOARD)<<configuration_s;
}

if (!m_iVirtAnalogSensorWrap->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"))
Expand Down
Loading

0 comments on commit c395265

Please sign in to comment.