fsb_urdf_name_map.h

 
#pragma once
 
#include <cstddef>
#include <string>
#include <unordered_map>
 
namespace fsb::urdf
{
 
enum class NameMapError
{
    SUCCESS,
    NOT_FOUND,
    EXISTS
};
 
class NameMap
{
public:
    NameMap() = default;
 
    NameMapError add(size_t index, const std::string& name);
 
    size_t get_index(const std::string& name, NameMapError& err) const;
 
    std::string get_name(size_t index, NameMapError& err) const;
 
    [[nodiscard]] bool name_exists(const std::string& name) const
    {
        const auto iter = m_index_map.find(name);
        return (iter != m_index_map.end());
    }
 
    [[nodiscard]] size_t size() const
    {
        return m_index_map.size();
    }
 
private:
    std::unordered_map<size_t, std::string> m_name_map;
    std::unordered_map<std::string, size_t> m_index_map;
};
 
class UrdfNameMap
{
public:
    UrdfNameMap() = default;
 
    void set_robot_name(const std::string& name)
    {
        m_robot_name = name;
    }
 
    NameMapError add_body(const size_t index, const std::string& name)
    {
        return m_bodies.add(index, name);
    }
 
    NameMapError add_joint(const size_t index, const std::string& name)
    {
        return m_joints.add(index, name);
    }
 
    size_t get_body_index(const std::string& name, NameMapError& err) const
    {
        return m_bodies.get_index(name, err);
    }
 
    size_t get_joint_index(const std::string& name, NameMapError& err) const
    {
        return m_joints.get_index(name, err);
    }
 
    [[nodiscard]] bool body_exists(const std::string& name) const
    {
        return m_bodies.name_exists(name);
    }
 
    [[nodiscard]] bool joint_exists(const std::string& name) const
    {
        return m_joints.name_exists(name);
    }
 
    [[nodiscard]] size_t get_num_bodies() const
    {
        return m_bodies.size();
    }
 
    [[nodiscard]] size_t get_num_joints() const
    {
        return m_joints.size();
    }
 
private:
    std::string m_robot_name;
    NameMap     m_bodies;
    NameMap     m_joints;
};
 
inline NameMapError NameMap::add(const size_t index, const std::string& name)
{
    auto err = NameMapError::SUCCESS;
 
    auto name_err = NameMapError::SUCCESS;
    const std::string existing_name = get_name(index, name_err);
    auto index_err = NameMapError::SUCCESS;
    const size_t existing_index = get_index(name, index_err);
    if (name_err == NameMapError::NOT_FOUND && index_err == NameMapError::NOT_FOUND)
    {
        // Name and index do not exist - Add to map
        m_index_map[name] = index;
        m_name_map[index] = name;
    }
    else if ((name_err == NameMapError::EXISTS) &&
             (existing_name == name) &&
             (index_err == NameMapError::EXISTS) &&
             (existing_index == index))
    {
        // no error, already in map
    }
    else
    {
        // name-index pair already exists but does not have the same index and/or name
        err = NameMapError::EXISTS;
    }
    return err;
}
 
inline size_t NameMap::get_index(const std::string& name, NameMapError& err) const
{
    size_t index = 0U;
    if (const auto iter = m_index_map.find(name); iter == m_index_map.end())
    {
        err = NameMapError::NOT_FOUND;
    }
    else
    {
        err = NameMapError::SUCCESS;
        index = iter->second;
    }
    return index;
}
 
inline std::string NameMap::get_name(const size_t index, NameMapError& err) const
{
    std::string name = {};
    if (const auto iter = m_name_map.find(index); iter == m_name_map.end())
    {
        err = NameMapError::NOT_FOUND;
    }
    else
    {
        err = NameMapError::SUCCESS;
        name = iter->second;
    }
    return name;
}
 
} // namespace fsb::urdf