behavior.hpp

// Copyright 2025 Robin Müller
//
// 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.
 
#pragma once
 
#include <filesystem>
#include <fstream>
#include <set>
#include <string>
#include <type_traits>
#include <vector>
 
#include "ament_index_cpp/get_resource.hpp"
#include "auto_apms_behavior_tree_core/exceptions.hpp"
#include "auto_apms_behavior_tree_core/node/node_manifest.hpp"
#include "auto_apms_util/resource.hpp"
#include "auto_apms_util/string.hpp"
#include "auto_apms_util/yaml.hpp"
 
namespace auto_apms_behavior_tree::core
{
 
// clang-format off

struct BehaviorResourceIdentity
// clang-format on
{
  BehaviorResourceIdentity(const std::string & identity);

  BehaviorResourceIdentity(const std::string & identity, const std::string & default_category);

  BehaviorResourceIdentity(const char * identity);

  BehaviorResourceIdentity() = default;
 
  virtual ~BehaviorResourceIdentity() = default;
 
  bool operator==(const BehaviorResourceIdentity & other) const;
 
  bool operator<(const BehaviorResourceIdentity & other) const;

  std::string str() const;

  bool empty() const;

  std::string category_name;
  std::string package_name;
  std::string behavior_alias;
};

std::set<BehaviorResourceIdentity> getBehaviorResourceIdentities(
  const std::set<std::string> & include_categories = {}, bool include_internal = false,
  const std::set<std::string> & exclude_packages = {});

template <class T, typename = std::enable_if_t<std::is_base_of_v<BehaviorResourceIdentity, T>>>
class BehaviorResourceTemplate
{
public:
  using Identity = T;

  BehaviorResourceTemplate(const Identity & search_identity);

  BehaviorResourceTemplate(const std::string & search_identity);

  BehaviorResourceTemplate(const char * search_identity);
 
  virtual ~BehaviorResourceTemplate() = default;

  static BehaviorResourceTemplate find(
    const std::string & behavior_alias, const std::string & package_name = "", const std::string & category_name = "");

  const Identity & getIdentity() const;

  const std::string & getBuildRequest() const;

  const std::string & getDefaultBuildHandlerName() const;

  const std::string & getEntryPoint() const;

