Add disableImplicitNetworkWrapper to gazebo_yarp_forcetorque

robotology · Nov 29, 2021 · f708cd5 · f708cd5
1 parent 880af12
commit f708cd5
Showing 1 changed file with 24 additions and 14 deletions.
diff --git 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,18 +109,20 @@ 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 (!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;
-    }
+        if( !m_forcetorqueWrapper.view(m_iWrap) ) {
+            yError() << "GazeboYarpForceTorque : error in loading wrapper";
+            return;
+        }
 
-    driver_list.push(&m_forceTorqueDriver,"dummy");
+        driver_list.push(&m_forceTorqueDriver,"dummy");
 
-    if( !m_iWrap->attachAll(driver_list) ) {
-        yError() << "GazeboYarpForceTorque : error in connecting wrapper and device ";
+        if( !m_iWrap->attachAll(driver_list) ) {
+            yError() << "GazeboYarpForceTorque : error in connecting wrapper and device ";
+        }
     }
 
     if(!driver_properties.check("yarpDeviceName"))