Dear Mr. Vaughan,
It seems that I made A* work. I used fasr2.world and fasr2.cc as you suggested. I edited both files and now I can load any map and use only one robot. I made some changes, now it won't use fiducial nor sonar, and I removed
the code regarding pool and fuel to make it work correctly. Now I give it a goal point and it will give me the waypoints from the point of origin of the robot to the goal point. I just take the x and y values from each of the nodes that are created in Plan() by using node->pose.x and node->pose.y.
The thing is that, depending on the length and complexity of the path, calling ast::astar in Plan() can take increadibly long, let's say 15 minutes, and the number of waypoints can reach up to 800 or thousands. I would like to ask for some help. Could you please give me any hint of how I can diminish the ammount of waypoints and/or the time to call ast::astar? What am I doing wrong?
Besides, which would be it's heuristic function so to say? That defined in GoodDirection()? "find the node with the lowest value within range of the given pose and return the absolute heading from pose to that node "
Many thanks in advance,