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 <vector>
#include <string>
#include <shared_mutex>
#include <chrono>
#include <console_bridge/console.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_environment/commands.h>
#include <tesseract_environment/events.h>
#include <tesseract_collision/core/discrete_contact_manager.h>
#include <tesseract_collision/core/continuous_contact_manager.h>
#include <tesseract_collision/core/contact_managers_plugin_factory.h>
#include <tesseract_scene_graph/graph.h>
#include <tesseract_scene_graph/scene_state.h>
#include <tesseract_state_solver/mutable_state_solver.h>
#include <tesseract_urdf/urdf_parser.h>
#include <tesseract_srdf/srdf_model.h>
#include <tesseract_common/resource_locator.h>
#include <tesseract_common/manipulator_info.h>
#include <tesseract_common/types.h>
#include <tesseract_common/utils.h>
#include <tesseract_kinematics/core/joint_group.h>
#include <tesseract_kinematics/core/kinematic_group.h>
#include <tesseract_kinematics/core/kinematics_plugin_factory.h>
 
namespace tesseract_environment
{
using FindTCPOffsetCallbackFn = std::function<Eigen::Isometry3d(const tesseract_common::ManipulatorInfo&)>;
 
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() = default;
  virtual ~Environment() = default;
  Environment(const Environment&) = delete;
  Environment& operator=(const Environment&) = delete;
  Environment(Environment&&) = delete;
  Environment& operator=(Environment&&) = delete;
 
  bool init(const Commands& commands);
 
  bool init(const tesseract_scene_graph::SceneGraph& scene_graph,
            const tesseract_srdf::SRDFModel::ConstPtr& srdf_model = nullptr);
 
  bool init(const std::string& urdf_string, const tesseract_common::ResourceLocator::ConstPtr& locator);
 
  bool init(const std::string& urdf_string,
            const std::string& srdf_string,
            const tesseract_common::ResourceLocator::ConstPtr& locator);
 
  bool init(const tesseract_common::fs::path& urdf_path, const tesseract_common::ResourceLocator::ConstPtr& locator);
 
  bool init(const tesseract_common::fs::path& urdf_path,
            const tesseract_common::fs::path& srdf_path,
            const tesseract_common::ResourceLocator::ConstPtr& locator);
 
  Environment::UPtr clone() const;
 
  bool reset();
 
  void clear();
 
  bool isInitialized() const;
 
  int getRevision() const;
 
  Commands getCommandHistory() const;
 
  bool applyCommands(const Commands& commands);
 
  bool applyCommand(Command::ConstPtr command);
 
  tesseract_scene_graph::SceneGraph::ConstPtr getSceneGraph() const;
 
  std::vector<std::string> getGroupJointNames(const std::string& group_name) const;
 
  tesseract_kinematics::JointGroup::UPtr getJointGroup(const std::string& group_name) const;
 
  tesseract_kinematics::JointGroup::UPtr getJointGroup(const std::string& name,
                                                       const std::vector<std::string>& joint_names) const;
 
