graph.h

Go to the documentation of this file.

 
#ifndef TESSERACT_SCENE_GRAPH_GRAPH_H
#define TESSERACT_SCENE_GRAPH_GRAPH_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <boost/serialization/export.hpp>
#include <boost/graph/adjacency_list.hpp>  // for customizable graphs
#include <boost/graph/properties.hpp>
#include <string>
#include <unordered_map>
#include <Eigen/Geometry>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_common/fwd.h>
 
#ifndef SWIG
 
namespace boost::serialization
{
class access;
}
 
/* definition of basic boost::graph properties */
namespace boost
{
enum vertex_link_t : std::uint8_t
{
  vertex_link
};
enum vertex_link_visible_t : std::uint8_t
{
  vertex_link_visible
};
enum vertex_link_collision_enabled_t : std::uint8_t
{
  vertex_link_collision_enabled
};
enum edge_joint_t : std::uint8_t
{
  edge_joint
};
enum graph_root_t : std::uint8_t
{
  graph_root
};
 
BOOST_INSTALL_PROPERTY(vertex, link);
BOOST_INSTALL_PROPERTY(vertex, link_visible);
BOOST_INSTALL_PROPERTY(vertex, link_collision_enabled);
BOOST_INSTALL_PROPERTY(edge, joint);
BOOST_INSTALL_PROPERTY(graph, root);
}  // namespace boost
 
#endif  // SWIG
 
namespace tesseract_scene_graph
{
class Link;
class Joint;
class JointLimits;
 
#ifndef SWIG
 
using GraphProperty =
    boost::property<boost::graph_name_t, std::string, boost::property<boost::graph_root_t, std::string>>;
 
using VertexProperty = boost::property<
    boost::vertex_link_t,
    std::shared_ptr<Link>,
    boost::property<boost::vertex_link_visible_t, bool, boost::property<boost::vertex_link_collision_enabled_t, bool>>>;
 
using EdgeProperty =
    boost::property<boost::edge_joint_t, std::shared_ptr<Joint>, boost::property<boost::edge_weight_t, double>>;
 
using Graph = boost::
    adjacency_list<boost::listS, boost::listS, boost::bidirectionalS, VertexProperty, EdgeProperty, GraphProperty>;
 
#endif  // SWIG
 
struct ShortestPath
{
  std::vector<std::string> links;
 
  std::vector<std::string> joints;
 
