environment.h

Go to the documentation of this file.

 
#ifndef TESSERACT_ENVIRONMENT_ENVIRONMENT_H
#define TESSERACT_ENVIRONMENT_ENVIRONMENT_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <functional>
#include <vector>
#include <string>
#include <chrono>
#include <set>
#include <memory>
#include <shared_mutex>
#include <Eigen/Geometry>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_common/fwd.h>
#include <tesseract_geometry/fwd.h>
#include <tesseract_scene_graph/fwd.h>
#include <tesseract_state_solver/fwd.h>
#include <tesseract_srdf/fwd.h>
#include <tesseract_kinematics/core/fwd.h>
#include <tesseract_collision/core/fwd.h>
#include <tesseract_environment/fwd.h>
 
#include <filesystem>
#include <tesseract_common/eigen_types.h>
#include <tesseract_common/any_poly.h>
#include <tesseract_common/contact_allowed_validator.h>
 
namespace boost::serialization
{
class access;
}
 
namespace tesseract_environment
{
using FindTCPOffsetCallbackFn = std::function<Eigen::Isometry3d(const tesseract_common::ManipulatorInfo&)>;
 
using EventCallbackFn = std::function<void(const Event& event)>;
 
class Environment
{
public:
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  using Ptr = std::shared_ptr<Environment>;
  using ConstPtr = std::shared_ptr<const Environment>;
  using UPtr = std::unique_ptr<Environment>;
  using ConstUPtr = std::unique_ptr<const Environment>;
 
  Environment();
  virtual ~Environment();
  Environment(const Environment&) = delete;
  Environment& operator=(const Environment&) = delete;
  Environment(Environment&&) = delete;
  Environment& operator=(Environment&&) = delete;
 
  bool init(const std::vector<std::shared_ptr<const Command>>& commands);
 
  bool init(const tesseract_scene_graph::SceneGraph& scene_graph,
            const std::shared_ptr<const tesseract_srdf::SRDFModel>& srdf_model = nullptr);
 
  bool init(const std::string& urdf_string, const std::shared_ptr<const tesseract_common::ResourceLocator>& locator);
 
  bool init(const std::string& urdf_string,
            const std::string& srdf_string,
            const std::shared_ptr<const tesseract_common::ResourceLocator>& locator);
 
  bool init(const std::filesystem::path& urdf_path,
            const std::shared_ptr<const tesseract_common::ResourceLocator>& locator);
 
  bool init(const std::filesystem::path& urdf_path,
            const std::filesystem::path& srdf_path,
            const std::shared_ptr<const tesseract_common::ResourceLocator>& locator);
 
  Environment::UPtr clone() const;
 
  bool reset();
 
  void clear();
 
  bool isInitialized() const;
 
  int getRevision() const;
 
  int getInitRevision() const;
 
  std::vector<std::shared_ptr<const Command>> getCommandHistory() const;
 
  bool applyCommands(const std::vector<std::shared_ptr<const Command>>& commands);
 
  bool applyCommand(std::shared_ptr<const Command> command);
 
  std::shared_ptr<const tesseract_scene_graph::SceneGraph> getSceneGraph() const;
 
  std::vector<std::string> getGroupJointNames(const std::string& group_name) const;
 
  std::shared_ptr<const tesseract_kinematics::JointGroup> getJointGroup(const std::string& group_name) const;
 
  std::shared_ptr<const tesseract_kinematics::JointGroup>
  getJointGroup(const std::string& name, const std::vector<std::string>& joint_names) const;
 
  std::shared_ptr<const tesseract_kinematics::KinematicGroup>
  getKinematicGroup(const std::string& group_name, const std::string& ik_solver_name = "") const;
 
  Eigen::Isometry3d findTCPOffset(const tesseract_common::ManipulatorInfo& manip_info) const;
 
  void addFindTCPOffsetCallback(const FindTCPOffsetCallbackFn& fn);
 
  std::vector<FindTCPOffsetCallbackFn> getFindTCPOffsetCallbacks() const;
 
  void addEventCallback(std::size_t hash, const EventCallbackFn& fn);
 
  void removeEventCallback(std::size_t hash);
 
  void clearEventCallbacks();
 
  std::map<std::size_t, EventCallbackFn> getEventCallbacks() const;
 
  void setResourceLocator(std::shared_ptr<const tesseract_common::ResourceLocator> locator);
 
  std::shared_ptr<const tesseract_common::ResourceLocator> getResourceLocator() const;
 
  void setName(const std::string& name);
 
  const std::string& getName() const;
 
  void setState(const std::unordered_map<std::string, double>& joints,
                const tesseract_common::TransformMap& floating_joints = {});
  void setState(const std::vector<std::string>& joint_names,
                const Eigen::Ref<const Eigen::VectorXd>& joint_values,
                const tesseract_common::TransformMap& floating_joints = {});
 
  void setState(const tesseract_common::TransformMap& floating_joints);
 
