Thanks for the comment,
You were right about using DB for only 3 objects, but
I did it this way because I can have more options to play with
in the future.
Mapping all objects on the map is sure troublesome. I'm
using simple grid-map the grid size is 1x1 cm.
The objects that I put on the map are a robot current position,
estimate origins of the obstacles, and target positions.
Before running the pathfinding algorithm, the size of those
objects will be expanded(from the center).
The key is I put extra clearance
space when expanding the obstacle.
obstacle -> 30x30 (object =10 and 10 more on each side for clearance)
The pathfinding algorithm was modified from http://www.policyalmanac.org/games/aStarTutorial.htm
By the way, do you know where I can find some other
mapping technique that works well with NXT(only one sonar sensor)?