More FM methods added

I would set this default to true. If a goal_point_ is set, by default stop when it is reached. Since this parameter can be true but goal_point_ can be not set, you have to include a check before starting to compute FM.