  tesseract_scene_graph::SceneState getState(const std::unordered_map<std::string, double>& joints,
                                             const tesseract_common::TransformMap& floating_joints = {}) const;
  tesseract_scene_graph::SceneState getState(const std::vector<std::string>& joint_names,
                                             const Eigen::Ref<const Eigen::VectorXd>& joint_values,
                                             const tesseract_common::TransformMap& floating_joints = {}) const;
 
  tesseract_scene_graph::SceneState getState(const tesseract_common::TransformMap& floating_joints) const;
 
  tesseract_scene_graph::SceneState getState() const;
 
  std::chrono::system_clock::time_point getTimestamp() const;
 
  std::chrono::system_clock::time_point getCurrentStateTimestamp() const;
 
  std::shared_ptr<const tesseract_scene_graph::Link> getLink(const std::string& name) const;
 
  std::shared_ptr<const tesseract_scene_graph::Joint> getJoint(const std::string& name) const;
 
  std::shared_ptr<const tesseract_scene_graph::JointLimits> getJointLimits(const std::string& joint_name) const;
 
  bool getLinkCollisionEnabled(const std::string& name) const;
 
  bool getLinkVisibility(const std::string& name) const;
 
  std::shared_ptr<const tesseract_common::AllowedCollisionMatrix> getAllowedCollisionMatrix() const;
 
  std::vector<std::string> getJointNames() const;
 
  std::vector<std::string> getActiveJointNames() const;
 
  Eigen::VectorXd getCurrentJointValues() const;
 
  Eigen::VectorXd getCurrentJointValues(const std::vector<std::string>& joint_names) const;
 
  tesseract_common::TransformMap getCurrentFloatingJointValues() const;
 
  tesseract_common::TransformMap getCurrentFloatingJointValues(const std::vector<std::string>& joint_names) const;
 
  std::string getRootLinkName() const;
 
  std::vector<std::string> getLinkNames() const;
 
  std::vector<std::string> getActiveLinkNames() const;
 
  std::vector<std::string> getActiveLinkNames(const std::vector<std::string>& joint_names) const;
 
  std::vector<std::string> getStaticLinkNames() const;
 
  std::vector<std::string> getStaticLinkNames(const std::vector<std::string>& joint_names) const;
 
  tesseract_common::VectorIsometry3d getLinkTransforms() const;
 
  Eigen::Isometry3d getLinkTransform(const std::string& link_name) const;
 
  Eigen::Isometry3d getRelativeLinkTransform(const std::string& from_link_name, const std::string& to_link_name) const;
 
  std::unique_ptr<tesseract_scene_graph::StateSolver> getStateSolver() const;
 
  tesseract_srdf::KinematicsInformation getKinematicsInformation() const;
 
  std::set<std::string> getGroupNames() const;
 
  tesseract_common::ContactManagersPluginInfo getContactManagersPluginInfo() const;
 
  bool setActiveDiscreteContactManager(const std::string& name);
 
  std::unique_ptr<tesseract_collision::DiscreteContactManager> getDiscreteContactManager() const;
 
  void clearCachedDiscreteContactManager() const;
 
  std::unique_ptr<tesseract_collision::DiscreteContactManager> getDiscreteContactManager(const std::string& name) const;
 
  bool setActiveContinuousContactManager(const std::string& name);
 
  std::unique_ptr<tesseract_collision::ContinuousContactManager> getContinuousContactManager() const;
 
  void clearCachedContinuousContactManager() const;
 
  std::unique_ptr<tesseract_collision::ContinuousContactManager>
  getContinuousContactManager(const std::string& name) const;
 
  tesseract_common::CollisionMarginData getCollisionMarginData() const;
 
  std::shared_lock<std::shared_mutex> lockRead() const;
 
  bool operator==(const Environment& rhs) const;
  bool operator!=(const Environment& rhs) const;
 
private:
  mutable std::shared_mutex mutex_;
 
  struct Implementation;
  std::unique_ptr<Implementation> impl_;
 
  friend class boost::serialization::access;
  friend struct tesseract_common::Serialization;
  template <class Archive>
  void save(Archive& ar, const unsigned int version) const;  // NOLINT
 
  template <class Archive>
  void load(Archive& ar, const unsigned int version);  // NOLINT
 
  template <class Archive>
  void serialize(Archive& ar, const unsigned int version);  // NOLINT
 
public:
  explicit Environment(std::unique_ptr<Implementation> impl);
};
 
using EnvironmentPtrAnyPoly = tesseract_common::AnyWrapper<std::shared_ptr<tesseract_environment::Environment>>;
using EnvironmentConstPtrAnyPoly =
    tesseract_common::AnyWrapper<std::shared_ptr<const tesseract_environment::Environment>>;
}  // namespace tesseract_environment
BOOST_CLASS_EXPORT_KEY(tesseract_environment::Environment)
BOOST_CLASS_EXPORT_KEY(tesseract_environment::EnvironmentPtrAnyPoly)
BOOST_CLASS_EXPORT_KEY(tesseract_environment::EnvironmentConstPtrAnyPoly)
#endif  // TESSERACT_ENVIRONMENT_ENVIRONMENT_H