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/graph/adjacency_list.hpp>  // for customizable graphs
#include <boost/graph/directed_graph.hpp>  // A subclass to provide reasonable arguments to adjacency_list for a typical directed graph
#include <boost/graph/properties.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/serialization/access.hpp>
#include <string>
#include <list>
#include <unordered_map>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_scene_graph/link.h>
#include <tesseract_scene_graph/joint.h>
#include <tesseract_common/allowed_collision_matrix.h>
 
#ifndef SWIG
 
/* definition of basic boost::graph properties */
namespace boost
{
enum vertex_link_t
{
  vertex_link
};
enum vertex_link_visible_t
{
  vertex_link_visible
};
enum vertex_link_collision_enabled_t
{
  vertex_link_collision_enabled
};
enum edge_joint_t
{
  edge_joint
};
enum graph_root_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
{
#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,
    Link::Ptr,
    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, Joint::Ptr, 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);
 
  Link::ConstPtr getLink(const std::string& name) const;
 
  std::vector<Link::ConstPtr> getLinks() const;
 
  std::vector<Link::ConstPtr> 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);
 
  Joint::ConstPtr 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<Joint::ConstPtr> getJoints() const;
 
  std::vector<Joint::ConstPtr> 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);
 
  JointLimits::ConstPtr getJointLimits(const std::string& name);
 
  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;
 
  tesseract_common::AllowedCollisionMatrix::ConstPtr getAllowedCollisionMatrix() const;
 
  tesseract_common::AllowedCollisionMatrix::Ptr getAllowedCollisionMatrix();
 
  Link::ConstPtr getSourceLink(const std::string& joint_name) const;
 
  Link::ConstPtr getTargetLink(const std::string& joint_name) const;
 
  std::vector<Joint::ConstPtr> getInboundJoints(const std::string& link_name) const;
 
  std::vector<Joint::ConstPtr> 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 Link::Ptr& link_ptr, bool replace_allowed = false);
 
  bool addJointHelper(const Joint::Ptr& joint_ptr);
 
private:
  std::unordered_map<std::string, std::pair<Link::Ptr, Vertex>> link_map_;
  std::unordered_map<std::string, std::pair<Joint::Ptr, Edge>> joint_map_;
  tesseract_common::AllowedCollisionMatrix::Ptr acm_;
 
  void rebuildLinkAndJointMaps();
 
  struct cycle_detector : public boost::dfs_visitor<>
  {
    cycle_detector(bool& ascyclic) : ascyclic_(ascyclic) {}
 
    template <class e, class g>
    void back_edge(e, g&)
    {
      ascyclic_ = false;
    }
 
  protected:
    bool& ascyclic_;
  };
 
  struct tree_detector : public boost::dfs_visitor<>
  {
    tree_detector(bool& tree) : tree_(tree) {}
 
    template <class u, class g>
    void discover_vertex(u vertex, const g& graph)
    {
      auto num_in_edges = static_cast<int>(boost::in_degree(vertex, graph));
 
      if (num_in_edges > 1)
      {
        tree_ = false;
        return;
      }
 
      // Check if more that one root exist
      if (num_in_edges == 0 && found_root_)
      {
        tree_ = false;
        return;
      }
 
      if (num_in_edges == 0)
        found_root_ = true;
 
      // Check if not vertex is unused.
      if (num_in_edges == 0 && boost::out_degree(vertex, graph) == 0)
      {
        tree_ = false;
        return;
      }
    }
 
    template <class e, class g>
    void back_edge(e, const g&)
    {
      tree_ = false;
    }
 
  protected:
    bool& tree_;
    bool found_root_{ false };
  };
 
  struct children_detector : public boost::default_bfs_visitor
  {
    children_detector(std::vector<std::string>& children) : children_(children) {}
 
    template <class u, class g>
    void discover_vertex(u vertex, const g& graph)
    {
      children_.push_back(boost::get(boost::vertex_link, graph)[vertex]->getName());
    }
 
  protected:
    std::vector<std::string>& children_;
  };
 
  struct adjacency_detector : public boost::default_bfs_visitor
  {
    adjacency_detector(std::unordered_map<std::string, std::string>& adjacency_map,
                       std::map<Vertex, boost::default_color_type>& color_map,
                       const std::string& base_link_name,
                       const std::vector<std::string>& terminate_on_links)
      : adjacency_map_(adjacency_map)
      , color_map_(color_map)
      , base_link_name_(base_link_name)
      , terminate_on_links_(terminate_on_links)
    {
    }
 
    template <class u, class g>
    void examine_vertex(u vertex, const g& graph)
    {
      for (auto vd : boost::make_iterator_range(adjacent_vertices(vertex, graph)))
      {
        std::string adj_link = boost::get(boost::vertex_link, graph)[vd]->getName();
        if (std::find(terminate_on_links_.begin(), terminate_on_links_.end(), adj_link) != terminate_on_links_.end())
          color_map_[vd] = boost::default_color_type::black_color;
      }
    }
 
    template <class u, class g>
    void discover_vertex(u vertex, const g& graph)
    {
      std::string adj_link = boost::get(boost::vertex_link, graph)[vertex]->getName();
      adjacency_map_[adj_link] = base_link_name_;
    }
 
  protected:
    std::unordered_map<std::string, std::string>& adjacency_map_;
    std::map<Vertex, boost::default_color_type>& color_map_;
    const std::string& base_link_name_;
    const std::vector<std::string>& terminate_on_links_;
  };
 
  std::vector<std::string> getLinkChildrenHelper(Vertex start_vertex) const
  {
    const auto& graph = static_cast<const Graph&>(*this);
    std::vector<std::string> child_link_names;
 
    std::map<Vertex, size_t> index_map;
    boost::associative_property_map<std::map<Vertex, size_t>> prop_index_map(index_map);
 
    std::map<Vertex, boost::default_color_type> color_map;
    boost::associative_property_map<std::map<Vertex, boost::default_color_type>> prop_color_map(color_map);
 
    int c = 0;
    Graph::vertex_iterator i, iend;
    for (boost::tie(i, iend) = boost::vertices(graph); i != iend; ++i, ++c)
      boost::put(prop_index_map, *i, c);
 
    children_detector vis(child_link_names);
    // NOLINTNEXTLINE
    boost::breadth_first_search(
        graph,
        start_vertex,
        boost::visitor(vis).root_vertex(start_vertex).vertex_index_map(prop_index_map).color_map(prop_color_map));
 
    return child_link_names;
  }
 
  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
};
 
inline std::ostream& operator<<(std::ostream& os, const ShortestPath& path)
{
  os << "Links:" << std::endl;
  for (const auto& l : path.links)
    os << "  " << l << std::endl;
 
  os << "Joints:" << std::endl;
  for (const auto& j : path.joints)
    os << "  " << j << std::endl;
 
  os << "Active Joints:" << std::endl;
  for (const auto& j : path.active_joints)
    os << "  " << j << std::endl;
  return os;
}
 
}  // namespace tesseract_scene_graph
 
#include <boost/serialization/export.hpp>
#include <boost/serialization/tracking.hpp>
BOOST_CLASS_EXPORT_KEY2(tesseract_scene_graph::SceneGraph, "SceneGraph")
 
#endif  // TESSERACT_SCENE_GRAPH_GRAPH_H