diff --git a/CHANGELOG.md b/CHANGELOG.md index d91352683..62d97a1e3 100644 --- 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 index a30ac3884..0997b24bc 100644 --- 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 index cda03f707..50b1c74bd 100644 --- a/plugins/forcetorque/src/ForceTorqueDriver.cpp +++ b/plugins/forcetorque/src/ForceTorqueDriver.cpp @@ -7,6 +7,7 @@ #include "ForceTorqueDriver.h" #include +#include #include #include @@ -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(GazeboYarpPlugins::Handler::getHandler()->getSensor(sensorScopedName)); diff --git a/plugins/imu/src/IMUDriver.cpp b/plugins/imu/src/IMUDriver.cpp index aa1bcb285..8c0e29b4e 100644 --- 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(GazeboYarpPlugins::Handler::getHandler()->getSensor(sensorScopedName));