  const NodeManifest & getNodeManifest() const;
 
protected:
  Identity unique_identity_;
  std::string build_request_file_path_;
  std::string build_request_;
  std::string default_build_handler_;
  std::string entry_point_;
  NodeManifest node_manifest_;
};

class BehaviorResource : public BehaviorResourceTemplate<BehaviorResourceIdentity>
{
public:
  using BehaviorResourceTemplate::BehaviorResourceTemplate;
};
 
// #####################################################################################################################
// ################################              DEFINITIONS              ##############################################
// #####################################################################################################################
 
template <class T, typename U>
inline BehaviorResourceTemplate<T, U>::BehaviorResourceTemplate(const Identity & search_identity)
{
  if (search_identity.empty()) {
    throw auto_apms_util::exceptions::ResourceIdentityFormatError(
      "Cannot create behavior resource with empty identity.");
  }
 
  std::set<std::string> search_packages;
  if (!search_identity.package_name.empty()) {
    search_packages.insert(search_identity.package_name);
  } else {
    search_packages =
      auto_apms_util::getPackagesWithResourceType(_AUTO_APMS_BEHAVIOR_TREE_CORE__RESOURCE_TYPE_NAME__BEHAVIOR);
  }
 
  size_t matching_count = 0;
  for (const auto & p : search_packages) {
    const ament_index_cpp::PathWithResource resource =
      ament_index_cpp::get_resource(_AUTO_APMS_BEHAVIOR_TREE_CORE__RESOURCE_TYPE_NAME__BEHAVIOR, p);
    if (resource.resourcePath) {
      const std::string & content = resource.contents;
      const std::string base_path = resource.resourcePath->string();
      for (const auto & line :
           auto_apms_util::splitString(content, _AUTO_APMS_BEHAVIOR_TREE_CORE__RESOURCE_MARKER_FILE_LINE_SEP)) {
        const std::vector<std::string> parts = auto_apms_util::splitString(
          line, _AUTO_APMS_BEHAVIOR_TREE_CORE__RESOURCE_MARKER_FILE_FIELD_PER_LINE_SEP, false);
        if (parts.size() != 6) {
          throw auto_apms_util::exceptions::ResourceError(
            "Invalid behavior tree resource file (Package: '" + p + "'). Invalid line: " + line + ".");
        }
 
        // Determine if resource is matching
        std::string found_category = parts[0];
        std::string found_alias = parts[1];
        if (search_identity.category_name.empty()) {
          // Disregard the category if not provided with the identity
          if (found_alias != search_identity.behavior_alias) {
            continue;
          }
        } else if (found_category != search_identity.category_name || found_alias != search_identity.behavior_alias) {
          continue;
        }
 
        // Found matching resource - Increase counter
        matching_count++;
 
        // Now fill the other member variables in case the resource matches (if match is not unique, error is thrown
        // later and the object is discarded)
 
        // Store behavior category
        unique_identity_.category_name = found_category;
 
        // Store behavior alias
        unique_identity_.behavior_alias = found_alias;
 
        // Store package name
        unique_identity_.package_name = p;
 
        // Store default build handler
        default_build_handler_ = parts[2];
 
        // Store build request
        build_request_file_path_ = base_path + "/" + parts[3];
        if (std::filesystem::is_regular_file(build_request_file_path_)) {
          std::ifstream file(build_request_file_path_);
          if (!file) {
            throw auto_apms_util::exceptions::ResourceError(
              "Failed to open behavior resource file '" + build_request_file_path_ + "'");
          }
          build_request_ = std::string((std::istreambuf_iterator<char>(file)), std::istreambuf_iterator<char>());
        } else {
          build_request_file_path_ = "";
          build_request_ = parts[3];
        }
 
        // Store entry_point
        entry_point_ = parts[4];
 
        // Store node manifest paths
        std::vector<std::string> node_manifest_paths;
        for (const std::string & path : auto_apms_util::splitString(parts[5], ";", true)) {
          node_manifest_paths.push_back(std::filesystem::path(path).is_absolute() ? path : (base_path + "/" + path));
        }
        node_manifest_ = NodeManifest::fromFiles(node_manifest_paths);
      }
    }
  }
 
  if (matching_count == 0) {
    throw auto_apms_util::exceptions::ResourceError(
      "No behavior resource with identity '" + search_identity.str() + "' was registered.");
  }
  if (matching_count > 1) {
    throw auto_apms_util::exceptions::ResourceError(
      "Behavior resource identity '" + search_identity.str() + "' is ambiguous. You must be more precise.");
  }
}
 
template <class T, typename U>
inline BehaviorResourceTemplate<T, U>::BehaviorResourceTemplate(const std::string & identity)
: BehaviorResourceTemplate(Identity(identity))
{
}
 
template <class T, typename U>
inline BehaviorResourceTemplate<T, U>::BehaviorResourceTemplate(const char * identity)
: BehaviorResourceTemplate(std::string(identity))
{
}
 
template <class T, typename U>
inline BehaviorResourceTemplate<T, U> BehaviorResourceTemplate<T, U>::find(
  const std::string & behavior_alias, const std::string & package_name, const std::string & category_name)
{
  return BehaviorResourceTemplate(
    category_name + _AUTO_APMS_BEHAVIOR_TREE_CORE__RESOURCE_IDENTITY_CATEGORY_SEP + package_name +
    _AUTO_APMS_BEHAVIOR_TREE_CORE__RESOURCE_IDENTITY_ALIAS_SEP + behavior_alias);
}
 
template <class T, typename U>
inline const T & BehaviorResourceTemplate<T, U>::getIdentity() const
{
  return unique_identity_;
}
 
template <class T, typename U>
inline const std::string & BehaviorResourceTemplate<T, U>::getBuildRequest() const
{
  return build_request_;
}
 
template <class T, typename U>
inline const std::string & BehaviorResourceTemplate<T, U>::getDefaultBuildHandlerName() const
{
  return default_build_handler_;
}
 
template <class T, typename U>
inline const std::string & BehaviorResourceTemplate<T, U>::getEntryPoint() const
{
  return entry_point_;
}
 
template <class T, typename U>
inline const NodeManifest & BehaviorResourceTemplate<T, U>::getNodeManifest() const
{
  return node_manifest_;
}
 
}  // namespace auto_apms_behavior_tree::core

namespace YAML
{
template <>
struct convert<auto_apms_behavior_tree::core::BehaviorResourceIdentity>
{
  using Identity = auto_apms_behavior_tree::core::BehaviorResourceIdentity;
  inline static Node encode(const Identity & rhs)
  {
    Node node;
    node = rhs.str();
    return node;
  }
  inline static bool decode(const Node & node, Identity & rhs)
  {
    if (!node.IsScalar()) return false;
    rhs = Identity(node.Scalar());
    return true;
  }
};
}  // namespace YAML