std::shared_ptr<node::Node> SceneGraph::find_node(std::string const& path_to_node,
                            std::string const& path_to_start) const {

    PathParser parser;
    parser.parse(path_to_node);

    std::shared_ptr<node::Node> to_be_found(path_to_start == "/"
                          ? root_
                          : find_node(path_to_start));

    for (auto const& node_name : parser.get_parsed_path()) {
      for (auto child : to_be_found->get_children()) {
            if (child->get_name() == node_name) {
                to_be_found = child;
                break;
            }
        }

        if (to_be_found->get_name() != node_name) {
            return nullptr;
        }
    }

    return to_be_found;
}
/* Node --------------------------------------------------------------------------- */
int main(int argc, char **argv)
{
  ros::init(argc, argv, "path_parser");
  PathParser parser;
  if(!parser.init())
    ROS_ERROR("Failed to initialize arm planner node. Exiting.");
  vis = atoi(argv[3]);
  parser.parse(argv[1],atoi(argv[2]));
  return 0;
}