kinematics_plugin_factory.h

Go to the documentation of this file.

 
#ifndef TESSERACT_KINEMATICS_KINEMATICS_PLUGIN_FACTORY_H
#define TESSERACT_KINEMATICS_KINEMATICS_PLUGIN_FACTORY_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <string>
#include <memory>
#include <map>
#include <yaml-cpp/yaml.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_scene_graph/fwd.h>
#include <tesseract_state_solver/fwd.h>
#include <tesseract_common/fwd.h>
#include <tesseract_common/plugin_info.h>
#include <boost_plugin_loader/plugin_loader.hpp>
#include <boost_plugin_loader/macros.h>
#include <filesystem>
 
// clang-format off
#define TESSERACT_ADD_FWD_KIN_PLUGIN(DERIVED_CLASS, ALIAS)                                                             \
  EXPORT_CLASS_SECTIONED(DERIVED_CLASS, ALIAS, FwdKin)
 
#define TESSERACT_ADD_INV_KIN_PLUGIN(DERIVED_CLASS, ALIAS)                                                             \
  EXPORT_CLASS_SECTIONED(DERIVED_CLASS, ALIAS, InvKin)
// clang-format on
 
namespace tesseract_kinematics
{
class KinematicsPluginFactory;
class InverseKinematics;
class ForwardKinematics;
 
class InvKinFactory
{
public:
  using Ptr = std::shared_ptr<InvKinFactory>;
  using ConstPtr = std::shared_ptr<const InvKinFactory>;
 
  virtual ~InvKinFactory() = default;
 
  virtual std::unique_ptr<InverseKinematics> create(const std::string& solver_name,
                                                    const tesseract_scene_graph::SceneGraph& scene_graph,
                                                    const tesseract_scene_graph::SceneState& scene_state,
                                                    const KinematicsPluginFactory& plugin_factory,
                                                    const YAML::Node& config) const = 0;
 
protected:
  static std::string getSection();
  friend class boost_plugin_loader::PluginLoader;
};
 
class FwdKinFactory
{
public:
  using Ptr = std::shared_ptr<FwdKinFactory>;
  using ConstPtr = std::shared_ptr<const FwdKinFactory>;
 
  virtual ~FwdKinFactory() = default;
 
  virtual std::unique_ptr<ForwardKinematics> create(const std::string& solver_name,
                                                    const tesseract_scene_graph::SceneGraph& scene_graph,
                                                    const tesseract_scene_graph::SceneState& scene_state,
                                                    const KinematicsPluginFactory& plugin_factory,
                                                    const YAML::Node& config) const = 0;
 
protected:
  static std::string getSection();
  friend class boost_plugin_loader::PluginLoader;
};
 
class KinematicsPluginFactory
{
public:
  KinematicsPluginFactory();
  ~KinematicsPluginFactory();
  KinematicsPluginFactory(const KinematicsPluginFactory&) = default;
  KinematicsPluginFactory& operator=(const KinematicsPluginFactory&) = default;
  KinematicsPluginFactory(KinematicsPluginFactory&&) = default;
  KinematicsPluginFactory& operator=(KinematicsPluginFactory&&) = default;
 
  KinematicsPluginFactory(YAML::Node config, const tesseract_common::ResourceLocator& locator);
 
  KinematicsPluginFactory(const std::filesystem::path& config, const tesseract_common::ResourceLocator& locator);
 
  KinematicsPluginFactory(const std::string& config, const tesseract_common::ResourceLocator& locator);
 
  void addSearchPath(const std::string& path);
 
  std::set<std::string> getSearchPaths() const;
 
  void addSearchLibrary(const std::string& library_name);
 
  std::set<std::string> getSearchLibraries() const;
 
  void addFwdKinPlugin(const std::string& group_name,
                       const std::string& solver_name,
                       tesseract_common::PluginInfo plugin_info);
 
  std::map<std::string, tesseract_common::PluginInfoContainer> getFwdKinPlugins() const;
 
  void removeFwdKinPlugin(const std::string& group_name, const std::string& solver_name);
 
  void setDefaultFwdKinPlugin(const std::string& group_name, const std::string& solver_name);
 
  std::string getDefaultFwdKinPlugin(const std::string& group_name) const;
 
  void addInvKinPlugin(const std::string& group_name,
                       const std::string& solver_name,
                       tesseract_common::PluginInfo plugin_info);
 
  std::map<std::string, tesseract_common::PluginInfoContainer> getInvKinPlugins() const;
 
  void removeInvKinPlugin(const std::string& group_name, const std::string& solver_name);
 
  void setDefaultInvKinPlugin(const std::string& group_name, const std::string& solver_name);
 
  std::string getDefaultInvKinPlugin(const std::string& group_name) const;
 
  std::unique_ptr<ForwardKinematics> createFwdKin(const std::string& group_name,
                                                  const std::string& solver_name,
                                                  const tesseract_scene_graph::SceneGraph& scene_graph,
                                                  const tesseract_scene_graph::SceneState& scene_state) const;
 
  std::unique_ptr<InverseKinematics> createInvKin(const std::string& group_name,
                                                  const std::string& solver_name,
                                                  const tesseract_scene_graph::SceneGraph& scene_graph,
                                                  const tesseract_scene_graph::SceneState& scene_state) const;
 
  std::unique_ptr<ForwardKinematics> createFwdKin(const std::string& solver_name,
                                                  const tesseract_common::PluginInfo& plugin_info,
                                                  const tesseract_scene_graph::SceneGraph& scene_graph,
                                                  const tesseract_scene_graph::SceneState& scene_state) const;
 
  std::unique_ptr<InverseKinematics> createInvKin(const std::string& solver_name,
                                                  const tesseract_common::PluginInfo& plugin_info,
                                                  const tesseract_scene_graph::SceneGraph& scene_graph,
                                                  const tesseract_scene_graph::SceneState& scene_state) const;
 
  void saveConfig(const std::filesystem::path& file_path) const;
 
  YAML::Node getConfig() const;
 
private:
  mutable std::map<std::string, FwdKinFactory::Ptr> fwd_kin_factories_;
  mutable std::map<std::string, InvKinFactory::Ptr> inv_kin_factories_;
  std::map<std::string, tesseract_common::PluginInfoContainer> fwd_plugin_info_;
  std::map<std::string, tesseract_common::PluginInfoContainer> inv_plugin_info_;
  boost_plugin_loader::PluginLoader plugin_loader_;
 
  void loadConfig(const YAML::Node& config);
};
 
}  // namespace tesseract_kinematics
#endif  // TESSERACT_KINEMATICS_KINEMATICS_PLUGIN_FACTORY_H