Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Port gazebo_imu for being used with the gazebo_robotinterface plugin #583

Merged
merged 2 commits into from
Nov 18, 2021
Merged

Port gazebo_imu for being used with the gazebo_robotinterface plugin #583

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bea46d3
gazebo_yarp_imu: add the support for the open without wrapper
Nicogene Nov 18, 2021
ba94f8e
Update changelog
Nicogene Nov 18, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 12 additions & 8 deletions CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,10 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo

## [Unreleased]

### Added
- The `gazebo_imu` plugin now handle the `yarpDeviceName` parameter.
- The `gazebo_imu` plugin can be now opened without implicit/explicit wrapper.

## [4.0.0] - 2021-09-03

### Added
Expand All @@ -21,8 +25,8 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
- Fixed the getRgbIntrinsicParam method in the depthCamera plugin when the distortion is not set (https://github.com/robotology/gazebo-yarp-plugins/pull/558).
- The property returned by `getRgbIntrinsicParam()`, now contains `rectificationMatrix` instead of `rectificationMatrix` (https://github.com/robotology/gazebo-yarp-plugins/pull/558, see also https://github.com/robotology/yarp/pull/2593).

### Removed
- The `gazebo_yarp_jointsensors` and the `gazebo_yarp_doublelaser` have been removed (https://github.com/robotology/gazebo-yarp-plugins/pull/574).
### Removed
- The `gazebo_yarp_jointsensors` and the `gazebo_yarp_doublelaser` have been removed (https://github.com/robotology/gazebo-yarp-plugins/pull/574).
- The support for Gazebo 9 and 10 has been removed. `gazebo-yarp-plugins` now requires Gazebo 11 (https://github.com/robotology/gazebo-yarp-plugins/pull/575).


Expand All @@ -33,7 +37,7 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo

## [3.6.1] - 2021-05-19

### Fixed
### Fixed
- Fixed use of libYARP_robotinterface with YARP devices spawned by sensor plugins (https://github.com/robotology/gazebo-yarp-plugins/pull/544).
- Fixed compilation against YARP 3.5 by removing spurious print in WorldInterface plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/548).

Expand All @@ -47,15 +51,15 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
configuration. This generator enables the trajectory to follow a trapezoidal speed profile in position control mode, limited
by provided reference speed (saturation) and acceleration (both ramps) values. If already executing a trajectory in this manner,
newly generated trajectories take into account previous joint velocities and update the motion accordingly.
- Add `gazebo_yarp_robotinterface` plugin, the documentation for it can be found at [plugins/robotinterface/README.md](plugins/robotinterface/README.md) (https://github.com/robotology/gazebo-yarp-plugins/pull/532).
- Add `gazebo_yarp_robotinterface` plugin, the documentation for it can be found at [plugins/robotinterface/README.md](plugins/robotinterface/README.md) (https://github.com/robotology/gazebo-yarp-plugins/pull/532).
- The `gazebo_yarp_depthcamera` and `gazebo_yarp_doublesensor` now accept a `yarpDeviceName` parameter (https://github.com/robotology/gazebo-yarp-plugins/pull/532).

### Changed
### Changed
- The `deviceId` parameter of the `gazebo_yarp_lasersensor` is now named `yarpDeviceName` )https://github.com/robotology/gazebo-yarp-plugins/pull/532).

### Fixed
- Fix the support for running Gazebo itself with the `gazebo_yarp_clock` with YARP_CLOCK set, without Gazebo freezing at startup.
In particular, setting YARP_CLOCK is suggested to ensure that all the threads of YARP Network Wrapper Servers are executed with
- Fix the support for running Gazebo itself with the `gazebo_yarp_clock` with YARP_CLOCK set, without Gazebo freezing at startup.
In particular, setting YARP_CLOCK is suggested to ensure that all the threads of YARP Network Wrapper Servers are executed with
the frequency correctly synchronized with the Gazebo simulation (https://github.com/robotology/gazebo-yarp-plugins/pull/537).

## [3.5.1] - 2020-10-05
Expand Down
56 changes: 40 additions & 16 deletions plugins/imu/src/IMU.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,9 +67,12 @@ void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
_sensor->SetActive(true);

// Add my gazebo device driver to the factory.
std::string netWrapper{""};
#ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
netWrapper = "inertial";
#endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
::yarp::dev::Drivers::factory().add(new ::yarp::dev::DriverCreatorOf< ::yarp::dev::GazeboYarpIMUDriver>
("gazebo_imu", "inertial", "GazeboYarpIMUDriver"));

("gazebo_imu", netWrapper.c_str(), "GazeboYarpIMUDriver"));
bool configuration_loaded = GazeboYarpPlugins::loadConfigSensorPlugin(_sensor,_sdf,m_parameters);

if (!configuration_loaded) {
Expand All @@ -80,6 +83,7 @@ void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
//Insert the pointer in the singleton handler for retriving it in the yarp driver
GazeboYarpPlugins::Handler::getHandler()->setSensor(_sensor.get());

#ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
/*
* Open the driver wrapper
*/
Expand Down Expand Up @@ -124,32 +128,27 @@ void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
yError() << "GazeboYarpIMU Plugin Load failed: error in opening the yarp wrapper";
}

#endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS

/*
* Open the imu driver
*/
//Retrieve part driver properties
::yarp::os::Bottle imu_properties = m_parameters.findGroup("IMU_DRIVER");
if(imu_properties.isNull())
{
yError("GazeboYarpIMU : [IMU_DRIVER] group not found in config file\n");
return;
}

//Add the model scoped name for later retrieval of the child sensors from the Handler
yarp::os::Bottle& robotNameProp = imu_properties.addList();
robotNameProp.addString(YarpIMUScopedName);
robotNameProp.addString(m_scopedSensorName);
::yarp::os::Property imu_properties { {"device", ::yarp::os::Value{"gazebo_imu"}},
{YarpIMUScopedName, ::yarp::os::Value{m_scopedSensorName}},
{"sensor_name", ::yarp::os::Value{_sensor->Name()}}
};

imu_properties.addString("sensor_name");
imu_properties.addString(_sensor->Name());

//Open the driver
if (m_imuDriver.open(imu_properties)) {
} else {
if (!m_imuDriver.open(imu_properties)) {
yError() << "GazeboYarpIMU Plugin Load failed: error in opening yarp driver";
}

//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";
Expand All @@ -161,6 +160,31 @@ void GazeboYarpIMU::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
{
yError() << "GazeboYarpIMU: error in connecting wrapper and device ";
}

if(!m_parameters.check("yarpDeviceName"))
{
scopedDeviceName = m_scopedSensorName + "::" + driverList[0]->key;
}
else
{
scopedDeviceName = m_scopedSensorName + "::" + m_parameters.find("yarpDeviceName").asString();
}
#else
if(!m_parameters.check("yarpDeviceName"))
{
yError() << "GazeboYarpIMU : missing yarpDeviceName parameter for device" << m_scopedSensorName;
return;
}
scopedDeviceName = m_scopedSensorName + "::" + m_parameters.find("yarpDeviceName").asString();
#endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS


if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_imuDriver))
{
yError()<<"GazeboYarpIMU: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
return;
}
yInfo() << "Registered YARP device with instance name:" << scopedDeviceName;
}

}
15 changes: 8 additions & 7 deletions plugins/robotinterface/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
gazebo_yarp_robotinterface
==========================

The `gazebo_yarp_robotinterface` plugin permits to load several [YARP devices](http://www.yarp.it/git-master/note_devices.html) that can be attached to YARP devices
The `gazebo_yarp_robotinterface` plugin permits to load several [YARP devices](http://www.yarp.it/git-master/note_devices.html) that can be attached to YARP devices
already opened by other Gazebo-YARP plugins using the same XML format used by the [`yarprobotinterface`](http://www.yarp.it/git-master/yarprobotinterface.html) tool and the [`libYARP_robotinterface` C++ library](https://github.com/robotology/yarp/tree/master/src/libYARP_robotinterface).

## Usage
## Usage

### Add the plugin in the SDF model
The `gazebo_yarp_robotinterface` plugin can be used by including in any SDF model:
Expand All @@ -15,7 +15,7 @@ The `gazebo_yarp_robotinterface` plugin can be used by including in any SDF mode

<!-- Insert other plugins -->
<!-- ... -->

<!-- Insert gazebo_yarp_robotinterface -->
<plugin name="robotinterface" filename="libgazebo_yarp_robotinterface.so">
<!-- This file can be specified with the model:// URI already used in Gazebo, see http://gazebosim.org/tutorials?tut=components&cat=get_started -->
Expand Down Expand Up @@ -66,22 +66,23 @@ The main use of the `gazebo_yarp_robotinterface` plugin is to spawn YARP devices
This is achieved by setting the elements in the `<param name="device">` or in the `<paramlist name="networks">` list under the `<action phase="startup" level="5" type="attach">` XML element.
In this context, we call this "device instance identified" as **YARP device instance name**, as for devices created by the robotinterface, this is specified by the `name` attribute of the `device` XML tag. It is important not to confuse this with the **YARP device type name**, i.e. the name that identifies the type of plugin that is spawned, i.e. the `type` attribute of the `device` tag.

The `gazebo_yarp_robotinterface` can be attached to any YARP device created by any plugin inside the model, or in any plugin contained in any nested sensor or model.
The `gazebo_yarp_robotinterface` can be attached to any YARP device created by any plugin inside the model, or in any plugin contained in any nested sensor or model.

For historic reason, not all the `gazebo-yarp-plugins` support specifying the **YARP device instance name** for the device that they spawn to permit to use them with the `gazebo_yarp_robotinterface` plugin. If you need to have this functionality in a specific plugin, feel free to open an issue.
For historic reason, not all the `gazebo-yarp-plugins` support specifying the **YARP device instance name** for the device that they spawn to permit to use them with the `gazebo_yarp_robotinterface` plugin. If you need to have this functionality in a specific plugin, feel free to open an issue.

**Warning: as the YARP device instance name is specified without any specific Gazebo model or sensor namespace, it is important to observe, while using the `gazebo_yarp_robotinterface` plugin, that all the YARP devices contained in the model have a unique YARP device instance name. If this is not the case, the plugin will print a clear error and exit without starting.**

The plugins that spawn YARP devices in a way that they can be then attached to the yarprobotinterface as specified in the following table.
For all the following plugins, the **YARP device instance name** can be specified by the `yarpDeviceName` parameter in the plugin configuration.

If the `GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS` option is set to `OFF` (default value), for the `gazebo_yarp_controlboard` if the `yarpDeviceName` parameter is not specified, for legacy reason the **YARP device instance name** for each created device can also be specified with the `networks` parameter list in the plugin configuration of `yarpDeviceName`. If instead the `GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS` option is set to `ON`, the `gazebo_yarp_controlboard` behaves like the rest of the plugins and requires to take the **YARP device instance name** from the `yarpDeviceName` parameter. The same behaviour applies to `gazebo_yarp_depthCamera`.
If the `GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS` option is set to `OFF` (default value), for the `gazebo_yarp_controlboard` if the `yarpDeviceName` parameter is not specified, for legacy reason the **YARP device instance name** for each created device can also be specified with the `networks` parameter list in the plugin configuration of `yarpDeviceName`. If instead the `GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS` option is set to `ON`, the `gazebo_yarp_controlboard` behaves like the rest of the plugins and requires to take the **YARP device instance name** from the `yarpDeviceName` parameter. The same behaviour applies to the plugins listed in the table below.

Furthermore if the `GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS` option is set to `ON` then the plugins `gazebo_yarp_multicamera`, `gazebo_yarp_lasersensor`, `gazebo_yarp_controlboard` and `gazebo_yarp_depthCamera` doesn't create the implicit wrapper device but is left to the user to attach it via the robotinterface.
Furthermore if the `GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS` option is set to `ON` then the plugins `gazebo_yarp_multicamera`, `gazebo_yarp_lasersensor`, `gazebo_yarp_controlboard`, `gazebo_yarp_depthCamera` and `gazebo_yarp_imu` doesn't create the implicit wrapper device but is left to the user to attach it via the robotinterface.

| Plugin | Details |
|:--------------------------:|:----------------------------------------------------:|
| `gazebo_yarp_controlboard` | This plugin can create multiple YARP devices that expose joint-level motor and control interfaces such as [`yarp::dev::IPositionControl`](https://www.yarp.it/git-master/classyarp_1_1dev_1_1IPositionControl.html), [`yarp::dev::ITorqueControl`](https://www.yarp.it/git-master/classyarp_1_1dev_1_1ITorqueControl.html) and [`yarp::dev::ITorqueControl`](https://www.yarp.it/git-master/classyarp_1_1dev_1_1IEncoders.html). |
| `gazebo_yarp_depthcamera` | This plugin can create a YARP device that expose a depth-camera interface. |
| `gazebo_yarp_lasersensor` | This plugin can create a YARP device that expose a laser-seensor interface. |
| `gazebo_yarp_multicamera` | This plugin can create a YARP device that expose a multicamera interface. |
| `gazebo_yarp_imu` | This plugin can create a YARP device that expose a imu interface. |