Build a a own navigation using RVIZ for initial and goal position

edit

I'm going to dissect this a bit.

I succesfully made it to let my robot start from a initial to a goal position using my own navigation algo

Nice!

The only thing I need is getting the initial and goal position as coordinates as input for my algo.

mhm, sounds reasonable.

rospy.wait_for_message('move_base_simple/goal', PoseStamped)

This line is unnecessary, just create the subscriber or you'll drop at minimum the first message.

rospy.Subscriber('move_base_simple/goal', PoseStamped, self.update_goal)

Looks good.

also something to get the initial position

Well, what do you have broadcasting the initial position? If nothing and you're just trying to mark on the map, you could use the goal tool where first call is pose and second is goal, but that's not very clean. The RVIZ goal tool is very simple, in about an hour you could make a new plugin of the same type that broadcasts a selected pose in the same way on another topic to subscribe to.

map_server

Not entirely sure what map_server has to do with this. That's just broadcasting the map topic, you really shouldn't see anything going on. This has nothing to do with navigation other that broadcasting an occupancy grid.