  std::vector<std::string> active_joints;
};
 
class SceneGraph
#ifndef SWIG
  : public Graph,
    public boost::noncopyable
#endif  // SWIG
{
public:
  using Vertex = SceneGraph::vertex_descriptor;
  using Edge = SceneGraph::edge_descriptor;
 
  using Ptr = std::shared_ptr<SceneGraph>;
  using ConstPtr = std::shared_ptr<const SceneGraph>;
  using UPtr = std::unique_ptr<SceneGraph>;
  using ConstUPtr = std::unique_ptr<const SceneGraph>;
 
  SceneGraph(const std::string& name = "");
  ~SceneGraph() = default;
  // SceneGraphs are non-copyable
  SceneGraph(const SceneGraph& other) = delete;
  SceneGraph& operator=(const SceneGraph& other) = delete;
 
  SceneGraph(SceneGraph&& other) noexcept;
  SceneGraph& operator=(SceneGraph&& other) noexcept;
 
  SceneGraph::UPtr clone() const;
 
  void clear();
 
  void setName(const std::string& name);
 
  const std::string& getName() const;
 
  bool setRoot(const std::string& name);
 
  const std::string& getRoot() const;
 
  bool addLink(const Link& link, bool replace_allowed = false);
 
  bool addLink(const Link& link, const Joint& joint);
 
  std::shared_ptr<const Link> getLink(const std::string& name) const;
 
  std::vector<std::shared_ptr<const Link>> getLinks() const;
 
  std::vector<std::shared_ptr<const Link>> getLeafLinks() const;
 
  bool removeLink(const std::string& name, bool recursive = false);
 
  bool moveLink(const Joint& joint);
 
  void setLinkVisibility(const std::string& name, bool visibility);
 
  bool getLinkVisibility(const std::string& name) const;
 
  void setLinkCollisionEnabled(const std::string& name, bool enabled);
 
  bool getLinkCollisionEnabled(const std::string& name) const;
 
  bool addJoint(const Joint& joint);
 
  std::shared_ptr<const Joint> getJoint(const std::string& name) const;
 
  bool removeJoint(const std::string& name, bool recursive = false);
 
  bool moveJoint(const std::string& name, const std::string& parent_link);
 
  std::vector<std::shared_ptr<const Joint>> getJoints() const;
 
  std::vector<std::shared_ptr<const Joint>> getActiveJoints() const;
 
  bool changeJointOrigin(const std::string& name, const Eigen::Isometry3d& new_origin);
 
  bool changeJointLimits(const std::string& name, const JointLimits& limits);
 
  bool changeJointPositionLimits(const std::string& name, double lower, double upper);
 
  bool changeJointVelocityLimits(const std::string& name, double limit);
 
  bool changeJointAccelerationLimits(const std::string& name, double limit);
 
  bool changeJointJerkLimits(const std::string& name, double limit);
 
  std::shared_ptr<const JointLimits> getJointLimits(const std::string& name);
 
  void setAllowedCollisionMatrix(std::shared_ptr<tesseract_common::AllowedCollisionMatrix> acm);
 
  void addAllowedCollision(const std::string& link_name1, const std::string& link_name2, const std::string& reason);
 
  void removeAllowedCollision(const std::string& link_name1, const std::string& link_name2);
 
  void removeAllowedCollision(const std::string& link_name);
 
  void clearAllowedCollisions();
 
  bool isCollisionAllowed(const std::string& link_name1, const std::string& link_name2) const;
 
  std::shared_ptr<const tesseract_common::AllowedCollisionMatrix> getAllowedCollisionMatrix() const;
 
  std::shared_ptr<tesseract_common::AllowedCollisionMatrix> getAllowedCollisionMatrix();
 
  std::shared_ptr<const Link> getSourceLink(const std::string& joint_name) const;
 
  std::shared_ptr<const Link> getTargetLink(const std::string& joint_name) const;
 
  std::vector<std::shared_ptr<const Joint>> getInboundJoints(const std::string& link_name) const;
 
  std::vector<std::shared_ptr<const Joint>> getOutboundJoints(const std::string& link_name) const;
 
  bool isAcyclic() const;
 
  bool isTree() const;
 
  bool isEmpty() const;
 
  std::vector<std::string> getAdjacentLinkNames(const std::string& name) const;
 
  std::vector<std::string> getInvAdjacentLinkNames(const std::string& name) const;
 
  std::vector<std::string> getLinkChildrenNames(const std::string& name) const;
 
  std::vector<std::string> getJointChildrenNames(const std::string& name) const;
 
  std::unordered_map<std::string, std::string> getAdjacencyMap(const std::vector<std::string>& link_names) const;
 
  std::vector<std::string> getJointChildrenNames(const std::vector<std::string>& names) const;
 
  void saveDOT(const std::string& path) const;
 
  ShortestPath getShortestPath(const std::string& root, const std::string& tip) const;
 
#ifndef SWIG
 
  Vertex getVertex(const std::string& name) const;
 
  Edge getEdge(const std::string& name) const;
#endif
 
  bool insertSceneGraph(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& prefix = "");
 
  bool insertSceneGraph(const tesseract_scene_graph::SceneGraph& scene_graph,
                        const tesseract_scene_graph::Joint& joint,
                        const std::string& prefix = "");
 
  bool operator==(const SceneGraph& rhs) const;
  bool operator!=(const SceneGraph& rhs) const;
 
protected:
  bool addLinkHelper(const std::shared_ptr<Link>& link_ptr, bool replace_allowed = false);
 
  bool addJointHelper(const std::shared_ptr<Joint>& joint_ptr);
 
private:
  std::unordered_map<std::string, std::pair<std::shared_ptr<Link>, Vertex>> link_map_;
  std::unordered_map<std::string, std::pair<std::shared_ptr<Joint>, Edge>> joint_map_;
  std::shared_ptr<tesseract_common::AllowedCollisionMatrix> acm_;
 
  void rebuildLinkAndJointMaps();
 
  std::vector<std::string> getLinkChildrenHelper(Vertex start_vertex) const;
 
  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
};
 
std::ostream& operator<<(std::ostream& os, const ShortestPath& path);
 
}  // namespace tesseract_scene_graph
 
BOOST_CLASS_EXPORT_KEY(tesseract_scene_graph::SceneGraph)
 
#endif  // TESSERACT_SCENE_GRAPH_GRAPH_H