helpers.hpp

// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef CONTROLLER_INTERFACE__HELPERS_HPP_
#define CONTROLLER_INTERFACE__HELPERS_HPP_
 
#include <algorithm>
#include <functional>
#include <memory>
#include <stdexcept>
#include <string>
#include <vector>
 
// Add hardware interface helpers here, so all inherited controllers can use them
#include "hardware_interface/helpers.hpp"
 
namespace controller_interface
{
 
template <typename T>
bool get_ordered_interfaces(
  std::vector<T> & unordered_interfaces, const std::vector<std::string> & ordered_names,
  const std::string & interface_type, std::vector<std::reference_wrapper<T>> & ordered_interfaces)
{
  if (ordered_interfaces.capacity() < ordered_names.size())
  {
    throw std::range_error(
      "Capacity of ordered_interfaces (" + std::to_string(ordered_interfaces.capacity()) +
      ") has to be equal or higher as size of ordered_names (" +
      std::to_string(ordered_names.size()) + ") for realtime reasons.");
  }
  for (const auto & name : ordered_names)
  {
    for (auto & interface : unordered_interfaces)
    {
      if (!interface_type.empty())
      {
        // check case where:
        // (<joint> == <joint> AND <interface> == <interface>) OR <joint>/<interface> == 'full name'
        if (
          ((name == interface.get_prefix_name()) &&
           (interface_type == interface.get_interface_name())) ||
          ((name + "/" + interface_type) == interface.get_name()))
        {
          ordered_interfaces.push_back(std::ref(interface));
        }
      }
      else
      {
        if (name == interface.get_name())
        {
          ordered_interfaces.push_back(std::ref(interface));
        }
      }
    }
  }
 
  return ordered_names.size() == ordered_interfaces.size();
}
 
inline bool interface_list_contains_interface_type(
  const std::vector<std::string> & interface_type_list, const std::string & interface_type)
{
  return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) !=
         interface_type_list.end();
}
 
}  // namespace controller_interface
 
#endif  // CONTROLLER_INTERFACE__HELPERS_HPP_