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; }
void Menu::kaos() { uint8_t target_x = PersistantStorage::getTargetXLocation(); uint8_t target_y = PersistantStorage::getTargetYLocation(); if (knowsBestPath(target_x, target_y)) { Compass8 absolute_end_direction; ContinuousRobotDriver maze_load_driver; Maze<16, 16> maze; maze_load_driver.loadState(maze); FloodFillPath<16, 16> flood_path (maze, 0, 0, target_x, target_y); KnownPath<16, 16> known_path (maze, 0, 0, target_x, target_y, flood_path); PathParser parser (&known_path); KaosDriver driver; gUserInterface.waitForHand(); speedRunMelody(); absolute_end_direction = parser.getEndDirection(); driver.execute(parser.getMoveList()); char buf[5]; snprintf(buf, 5, "%02d%02d", parser.end_x, parser.end_y); gUserInterface.showString(buf, 4); searchFinishMelody(); ContinuousRobotDriver other_driver(parser.end_x, parser.end_y, absolute_end_direction, false); { FloodFillPath<16, 16> flood_path(maze, other_driver.getX(), other_driver.getY(), 0, 0); KnownPath<16, 16> known_path(maze, other_driver.getX(), other_driver.getY(), 0, 0, flood_path); if (known_path.isEmpty()) return; other_driver.move(&known_path); snprintf(buf, 5, "%02d%02d", other_driver.getX(), other_driver.getY()); gUserInterface.showString(buf, 4); } other_driver.move(kNorth, 0); //ContinuousRobotDriver return_driver(parser.end_x, parser.end_y, // absolute_end_direction); //return_driver.loadState(maze); //FloodFillPath<16, 16> return_path (maze, 8, 8, 0, 0); //KnownPath<16, 16> return_best_path (maze, 8, 8, 0, 0, return_path); //return_driver.move(&return_best_path); } }