Fix behaviour of MultipleAnalogSensor get**Name methods in nested mod…

…els (#633)
robotology · Aug 29, 2022 · 48f0da6 · 48f0da6
1 parent 5c2ce84
commit 48f0da6
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 3 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -5,6 +5,9 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ## [Unreleased]
 
+### Fixed
+- Fix behaviour of MultipleAnalogSensor `get**Name` methods in nested models (https://github.com/robotology/gazebo-yarp-plugins/pull/633).
+
 ## [4.5.0] - 2022-08-23
 
 ### Added

diff --git a/libraries/common/include/GazeboYarpPlugins/common.h b/libraries/common/include/GazeboYarpPlugins/common.h
@@ -57,6 +57,22 @@ namespace GazeboYarpPlugins {
             return false;
         }
     }
+
+    /**
+     * \brief Get last part of string after separator
+     * \param[in] fullString the full string
+     * \param[in] separator separator string
+     * \return lastPart the last part of the string, or the fullString if the seperator is not found
+     */
+    inline std::string lastPartOfStringAfterSeparator(std::string const &fullString, std::string const &separator)
+    {
+        auto pos = fullString.find_last_of(separator);
+        if (pos == std::string::npos) {
+            return fullString;
+        } else {
+            return fullString.substr(pos + separator.size() - 1); 
+        }
+    }
 }
 
 

diff --git a/plugins/forcetorque/src/ForceTorqueDriver.cpp b/plugins/forcetorque/src/ForceTorqueDriver.cpp
@@ -7,6 +7,7 @@
 
 #include "ForceTorqueDriver.h"
 #include <GazeboYarpPlugins/Handler.hh>
+#include <GazeboYarpPlugins/common.h>
 
 #include <boost/bind/bind.hpp>
 #include <gazebo/sensors/ForceTorqueSensor.hh>
@@ -63,7 +64,7 @@ bool GazeboYarpForceTorqueDriver::open(yarp::os::Searchable& config)
     //Get gazebo pointers
     std::string sensorScopedName(config.find(YarpForceTorqueScopedName.c_str()).asString().c_str());
 
-    m_sensorName = config.find("sensor_name").asString();
+    m_sensorName = GazeboYarpPlugins::lastPartOfStringAfterSeparator(config.find("sensor_name").asString(), "::");
     m_frameName = m_sensorName;
 
     m_parentSensor = dynamic_cast<gazebo::sensors::ForceTorqueSensor*>(GazeboYarpPlugins::Handler::getHandler()->getSensor(sensorScopedName));

diff --git a/plugins/imu/src/IMUDriver.cpp b/plugins/imu/src/IMUDriver.cpp
@@ -78,8 +78,8 @@ bool GazeboYarpIMUDriver::open(yarp::os::Searchable& config)
     std::string sensorScopedName(config.find(YarpIMUScopedName).asString());
     std::string sensorName(config.find("sensor_name").asString());
 
-    m_sensorName=sensorName;
-    m_frameName=sensorName;
+    m_sensorName = GazeboYarpPlugins::lastPartOfStringAfterSeparator(sensorName, "::");
+    m_frameName = GazeboYarpPlugins::lastPartOfStringAfterSeparator(sensorName, "::");
 
     m_parentSensor = dynamic_cast<gazebo::sensors::ImuSensor*>(GazeboYarpPlugins::Handler::getHandler()->getSensor(sensorScopedName));