  tesseract_kinematics::KinematicGroup::UPtr getKinematicGroup(const std::string& group_name,
                                                               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(tesseract_common::ResourceLocator::ConstPtr locator);
 
  tesseract_common::ResourceLocator::ConstPtr getResourceLocator() const;
 
  void setName(const std::string& name);
 
  const std::string& getName() const;
 
  void setState(const std::unordered_map<std::string, double>& joints);
  void setState(const std::vector<std::string>& joint_names, const Eigen::Ref<const Eigen::VectorXd>& joint_values);
 
  tesseract_scene_graph::SceneState getState(const std::unordered_map<std::string, double>& joints) const;
  tesseract_scene_graph::SceneState getState(const std::vector<std::string>& joint_names,
                                             const Eigen::Ref<const Eigen::VectorXd>& joint_values) const;
 
  tesseract_scene_graph::SceneState getState() const;
 
  std::chrono::system_clock::time_point getTimestamp() const;
 
  std::chrono::system_clock::time_point getCurrentStateTimestamp() const;
 
  tesseract_scene_graph::Link::ConstPtr getLink(const std::string& name) const;
 
  tesseract_scene_graph::Joint::ConstPtr getJoint(const std::string& name) const;
 
  tesseract_scene_graph::JointLimits::ConstPtr getJointLimits(const std::string& joint_name) const;
 
  bool getLinkCollisionEnabled(const std::string& name) const;
 
  bool getLinkVisibility(const std::string& name) const;
 
  tesseract_common::AllowedCollisionMatrix::ConstPtr 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;
 
  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;
 
  tesseract_scene_graph::StateSolver::UPtr getStateSolver() const;
 
  tesseract_srdf::KinematicsInformation getKinematicsInformation() const;
 
  tesseract_srdf::GroupNames getGroupNames() const;
 
  tesseract_common::ContactManagersPluginInfo getContactManagersPluginInfo() const;
 
  bool setActiveDiscreteContactManager(const std::string& name);
 
  tesseract_collision::DiscreteContactManager::UPtr getDiscreteContactManager() const;
 
  void clearCachedDiscreteContactManager() const;
 
  tesseract_collision::DiscreteContactManager::UPtr getDiscreteContactManager(const std::string& name) const;
 
  bool setActiveContinuousContactManager(const std::string& name);
 
  tesseract_collision::ContinuousContactManager::UPtr getContinuousContactManager() const;
 
  void clearCachedContinuousContactManager() const;
 
  tesseract_collision::ContinuousContactManager::UPtr 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;
 
protected:
  bool initialized_{ false };
 
  int revision_{ 0 };
 
  int init_revision_{ 0 };
 
  Commands commands_{};
 
  tesseract_scene_graph::SceneGraph::Ptr scene_graph_{ nullptr };
 
  tesseract_scene_graph::SceneGraph::ConstPtr scene_graph_const_{ nullptr };
 
  tesseract_srdf::KinematicsInformation kinematics_information_;
 
  tesseract_kinematics::KinematicsPluginFactory kinematics_factory_;
 
  tesseract_scene_graph::SceneState current_state_;
 
  std::chrono::system_clock::time_point timestamp_{ std::chrono::system_clock::now() };
 
  std::chrono::system_clock::time_point current_state_timestamp_{ std::chrono::system_clock::now() };
 
  tesseract_scene_graph::MutableStateSolver::UPtr state_solver_{ nullptr };
 
  tesseract_collision::IsContactAllowedFn is_contact_allowed_fn_;
 
  std::vector<FindTCPOffsetCallbackFn> find_tcp_cb_{};
 
  std::map<std::size_t, EventCallbackFn> event_cb_{};
 
  tesseract_common::ResourceLocator::ConstPtr resource_locator_{ nullptr };
 
  tesseract_common::ContactManagersPluginInfo contact_managers_plugin_info_;
 
  tesseract_collision::ContactManagersPluginFactory contact_managers_factory_;
 
  tesseract_collision::CollisionMarginData collision_margin_data_;
 
  mutable tesseract_collision::DiscreteContactManager::UPtr discrete_manager_{ nullptr };
  mutable std::shared_mutex discrete_manager_mutex_;
 
  mutable tesseract_collision::ContinuousContactManager::UPtr continuous_manager_{ nullptr };
  mutable std::shared_mutex continuous_manager_mutex_;
 
  mutable std::unordered_map<std::string, std::vector<std::string>> group_joint_names_cache_{};
  mutable std::shared_mutex group_joint_names_cache_mutex_;
 
  mutable std::unordered_map<std::string, tesseract_kinematics::JointGroup::UPtr> joint_group_cache_{};
  mutable std::shared_mutex joint_group_cache_mutex_;
 
  mutable std::map<std::pair<std::string, std::string>, tesseract_kinematics::KinematicGroup::UPtr>
      kinematic_group_cache_{};
  mutable std::shared_mutex kinematic_group_cache_mutex_;
 
  mutable std::shared_mutex mutex_;
 
  void currentStateChanged();
 
  void environmentChanged();
 
  void triggerCurrentStateChangedCallbacks();
 
  void triggerEnvironmentChangedCallbacks();
 
private:
  bool removeLinkHelper(const std::string& name);
 
  static void getCollisionObject(tesseract_collision::CollisionShapesConst& shapes,
                                 tesseract_common::VectorIsometry3d& shape_poses,
                                 const tesseract_scene_graph::Link& link);
 
  bool setActiveDiscreteContactManagerHelper(const std::string& name);
  bool setActiveContinuousContactManagerHelper(const std::string& name);
 
  tesseract_collision::DiscreteContactManager::UPtr getDiscreteContactManagerHelper(const std::string& name) const;
 
  tesseract_collision::ContinuousContactManager::UPtr getContinuousContactManagerHelper(const std::string& name) const;
 
  bool initHelper(const Commands& commands);
  static Commands getInitCommands(const tesseract_scene_graph::SceneGraph& scene_graph,
                                  const tesseract_srdf::SRDFModel::ConstPtr& srdf_model = nullptr);
 
  bool applyCommandsHelper(const Commands& commands);
 
  // Command Helper function
  bool applyAddCommand(AddLinkCommand::ConstPtr cmd);
  bool applyMoveLinkCommand(const MoveLinkCommand::ConstPtr& cmd);
  bool applyMoveJointCommand(const MoveJointCommand::ConstPtr& cmd);
  bool applyRemoveLinkCommand(const RemoveLinkCommand::ConstPtr& cmd);
  bool applyRemoveJointCommand(const RemoveJointCommand::ConstPtr& cmd);
  bool applyReplaceJointCommand(const ReplaceJointCommand::ConstPtr& cmd);
  bool applyChangeLinkOriginCommand(const ChangeLinkOriginCommand::ConstPtr& cmd);
  bool applyChangeJointOriginCommand(const ChangeJointOriginCommand::ConstPtr& cmd);
  bool applyChangeLinkCollisionEnabledCommand(const ChangeLinkCollisionEnabledCommand::ConstPtr& cmd);
  bool applyChangeLinkVisibilityCommand(const ChangeLinkVisibilityCommand::ConstPtr& cmd);
  bool applyModifyAllowedCollisionsCommand(const ModifyAllowedCollisionsCommand::ConstPtr& cmd);
  bool applyRemoveAllowedCollisionLinkCommand(const RemoveAllowedCollisionLinkCommand::ConstPtr& cmd);
  bool applyAddSceneGraphCommand(AddSceneGraphCommand::ConstPtr cmd);
  bool applyChangeJointPositionLimitsCommand(const ChangeJointPositionLimitsCommand::ConstPtr& cmd);
  bool applyChangeJointVelocityLimitsCommand(const ChangeJointVelocityLimitsCommand::ConstPtr& cmd);
  bool applyChangeJointAccelerationLimitsCommand(const ChangeJointAccelerationLimitsCommand::ConstPtr& cmd);
  bool applyAddKinematicsInformationCommand(const AddKinematicsInformationCommand::ConstPtr& cmd);
  bool applyChangeCollisionMarginsCommand(const ChangeCollisionMarginsCommand::ConstPtr& cmd);
  bool applyAddContactManagersPluginInfoCommand(const AddContactManagersPluginInfoCommand::ConstPtr& cmd);
  bool applySetActiveContinuousContactManagerCommand(const SetActiveContinuousContactManagerCommand::ConstPtr& cmd);
  bool applySetActiveDiscreteContactManagerCommand(const SetActiveDiscreteContactManagerCommand::ConstPtr& cmd);
 
  friend class boost::serialization::access;
  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
};
}  // namespace tesseract_environment
 
#endif  // TESSERACT_ENVIRONMENT_ENVIRONMENT_H