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