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;
}
