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_kinematics/core/inverse_kinematics.h>
#include <tesseract_kinematics/core/forward_kinematics.h>
#include <tesseract_scene_graph/graph.h>
#include <tesseract_scene_graph/scene_state.h>
#include <tesseract_common/plugin_loader.h>
#include <tesseract_common/types.h>
 
// clang-format off
#define TESSERACT_ADD_FWD_KIN_PLUGIN(DERIVED_CLASS, ALIAS)                                                             \
  TESSERACT_ADD_PLUGIN_SECTIONED(DERIVED_CLASS, ALIAS, FwdKin)
 
#define TESSERACT_ADD_INV_KIN_PLUGIN(DERIVED_CLASS, ALIAS)                                                             \
  TESSERACT_ADD_PLUGIN_SECTIONED(DERIVED_CLASS, ALIAS, InvKin)
// clang-format on
 
namespace tesseract_kinematics
{
class KinematicsPluginFactory;
 
class InvKinFactory
{
public:
  using Ptr = std::shared_ptr<InvKinFactory>;
  using ConstPtr = std::shared_ptr<const InvKinFactory>;
 
  virtual ~InvKinFactory() = default;
 
  virtual InverseKinematics::UPtr 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 const std::string SECTION_NAME;
  friend class PluginLoader;
};
 
class FwdKinFactory
{
public:
  using Ptr = std::shared_ptr<FwdKinFactory>;
  using ConstPtr = std::shared_ptr<const FwdKinFactory>;
 
  virtual ~FwdKinFactory() = default;
 
  virtual ForwardKinematics::UPtr 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 const std::string SECTION_NAME;
  friend class 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);
 
  KinematicsPluginFactory(const tesseract_common::fs::path& config);
 
  KinematicsPluginFactory(const std::string& config);
 
  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;
 
  ForwardKinematics::UPtr 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;
 
  InverseKinematics::UPtr 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;
 
  ForwardKinematics::UPtr 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;
 
  InverseKinematics::UPtr 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 tesseract_common::fs::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_;
  tesseract_common::PluginLoader plugin_loader_;
};
 
}  // namespace tesseract_kinematics
#endif  // TESSERACT_KINEMATICS_KINEMATICS_PLUGIN_FACTORY_H