Merge pull request #601 from robotology/devel

Merge devel in master
robotology · Dec 23, 2021 · 6dadd10 · 6dadd10
2 parents ab0306a + a5c5e8f
commit 6dadd10
Show file tree

Hide file tree

Showing 6 changed files with 118 additions and 54 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -5,6 +5,17 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ## [Unreleased]
 
+## [4.1.0] - 2021-12-23
+
+### Changed
+- The `gazebo_yarp_forcetorque` plugin now handle the `yarpDeviceName` parameter (https://github.com/robotology/gazebo-yarp-plugins/pull/584).
+- The `gazebo_yarp_forcetorque` plugin can be now opened without implicit wrapper, by using the `GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS` CMake option (https://github.com/robotology/gazebo-yarp-plugins/pull/584).
+- The `gazebo_imu` plugin now handle the `yarpDeviceName` parameter (https://github.com/robotology/gazebo-yarp-plugins/pull/583).
+- The `gazebo_imu` plugin can be now opened without implicit wrapper, by using the `GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS` CMake option (https://github.com/robotology/gazebo-yarp-plugins/pull/583).
+
+### Fixed
+- Fixed compilation with YARP 3.6 (https://github.com/robotology/gazebo-yarp-plugins/pull/599).
+
 ## [4.0.0] - 2021-09-03
 
 ### Added
@@ -21,8 +32,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).
 
 
@@ -33,7 +44,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).
 
@@ -47,15 +58,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

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -8,7 +8,7 @@ option(GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS "if enabled removes
 
 # Project version
 set(${PROJECT_NAME}_MAJOR_VERSION 4)
-set(${PROJECT_NAME}_MINOR_VERSION 0)
+set(${PROJECT_NAME}_MINOR_VERSION 1)
 set(${PROJECT_NAME}_PATCH_VERSION 0)
 
 set(${PROJECT_NAME}_VERSION

diff --git a/plugins/forcetorque/src/ForceTorque.cc b/plugins/forcetorque/src/ForceTorque.cc
@@ -52,52 +52,55 @@ void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
 
     _sensor->SetActive(true);
 
+    std::string netWrapper {""};
+
+    #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
+    netWrapper = "analogServer";
+    #endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
+
     // Add my gazebo device driver to the factory.
     ::yarp::dev::Drivers::factory().add(new ::yarp::dev::DriverCreatorOf< ::yarp::dev::GazeboYarpForceTorqueDriver>
-                                      ("gazebo_forcetorque", "analogServer", "GazeboYarpForceTorqueDriver"));
-
-    //Getting .ini configuration file from sdf
-    ::yarp::os::Property wrapper_properties;
+                                      ("gazebo_forcetorque", netWrapper.c_str(), "GazeboYarpForceTorqueDriver"));
+
     ::yarp::os::Property driver_properties;
     bool configuration_loaded = GazeboYarpPlugins::loadConfigSensorPlugin(_sensor,_sdf,driver_properties);
 
     if (!configuration_loaded) {
         return;
     };
 
-
+    #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
     ///< \todo TODO handle in a better way the parameters that are for the wrapper and the one that are for driver
-    wrapper_properties = driver_properties;
-
-    if( !configuration_loaded )
-    {
-        return;
-    }
+    ::yarp::os::Property wrapper_properties = driver_properties;
+    #endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
+
     m_sensorName = _sensor->ScopedName();
 
     //Insert the pointer in the singleton handler for retriving it in the yarp driver
     GazeboYarpPlugins::Handler::getHandler()->setSensor(_sensor.get());
 
     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) ) {
-    } else {
+    if( !m_forcetorqueWrapper.open(wrapper_properties) ) {
         yError()<<"GazeboYarpForceTorque Plugin failed: error in opening yarp driver wrapper";
         return;
     }
+    #endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
 
     //Open the driver
     //Force the device to be of type "gazebo_forcetorque" (it make sense? probably yes)
     driver_properties.put("device","gazebo_forcetorque");
-    if( m_forceTorqueDriver.open(driver_properties) ) {
-    } else {
+    if( !m_forceTorqueDriver.open(driver_properties) ) {
         yError()<<"GazeboYarpForceTorque Plugin failed: error in opening yarp driver";
         return;
     }
-
+
+    std::string scopedDeviceName;
+    #ifndef GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
     //Attach the driver to the wrapper
     ::yarp::dev::PolyDriverList driver_list;
 
@@ -108,11 +111,33 @@ void GazeboYarpForceTorque::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
 
     driver_list.push(&m_forceTorqueDriver,"dummy");
 
-    if( m_iWrap->attachAll(driver_list) ) {
-    } else {
+    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"))
+    {
+        yError() << "GazeboYarpForceTorque : missing yarpDeviceName parameter for device" << m_sensorName;
+        return;
+    }
+    scopedDeviceName = m_sensorName + "::" + driver_properties.find("yarpDeviceName").asString();
+    #endif // GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS
+
+    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_forceTorqueDriver))
+    {
+        yError()<<"GazeboYarpForceTorque: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
+        return;
+    }
+    yInfo() << "Registered YARP device with instance name:" << scopedDeviceName;
 }
 
 }
diff --git a/plugins/imu/src/IMU.cc b/plugins/imu/src/IMU.cc
@@ -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) {
@@ -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
     */
@@ -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";
@@ -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;
 }
 
 }
diff --git a/plugins/lasersensor/include/yarp/dev/LaserSensorDriver.h b/plugins/lasersensor/include/yarp/dev/LaserSensorDriver.h
@@ -65,7 +65,9 @@ class yarp::dev::GazeboYarpLaserSensorDriver:
     virtual bool setScanRate (double rate) override;
 
     //PRECISELY TIMED
-    virtual yarp::os::Stamp getLastInputStamp() override;
+    // TODO(traversaro): Remove once we require YARP 3.6
+    // See https://github.com/robotology/gazebo-yarp-plugins/issues/598
+    virtual yarp::os::Stamp getLastInputStamp();
 
 public:
     //Lidar2DDeviceBase