Check the absolute angle difference when checking angular tolerance

[corot]

Actually, this is still not enough, as we also need to check that the robot is almost stopped. Like in the base_local_planner:

  bool stopped(const nav_msgs::Odometry& base_odom, 
      const double& rot_stopped_velocity, const double& trans_stopped_velocity){
    return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity 
      && fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity
      && fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity;
  }

But wee need to merge #189 to have actual robot speed to check that, so I'll do in another PR.

[corot]

my bad; mbf_utility::angle always returns positive angles