Get names of parameters and groups 2/2

YarpImplementation

src/ParametersHandler/YarpImplementation/include/BipedalLocomotion/ParametersHandler/YarpImplementation.h

@@ -31,10 +31,11 @@ namespace ParametersHandler
 class YarpImplementation : public IParametersHandler
 {
 
-    yarp::os::Bottle m_container; /**< Bottle object */
-    std::unordered_map<std::string, std::shared_ptr<YarpImplementation>> m_lists; /**< Map containing
-                                                                                     pointers to the
-                                                                                     (asked) groups */
+    /**  Object containing the top-level scalar and vector parameters */
+    yarp::os::Bottle m_container;
+
+    /** Map containing pointers to the groups */
+    std::unordered_map<std::string, std::shared_ptr<YarpImplementation>> m_groups;
 
     /**
      * Private implementation of getParameter
@@ -228,7 +229,13 @@ class YarpImplementation : public IParametersHandler
      * Return the names of all the stored parameters.
      * @return A set containing the names of all stored parameters.
      */
-    std::unordered_set<std::string> getParametersNames() const final;
+    std::unordered_set<std::string> getParameterNames() const final;
+
+    /**
+     * Return the names of all the stored groups.
+     * @return A set containing the names of all stored groups.
+     */
+    std::unordered_set<std::string> getGroupNames() const final;
 
     /**
      * Check if the handler contains parameters

src/ParametersHandler/YarpImplementation/include/BipedalLocomotion/ParametersHandler/YarpImplementation.tpp

@@ -10,8 +10,8 @@
 
 #include <type_traits>
 
-#include <BipedalLocomotion/YarpUtilities/Helper.h>
 #include <BipedalLocomotion/ParametersHandler/YarpImplementation.h>
+#include <BipedalLocomotion/YarpUtilities/Helper.h>
 
 namespace BipedalLocomotion
 {
@@ -21,56 +21,66 @@ namespace ParametersHandler
 template <typename T>
 bool YarpImplementation::getParameterPrivate(const std::string& parameterName, T& parameter) const
 {
-    if (m_lists.find(parameterName) != m_lists.end()) // A list is called with the same name of the parameter we are searching
-    {
-        return m_lists.at(parameterName)->getParameter(parameterName, parameter);
-    }
+    if constexpr (std::is_scalar<T>::value || is_string<T>::value)
+        // Scalars and strings are retrieved using getElementFromSearchable() function
+        return YarpUtilities::getElementFromSearchable(m_container, parameterName, parameter);
     else
-    {
-        // a scalar element and a strings is retrieved using getElementFromSearchable() function
-        if constexpr (std::is_scalar<T>::value || is_string<T>::value)
-            return YarpUtilities::getElementFromSearchable(m_container, parameterName, parameter);
-        else
-            // otherwise it is considered as a vector
-            return YarpUtilities::getVectorFromSearchable(m_container, parameterName, parameter);
-    }
+        // Otherwise the parameter is considered storing a vector
+        return YarpUtilities::getVectorFromSearchable(m_container, parameterName, parameter);
 }
 
 template <typename T>
 void YarpImplementation::setParameterPrivate(const std::string& parameterName, const T& parameter)
 {
-    // a scalar element and a strings is retrieved using getElementFromSearchable() function
+    // Scalars and strings are retrieved using getElementFromSearchable() function
     if constexpr (std::is_scalar<T>::value || is_string<T>::value)
     {
         yarp::os::Value& check = m_container.find(parameterName);
+
         if (check.isNull())
         {
+            // Create the pair (param_name, scalar_value)
             yarp::os::Value newVal;
             yarp::os::Bottle* list = newVal.asList();
+
+            // Fill the pair
             list->add(yarp::os::Value(parameterName));
             list->add(yarp::os::Value(parameter));
+
+            // Add the pair to the Bottle
             m_container.add(newVal);
-        }
-        else
+        } else
         {
+            // Update the Bottle
             check = yarp::os::Value(parameter);
         }
-    }
-    else
+    } else
     {
+        // Initialize the list that will store (param_name, vector_value)
         yarp::os::Value yarpValue;
         auto property = yarpValue.asList();
+
+        // Insert the param_name
         property->add(yarp::os::Value(parameterName));
 
+        // Create the vector_value list
         yarp::os::Value yarpNewList;
         auto newList = yarpNewList.asList();
 
+        // Fill the vector_value list
         for (const auto& v : parameter)
             newList->add(yarp::os::Value(v));
 
+        // Add the vector_value list as second element of the pair
         property->add(yarpNewList);
 
-        m_lists[parameterName] = std::make_shared<YarpImplementation>(yarpValue);
+        // Check if the list already exists
+        yarp::os::Value& check = m_container.find(parameterName);
+
+        if (check.isNull())
+            m_container.add(yarpValue);
+        else
+            check = yarpNewList;
     }
 }
 

src/ParametersHandler/YarpImplementation/src/YarpImplementation.cpp

@@ -114,49 +114,136 @@ YarpImplementation::YarpImplementation(const yarp::os::Searchable& searchable)
 
 void YarpImplementation::set(const yarp::os::Searchable& searchable)
 {
-    clear();
-
-    yarp::os::Bottle bot;
-    bot.fromString(searchable.toString());
+    // Convert the searchable to Bottle
+    const yarp::os::Bottle bottle(searchable.toString());
 
-    for (size_t i = 0; i < bot.size(); i++) // all sublists are included in a new object
+    // Validate the Searchable before processing it
+    for (size_t i = 0; i < bottle.size(); ++i)
     {
-        yarp::os::Value& bb = bot.get(i);
-
-        yarp::os::Bottle* sub = bb.asList();
-        if ((sub) && (sub->size() > 1))
+        if (!bottle.get(i).isList())
         {
-            std::string name = sub->get(0).toString();
-            yarp::os::Bottle* subSub = sub->get(1).asList();
-            if ((subSub) && (subSub->size() > 1))
-            {
-                m_lists.emplace(name, std::make_shared<YarpImplementation>(*sub));
-            } else
-            {
-                m_container.add(bb);
-            }
-
-        } else
+            std::cerr << "Searchable not valid: " << bottle.get(i).toString() << std::endl;
+            return;
+        }
+    }
+
+    // Clear the handler
+    clear();
+
+    enum class Type
+    {
+        GROUP,
+        SCALAR,
+        VECTOR,
+    };
+
+    // Loop over pairs:
+    // 1. (name, scalar_value)
+    // 2. (name, vector_value)
+    // 3. (name, group_value1, group_value2, ...)
+    for (size_t i = 0; i < bottle.size(); ++i)
+    {
+        // Get the pair
+        const yarp::os::Value& pair = bottle.get(i);
+
+        // Parse the pair into (name, value)
+        std::string name;
+        yarp::os::Value value;
+
+        const auto type = [&pair, &name, &value]() {
+            // Unpack the pair
+            auto* list = pair.asList();
+            // const std::string name = list->get(0).asString();
+            // const yarp::os::Value& value = list->get(1);
+            name = list->get(0).asString();
+            value = list->get(1);
+
+            // If there are more than 2 elements is for sure a group
+            if (pair.asList()->size() > 2)
+                return Type::GROUP;
+
+            // If the value is not a list, it is a scalar parameter
+            if (!value.isList())
+                return Type::SCALAR;
+
+            // If the value is a list, is a vector parameter.
+            // Let's leave out the case of two elements.
+            if (value.asList()->size() != 2)
+                return Type::VECTOR;
+
+            // Now it could be either a group with one pair of parameters or
+            // a vector parameter with 2 elements:
+            //      GROUP_NAME (param1 value1)
+            //      param_name (elem1 elem2)
+
+            // If the first element of the value is not a string, it cannot be a group
+            if (!value.asList()->get(0).isString())
+                return Type::VECTOR;
+
+            // This last case is the most problematic because we cannot be sure.
+            // Let's decide that it is a group if the name is uppercase.
+            if (std::all_of(name.begin(), name.end(), [](unsigned char c) {
+                    return std::isupper(c) || c == '_' || c == '-';
+                }))
+                return Type::GROUP;
+
+            // It is a parameter that stores a vector of strings
+            return Type::VECTOR;
+        }();
+
+        switch (type)
         {
-            m_container.add(bb);
+        case Type::SCALAR:
+        case Type::VECTOR: {
+            // Skip special parameter of RF
+            if (name == "from")
+                continue;
+
+            // Create the pair (param_name, value)
+            yarp::os::Value newVal;
+            yarp::os::Bottle* list = newVal.asList();
+
+            // Fill the pair
+            list->add(yarp::os::Value(name));
+            list->add(value);
+
+            m_container.add(newVal);
+            break;
+        }
+        case Type::GROUP: {
+            // Create a new list containing all the group parameters
+            yarp::os::Value newList;
+            yarp::os::Bottle* newListBottle = newList.asList();
+
+            // Leave out the first element of the pair since it is the name of the group
+            for (size_t i = 1; i < pair.asList()->size(); ++i)
+                newListBottle->add(pair.asList()->get(i));
+
+            // Store the new group in the map
+            m_groups[name] = std::make_shared<YarpImplementation>(*newListBottle);
+            break;
+        }
         }
     }
 }
 
 YarpImplementation::weak_ptr YarpImplementation::getGroup(const std::string& name) const
 {
-    if (m_lists.find(name) != m_lists.end())
+    if (m_groups.find(name) != m_groups.end())
     {
-        return m_lists.at(name);
+        std::cout << "[getGroup] Returning: " << name << std::endl;
+        return m_groups.at(name);
     }
 
+    // Return an expired handler
     return std::make_shared<YarpImplementation>();
 }
 
 bool YarpImplementation::setGroup(const std::string& name, IParametersHandler::shared_ptr newGroup)
 {
-    auto downcastedPtr = std::dynamic_pointer_cast<YarpImplementation>(newGroup); // to access
-                                                                                  // m_container
+    // Downcast to YarpImplementation to access the storage
+    auto downcastedPtr = std::dynamic_pointer_cast<YarpImplementation>(newGroup);
+
     if (downcastedPtr == nullptr)
     {
         std::cerr << "[YarpImplementation::setGroup] Unable to downcast the pointer to "
@@ -165,50 +252,64 @@ bool YarpImplementation::setGroup(const std::string& name, IParametersHandler::s
         return false;
     }
 
-    yarp::os::Bottle backup = downcastedPtr->m_container;
-    yarp::os::Bottle nameAdded;
-    nameAdded.add(yarp::os::Value(name));
-    nameAdded.append(backup);
-    downcastedPtr->m_container = nameAdded; // This is all to add the name at the beginning of the
-                                            // bottle
-    m_lists[name] = downcastedPtr;
+    // Add the group to the group map
+    m_groups[name] = downcastedPtr;
 
     return true;
 }
 
 std::string YarpImplementation::toString() const
 {
     std::string output = m_container.toString();
-    for (auto& group : m_lists)
-    {
-        output += " (" + group.second->toString() + ")";
-    }
+
+    for (const auto& [name, group] : m_groups)
+        output += " (" + name + " " + group->toString() + ")";
+
     return output;
 }
 
-std::unordered_set<std::string> YarpImplementation::getParametersNames() const
+std::unordered_set<std::string> YarpImplementation::getParameterNames() const
 {
+    // Initialize the names with the raw parameters of this object
     std::unordered_set<std::string> names;
-    for (const auto& [name, _] : m_lists)
+
+    // Insert the names of the scalar parameters
+    for (size_t i = 0; i < m_container.size(); i++)
     {
+        // Get the pair
+        const yarp::os::Value& pair = m_container.get(i);
+
+        // Unpack the pair getting the first element that is always the name
+        yarp::os::Bottle* list = pair.asList();
+        const std::string name = list->get(0).asString();
         names.insert(name);
     }
 
     return names;
 }
 
+std::unordered_set<std::string> YarpImplementation::getGroupNames() const
+{
+    std::unordered_set<std::string> names;
+
+    for (const auto& [name, _] : m_groups)
+        names.insert(name);
+
+    return names;
+}
+
 bool YarpImplementation::isEmpty() const
 {
     // We check if the container is null and there are no lists.
     // A special case is when the container has a single string element.
     // This is the case for newly created groups, where the container has only the name of the group
     // itself.
     return ((m_container.size() == 0 || (m_container.size() == 1 && m_container.get(0).isString()))
-            && (m_lists.size() == 0));
+            && (m_groups.size() == 0));
 }
 
 void YarpImplementation::clear()
 {
     m_container.clear();
-    m_lists.clear();
+    m_groups.clear();
 }