Update: update the logic of setting init pose and goal pose

[kabi-robot]

In current version, all subscibers as costmap_subscriber, initpose_subscriber and goalpose_subscriber will hold all received messages in a queue, and each time Run() function got run, it will retrieve the front message.

But this is unreasonable. For example, if we publish several /initial_pose messages, we mean that the last initial_pose is what we want to set as the real initial pose, and then do the A* search. But in current code, it will use the first initial pose it got.

This commit is used to fix this, it mainly does the following things:

1. Remove costmap_subscriber, init_pose_subscriber and goal_pose_subscriber, and use origin ros subscriber;
2. Set local variable for costmap, init pose and goal pose, and a flag that indicates whether this message is got, and the value is false at initial;
3. Each time the process get a new message, it will override the local one, and set the is_xxx_get flat to true;
4. The costmap will set only once as before, this is confirmed by is_costmap_init flag;
5. When Run() function gets running, it will do A* search only if both init_pose and goal_pose is get, and will reset is_initpose_get and is_goalpose_get to false at the end.