MessageHandling.cpp
Go to the documentation of this file.
00001 /*
00002  *      This file is part of qpOASES.
00003  *
00004  *      qpOASES -- An Implementation of the Online Active Set Strategy.
00005  *      Copyright (C) 2007-2011 by Hans Joachim Ferreau, Andreas Potschka,
00006  *      Christian Kirches et al. All rights reserved.
00007  *
00008  *      qpOASES is free software; you can redistribute it and/or
00009  *      modify it under the terms of the GNU Lesser General Public
00010  *      License as published by the Free Software Foundation; either
00011  *      version 2.1 of the License, or (at your option) any later version.
00012  *
00013  *      qpOASES is distributed in the hope that it will be useful,
00014  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016  *      See the GNU Lesser General Public License for more details.
00017  *
00018  *      You should have received a copy of the GNU Lesser General Public
00019  *      License along with qpOASES; if not, write to the Free Software
00020  *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00021  *
00022  */
00023 
00024 
00036 #include <stdio.h>
00037 
00038 #ifdef __MATLAB__
00039   #include "mex.h"
00040 #endif
00041 
00042 #include <qpOASES/MessageHandling.hpp>
00043 #include <qpOASES/Utils.hpp>
00044 
00045 
00046 BEGIN_NAMESPACE_QPOASES
00047 
00048 
00049 
00051 MessageHandling::ReturnValueList returnValueList[] =
00052 {
00053 /* miscellaneous */
00054 { SUCCESSFUL_RETURN, "Successful return", VS_VISIBLE },
00055 { RET_DIV_BY_ZERO, "Division by zero", VS_VISIBLE },
00056 { RET_INDEX_OUT_OF_BOUNDS, "Index out of bounds", VS_VISIBLE },
00057 { RET_INVALID_ARGUMENTS, "At least one of the arguments is invalid", VS_VISIBLE },
00058 { RET_ERROR_UNDEFINED, "Error number undefined", VS_VISIBLE },
00059 { RET_WARNING_UNDEFINED, "Warning number undefined", VS_VISIBLE },
00060 { RET_INFO_UNDEFINED, "Info number undefined", VS_VISIBLE },
00061 { RET_EWI_UNDEFINED, "Error/warning/info number undefined", VS_VISIBLE },
00062 { RET_AVAILABLE_WITH_LINUX_ONLY, "This function is available under Linux only", VS_HIDDEN },
00063 { RET_UNKNOWN_BUG, "The error occured is not yet known", VS_VISIBLE },
00064 { RET_PRINTLEVEL_CHANGED, "Print level changed", VS_VISIBLE },
00065 { RET_NOT_YET_IMPLEMENTED, "Requested function is not yet implemented.", VS_VISIBLE },
00066 /* Indexlist */
00067 { RET_INDEXLIST_MUST_BE_REORDERD, "Index list has to be reordered", VS_VISIBLE },
00068 { RET_INDEXLIST_EXCEEDS_MAX_LENGTH, "Index list exceeds its maximal physical length", VS_VISIBLE },
00069 { RET_INDEXLIST_CORRUPTED, "Index list corrupted", VS_VISIBLE },
00070 { RET_INDEXLIST_OUTOFBOUNDS, "Physical index is out of bounds", VS_VISIBLE },
00071 { RET_INDEXLIST_ADD_FAILED, "Adding indices from another index set failed", VS_VISIBLE },
00072 { RET_INDEXLIST_INTERSECT_FAILED, "Intersection with another index set failed", VS_VISIBLE },
00073 /* SubjectTo / Bounds / Constraints */
00074 { RET_INDEX_ALREADY_OF_DESIRED_STATUS, "Index is already of desired status", VS_VISIBLE },
00075 { RET_ADDINDEX_FAILED, "Adding index to index set failed", VS_VISIBLE },
00076 { RET_REMOVEINDEX_FAILED, "Removing index from index set failed", VS_VISIBLE },
00077 { RET_SWAPINDEX_FAILED, "Cannot swap between different indexsets", VS_VISIBLE },
00078 { RET_NOTHING_TO_DO, "Nothing to do", VS_VISIBLE },
00079 { RET_SETUP_BOUND_FAILED, "Setting up bound index failed", VS_VISIBLE },
00080 { RET_SETUP_CONSTRAINT_FAILED, "Setting up constraint index failed", VS_VISIBLE },
00081 { RET_MOVING_BOUND_FAILED, "Moving bound between index sets failed", VS_VISIBLE },
00082 { RET_MOVING_CONSTRAINT_FAILED, "Moving constraint between index sets failed", VS_VISIBLE },
00083 { RET_SHIFTING_FAILED, "Shifting of bounds/constraints failed", VS_VISIBLE },
00084 { RET_ROTATING_FAILED, "Rotating of bounds/constraints failed", VS_VISIBLE },
00085 /* QProblem */
00086 { RET_QPOBJECT_NOT_SETUP, "The QP object has not been setup correctly, use another constructor", VS_VISIBLE },
00087 { RET_QP_ALREADY_INITIALISED, "QProblem has already been initialised", VS_VISIBLE },
00088 { RET_NO_INIT_WITH_STANDARD_SOLVER, "Initialisation via extern QP solver is not yet implemented", VS_VISIBLE },
00089 { RET_RESET_FAILED, "Reset failed", VS_VISIBLE },
00090 { RET_INIT_FAILED, "Initialisation failed", VS_VISIBLE },
00091 { RET_INIT_FAILED_TQ, "Initialisation failed due to TQ factorisation", VS_VISIBLE },
00092 { RET_INIT_FAILED_CHOLESKY, "Initialisation failed due to Cholesky decomposition", VS_VISIBLE },
00093 { RET_INIT_FAILED_HOTSTART, "Initialisation failed! QP could not be solved!", VS_VISIBLE },
00094 { RET_INIT_FAILED_INFEASIBILITY, "Initial QP could not be solved due to infeasibility!", VS_VISIBLE },
00095 { RET_INIT_FAILED_UNBOUNDEDNESS, "Initial QP could not be solved due to unboundedness!", VS_VISIBLE },
00096 { RET_INIT_FAILED_REGULARISATION, "Initialisation failed as Hessian matrix could not be regularised", VS_VISIBLE },
00097 { RET_INIT_SUCCESSFUL, "Initialisation done", VS_VISIBLE },
00098 { RET_OBTAINING_WORKINGSET_FAILED, "Failed to obtain working set for auxiliary QP", VS_VISIBLE },
00099 { RET_SETUP_WORKINGSET_FAILED, "Failed to setup working set for auxiliary QP", VS_VISIBLE },
00100 { RET_SETUP_AUXILIARYQP_FAILED, "Failed to setup auxiliary QP for initialised homotopy", VS_VISIBLE },
00101 { RET_NO_EXTERN_SOLVER, "No extern QP solver available", VS_VISIBLE },
00102 { RET_QP_UNBOUNDED, "QP is unbounded", VS_VISIBLE },
00103 { RET_QP_INFEASIBLE, "QP is infeasible", VS_VISIBLE },
00104 { RET_QP_NOT_SOLVED, "Problems occured while solving QP with standard solver", VS_VISIBLE },
00105 { RET_QP_SOLVED, "QP successfully solved", VS_VISIBLE },
00106 { RET_UNABLE_TO_SOLVE_QP, "Problems occured while solving QP", VS_VISIBLE },
00107 { RET_INITIALISATION_STARTED, "Starting problem initialisation...", VS_VISIBLE },
00108 { RET_HOTSTART_FAILED, "Unable to perform homotopy due to internal error", VS_VISIBLE },
00109 { RET_HOTSTART_FAILED_TO_INIT, "Unable to initialise problem", VS_VISIBLE },
00110 { RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, "Unable to perform homotopy as previous QP is not solved", VS_VISIBLE },
00111 { RET_ITERATION_STARTED, "Iteration", VS_VISIBLE },
00112 { RET_SHIFT_DETERMINATION_FAILED, "Determination of shift of the QP data failed", VS_VISIBLE },
00113 { RET_STEPDIRECTION_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE },
00114 { RET_STEPLENGTH_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE },
00115 { RET_OPTIMAL_SOLUTION_FOUND, "Optimal solution of neighbouring QP found", VS_VISIBLE },
00116 { RET_HOMOTOPY_STEP_FAILED, "Unable to perform homotopy step", VS_VISIBLE },
00117 { RET_HOTSTART_STOPPED_INFEASIBILITY, "Premature homotopy termination because QP is infeasible", VS_VISIBLE },
00118 { RET_HOTSTART_STOPPED_UNBOUNDEDNESS, "Premature homotopy termination because QP is unbounded", VS_VISIBLE },
00119 { RET_WORKINGSET_UPDATE_FAILED, "Unable to update working sets according to initial guesses", VS_VISIBLE },
00120 { RET_MAX_NWSR_REACHED, "Maximum number of working set recalculations performed", VS_VISIBLE },
00121 { RET_CONSTRAINTS_NOT_SPECIFIED, "Problem does comprise constraints! You have to specify new constraints' bounds", VS_VISIBLE },
00122 { RET_INVALID_FACTORISATION_FLAG, "Invalid factorisation flag", VS_VISIBLE },
00123 { RET_UNABLE_TO_SAVE_QPDATA, "Unable to save QP data", VS_VISIBLE },
00124 { RET_STEPDIRECTION_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE },
00125 { RET_STEPDIRECTION_FAILED_CHOLESKY, "Abnormal termination due to Cholesky factorisation", VS_VISIBLE },
00126 { RET_CYCLING_DETECTED, "Cycling detected", VS_VISIBLE },
00127 { RET_CYCLING_NOT_RESOLVED, "Cycling cannot be resolved, QP is probably infeasible", VS_VISIBLE },
00128 { RET_CYCLING_RESOLVED, "Cycling probably resolved", VS_VISIBLE },
00129 { RET_STEPSIZE, "", VS_VISIBLE },
00130 { RET_STEPSIZE_NONPOSITIVE, "", VS_VISIBLE },
00131 { RET_SETUPSUBJECTTOTYPE_FAILED, "Setup of SubjectToTypes failed", VS_VISIBLE },
00132 { RET_ADDCONSTRAINT_FAILED, "Addition of constraint to working set failed", VS_VISIBLE },
00133 { RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, "Addition of constraint to working set failed", VS_VISIBLE },
00134 { RET_ADDBOUND_FAILED, "Addition of bound to working set failed", VS_VISIBLE },
00135 { RET_ADDBOUND_FAILED_INFEASIBILITY, "Addition of bound to working set failed", VS_VISIBLE },
00136 { RET_REMOVECONSTRAINT_FAILED, "Removal of constraint from working set failed", VS_VISIBLE },
00137 { RET_REMOVEBOUND_FAILED, "Removal of bound from working set failed", VS_VISIBLE },
00138 { RET_REMOVE_FROM_ACTIVESET, "Removing from active set:", VS_VISIBLE },
00139 { RET_ADD_TO_ACTIVESET, "Adding to active set:", VS_VISIBLE },
00140 { RET_REMOVE_FROM_ACTIVESET_FAILED, "Removing from active set failed", VS_VISIBLE },
00141 { RET_ADD_TO_ACTIVESET_FAILED, "Adding to active set failed", VS_VISIBLE },
00142 { RET_CONSTRAINT_ALREADY_ACTIVE, "Constraint is already active", VS_VISIBLE },
00143 { RET_ALL_CONSTRAINTS_ACTIVE, "All constraints are active, no further constraint can be added", VS_VISIBLE },
00144 { RET_LINEARLY_DEPENDENT, "New bound/constraint is linearly dependent", VS_VISIBLE },
00145 { RET_LINEARLY_INDEPENDENT, "New bound/constraint is linearly independent", VS_VISIBLE },
00146 { RET_LI_RESOLVED, "Linear indepence of active contraint matrix successfully resolved", VS_VISIBLE },
00147 { RET_ENSURELI_FAILED, "Failed to ensure linear indepence of active contraint matrix", VS_VISIBLE },
00148 { RET_ENSURELI_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE },
00149 { RET_ENSURELI_FAILED_NOINDEX, "QP is infeasible", VS_VISIBLE },
00150 { RET_ENSURELI_FAILED_CYCLING, "QP is infeasible", VS_VISIBLE },
00151 { RET_BOUND_ALREADY_ACTIVE, "Bound is already active", VS_VISIBLE },
00152 { RET_ALL_BOUNDS_ACTIVE, "All bounds are active, no further bound can be added", VS_VISIBLE },
00153 { RET_CONSTRAINT_NOT_ACTIVE, "Constraint is not active", VS_VISIBLE },
00154 { RET_BOUND_NOT_ACTIVE, "Bound is not active", VS_VISIBLE },
00155 { RET_HESSIAN_NOT_SPD, "Projected Hessian matrix not positive definite", VS_VISIBLE },
00156 { RET_HESSIAN_INDEFINITE, "Hessian matrix is indefinite", VS_VISIBLE },
00157 { RET_MATRIX_SHIFT_FAILED, "Unable to update matrices or to transform vectors", VS_VISIBLE },
00158 { RET_MATRIX_FACTORISATION_FAILED, "Unable to calculate new matrix factorisations", VS_VISIBLE },
00159 { RET_PRINT_ITERATION_FAILED, "Unable to print information on current iteration", VS_VISIBLE },
00160 { RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, "No global message output file initialised", VS_VISIBLE },
00161 { RET_DISABLECONSTRAINTS_FAILED, "Unable to disbable constraints", VS_VISIBLE },
00162 { RET_ENABLECONSTRAINTS_FAILED, "Unable to enable constraints", VS_VISIBLE },
00163 { RET_ALREADY_ENABLED, "Bound or constraint is already enabled", VS_VISIBLE },
00164 { RET_ALREADY_DISABLED, "Bound or constraint is already disabled", VS_VISIBLE },
00165 { RET_NO_HESSIAN_SPECIFIED, "No Hessian matrix has been specified", VS_VISIBLE },
00166 { RET_USING_REGULARISATION, "Using regularisation as Hessian matrix is not positive definite", VS_VISIBLE },
00167 { RET_EPS_MUST_BE_POSITVE, "Eps for regularisation must be sufficiently positive", VS_VISIBLE },
00168 { RET_REGSTEPS_MUST_BE_POSITVE, "Maximum number of regularisation steps must be non-negative", VS_VISIBLE },
00169 { RET_HESSIAN_ALREADY_REGULARISED, "Hessian has been already regularised", VS_VISIBLE },
00170 { RET_CANNOT_REGULARISE_IDENTITY, "Identity Hessian matrix cannot be regularised", VS_VISIBLE },
00171 { RET_CANNOT_REGULARISE_SPARSE, "Sparse matrix cannot be regularised as diagonal entry is missing", VS_VISIBLE },
00172 { RET_NO_REGSTEP_NWSR, "No additional regularisation step could be performed due to limits", VS_VISIBLE },
00173 { RET_FEWER_REGSTEPS_NWSR, "Fewer additional regularisation steps have been performed due to limits", VS_VISIBLE },
00174 { RET_CHOLESKY_OF_ZERO_HESSIAN, "Cholesky decomposition of (unregularised) zero Hessian matrix", VS_VISIBLE },
00175 { RET_CONSTRAINTS_ARE_NOT_SCALED, "When defining __MANY_CONSTRAINTS__, l1 norm of each constraint must be not greater than one", VS_VISIBLE },
00176 { RET_ERROR_IN_CONSTRAINTPRODUCT, "Error in user-defined constraint product function", VS_VISIBLE },
00177 /* SQProblem */
00178 { RET_UPDATEMATRICES_FAILED, "Unable to update QP matrices", VS_VISIBLE },
00179 { RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED, "Unable to update matrices as previous QP is not solved", VS_VISIBLE },
00180 /* Utils */
00181 { RET_UNABLE_TO_OPEN_FILE, "Unable to open file", VS_VISIBLE },
00182 { RET_UNABLE_TO_WRITE_FILE, "Unable to write into file", VS_VISIBLE },
00183 { RET_UNABLE_TO_READ_FILE, "Unable to read from file", VS_VISIBLE },
00184 { RET_FILEDATA_INCONSISTENT, "File contains inconsistent data", VS_VISIBLE },
00185 /* SolutionAnalysis */
00186 { RET_UNABLE_TO_ANALYSE_QPROBLEM, "Unable to analyse (S)QProblem(B) object", VS_VISIBLE },
00187 /* Benchmark */
00188 { RET_NWSR_SET_TO_ONE, "Maximum number of working set changes was set to 1", VS_VISIBLE },
00189 { RET_UNABLE_TO_READ_BENCHMARK, "Unable to read benchmark data", VS_VISIBLE },
00190 { RET_BENCHMARK_ABORTED, "Benchmark aborted", VS_VISIBLE },
00191 { RET_INITIAL_QP_SOLVED, "Initial QP solved", VS_VISIBLE },
00192 { RET_QP_SOLUTION_STARTED, "Solving QP no.", VS_VISIBLE },
00193 { RET_BENCHMARK_SUCCESSFUL, "Benchmark terminated successfully", VS_VISIBLE },
00194 { TERMINAL_LIST_ELEMENT, "", VS_HIDDEN } /* IMPORTANT: Terminal list element! */
00195 };
00196 
00197 
00198 
00199 /*****************************************************************************
00200  *  P U B L I C                                                              *
00201  *****************************************************************************/
00202 
00203 /*
00204  *      M e s s a g e H a n d l i n g
00205  */
00206 MessageHandling::MessageHandling( )
00207 {
00208         errorVisibility   = VS_VISIBLE;
00209         warningVisibility = VS_VISIBLE;
00210         infoVisibility    = VS_VISIBLE;
00211 
00212         outputFile = stdout;
00213         errorCount = 0;
00214 }
00215 
00216 /*
00217  *      M e s s a g e H a n d l i n g
00218  */
00219 MessageHandling::MessageHandling( FILE* _outputFile )
00220 {
00221         errorVisibility   = VS_VISIBLE;
00222         warningVisibility = VS_VISIBLE;
00223         infoVisibility    = VS_VISIBLE;
00224 
00225         outputFile = _outputFile;
00226         errorCount = 0;
00227 }
00228 
00229 /*
00230  *      M e s s a g e H a n d l i n g
00231  */
00232 MessageHandling::MessageHandling(       VisibilityStatus _errorVisibility,
00233                                                                         VisibilityStatus _warningVisibility,
00234                                                                         VisibilityStatus _infoVisibility
00235                                                                         )
00236 {
00237         errorVisibility   = _errorVisibility;
00238         warningVisibility = _warningVisibility;
00239         infoVisibility    = _infoVisibility;
00240 
00241         outputFile = stderr;
00242         errorCount = 0;
00243 }
00244 
00245 /*
00246  *      M e s s a g e H a n d l i n g
00247  */
00248 MessageHandling::MessageHandling(       FILE* _outputFile,
00249                                                                         VisibilityStatus _errorVisibility,
00250                                                                         VisibilityStatus _warningVisibility,
00251                                                                         VisibilityStatus _infoVisibility
00252                                                                         )
00253 {
00254         errorVisibility   = _errorVisibility;
00255         warningVisibility = _warningVisibility;
00256         infoVisibility    = _infoVisibility;
00257 
00258         outputFile = _outputFile;
00259         errorCount = 0;
00260 }
00261 
00262 
00263 
00264 /*
00265  *      M e s s a g e H a n d l i n g
00266  */
00267 MessageHandling::MessageHandling( const MessageHandling& rhs )
00268 {
00269         errorVisibility   = rhs.errorVisibility;
00270         warningVisibility = rhs.warningVisibility;
00271         infoVisibility    = rhs.infoVisibility;
00272 
00273         outputFile = rhs.outputFile;
00274         errorCount = rhs.errorCount;
00275 }
00276 
00277 
00278 /*
00279  *      ~ M e s s a g e H a n d l i n g
00280  */
00281 MessageHandling::~MessageHandling( )
00282 {
00283         if ( outputFile != 0 )
00284                 fclose( outputFile );
00285 }
00286 
00287 
00288 /*
00289  *      o p e r a t o r =
00290  */
00291 MessageHandling& MessageHandling::operator=( const MessageHandling& rhs )
00292 {
00293         if ( this != &rhs )
00294         {
00295                 errorVisibility   = rhs.errorVisibility;
00296                 warningVisibility = rhs.warningVisibility;
00297                 infoVisibility    = rhs.infoVisibility;
00298 
00299                 outputFile = rhs.outputFile;
00300                 errorCount = rhs.errorCount;
00301         }
00302 
00303         return *this;
00304 }
00305 
00306 
00307 /*
00308  *      t h r o w E r r o r
00309  */
00310 returnValue MessageHandling::throwError(
00311         returnValue Enumber,
00312         const char* additionaltext,
00313         const char* functionname,
00314         const char* filename,
00315         const unsigned long linenumber,
00316         VisibilityStatus localVisibilityStatus
00317         )
00318 {
00319         /* consistency check */
00320         if ( Enumber <= SUCCESSFUL_RETURN )
00321                 return throwError( RET_ERROR_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00322 
00323         /* Call to common throwMessage function if error shall be displayed. */
00324         if ( errorVisibility == VS_VISIBLE )
00325                 return throwMessage( Enumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"ERROR" );
00326         else
00327                 return Enumber;
00328 }
00329 
00330 
00331 /*
00332  *      t h r o w W a r n i n g
00333  */
00334 returnValue MessageHandling::throwWarning(
00335         returnValue Wnumber,
00336         const char* additionaltext,
00337         const char* functionname,
00338         const char* filename,
00339         const unsigned long linenumber,
00340         VisibilityStatus localVisibilityStatus
00341         )
00342 {
00343         /* consistency check */
00344         if ( Wnumber <= SUCCESSFUL_RETURN )
00345                 return throwError( RET_WARNING_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00346 
00347         /* Call to common throwMessage function if warning shall be displayed. */
00348         if ( warningVisibility == VS_VISIBLE )
00349                 return throwMessage( Wnumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"WARNING" );
00350         else
00351                 return Wnumber;
00352 }
00353 
00354 
00355 /*
00356  *      t h r o w I n f o
00357  */
00358 returnValue MessageHandling::throwInfo(
00359         returnValue Inumber,
00360         const char* additionaltext,
00361         const char* functionname,
00362         const char* filename,
00363         const unsigned long linenumber,
00364         VisibilityStatus localVisibilityStatus
00365         )
00366 {
00367         /* consistency check */
00368         if ( Inumber < SUCCESSFUL_RETURN )
00369                 return throwError( RET_INFO_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00370 
00371         /* Call to common throwMessage function if info shall be displayed. */
00372         if ( infoVisibility == VS_VISIBLE )
00373                 return throwMessage( Inumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"INFO" );
00374         else
00375                 return Inumber;
00376 }
00377 
00378 
00379 /*
00380  *      r e s e t
00381  */
00382 returnValue MessageHandling::reset( )
00383 {
00384         setErrorVisibilityStatus(   VS_VISIBLE );
00385         setWarningVisibilityStatus( VS_VISIBLE );
00386         setInfoVisibilityStatus(    VS_VISIBLE );
00387 
00388         setOutputFile( stderr );
00389         setErrorCount( 0 );
00390 
00391         return SUCCESSFUL_RETURN;
00392 }
00393 
00394 
00395 /*
00396  *      l i s t A l l M e s s a g e s
00397  */
00398 returnValue MessageHandling::listAllMessages( )
00399 {
00400         #ifndef __XPCTARGET__
00401         int keypos = 0;
00402         char myPrintfString[160];
00403 
00404         /* Run through whole returnValueList and print each item. */
00405         while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
00406         {
00407                 snprintf( myPrintfString,160," %d - %s \n",keypos,returnValueList[keypos].data );
00408                 myPrintf( myPrintfString );
00409 
00410                 ++keypos;
00411         }
00412         #endif
00413 
00414         return SUCCESSFUL_RETURN;
00415 }
00416 
00417 
00418 
00419 /*****************************************************************************
00420  *  P R O T E C T E D                                                        *
00421  *****************************************************************************/
00422 
00423 
00424 /*
00425  *      t h r o w M e s s a g e
00426  */
00427 returnValue MessageHandling::throwMessage(
00428         returnValue RETnumber,
00429         const char* additionaltext,
00430         const char* functionname,
00431         const char* filename,
00432         const unsigned long linenumber,
00433         VisibilityStatus localVisibilityStatus,
00434         const char* RETstring
00435         )
00436 {
00437         #ifndef __SUPPRESSANYOUTPUT__
00438         #ifndef __XPCTARGET__
00439         int keypos = 0;
00440         char myPrintfString[160];
00441 
00442         /* 1) Determine number of whitespace for output. */
00443         char whitespaces[41];
00444         int numberOfWhitespaces = (errorCount-1)*2;
00445 
00446         if ( numberOfWhitespaces < 0 )
00447                 numberOfWhitespaces = 0;
00448 
00449         if ( numberOfWhitespaces > 40 )
00450                 numberOfWhitespaces = 40;
00451 
00452         memset( whitespaces, ' ', (size_t) numberOfWhitespaces );
00453         whitespaces[numberOfWhitespaces] = '\0';
00454 
00455         /* 2) Find error/warning/info in list. */
00456         while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
00457         {
00458                 if ( returnValueList[keypos].key == RETnumber )
00459                         break;
00460                 else
00461                         ++keypos;
00462         }
00463 
00464         if ( returnValueList[keypos].key == TERMINAL_LIST_ELEMENT )
00465         {
00466                 throwError( RET_EWI_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
00467                 return RETnumber;
00468         }
00469 
00470         /* 3) Print error/warning/info. */
00471         if ( ( returnValueList[keypos].globalVisibilityStatus == VS_VISIBLE ) && ( localVisibilityStatus == VS_VISIBLE ) )
00472         {
00473 
00474                 if ( errorCount > 0 )
00475                 {
00476                         snprintf( myPrintfString,160,"%s->", whitespaces );
00477                         myPrintf( myPrintfString );
00478                 }
00479 
00480                 if ( additionaltext == 0 )
00481                 {
00482                         #ifdef __DEBUG__
00483                         snprintf(       myPrintfString,160,"%s (%s, %s:%d): \t%s\n",
00484                                                 RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data
00485                                                 );
00486                         #else
00487                         snprintf(       myPrintfString,160,"%s:  %s\n",
00488                                                                         RETstring,returnValueList[keypos].data
00489                                                 );
00490                         #endif
00491                         myPrintf( myPrintfString );
00492                 }
00493                 else
00494                 {
00495                         #ifdef __DEBUG__
00496                         snprintf(       myPrintfString,160,"%s (%s, %s:%d): \t%s %s\n",
00497                                                 RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data,additionaltext
00498                                                 );
00499                         #else
00500                         snprintf(       myPrintfString,160,"%s:  %s %s\n",
00501                                                 RETstring,returnValueList[keypos].data,additionaltext
00502                                                 );
00503                         #endif
00504                         myPrintf( myPrintfString );
00505                 }
00506 
00507                 /* take care of proper indention for subsequent error messages */
00508                 if ( RETstring[0] == 'E' )
00509                 {
00510                         ++errorCount;
00511                 }
00512                 else
00513                 {
00514                         if ( errorCount > 0 )
00515                                 myPrintf( "\n" );
00516                         errorCount = 0;
00517                 }
00518         }
00519         #endif /* __XPCTARGET__ */
00520         #endif /* __SUPPRESSANYOUTPUT__ */
00521 
00522         return RETnumber;
00523 }
00524 
00525 /****************************************************************************/
00526 /* S T A T I C  P U B L I C */
00527 /****************************************************************************/
00528 
00529 const char* MessageHandling::getErrorCodeMessage(       const returnValue _returnValue
00530                                                                                                         )
00531 {
00532         int keypos = 0;
00533         
00534         /* 2) Find error/warning/info in list. */
00535         while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
00536         {
00537                 if ( returnValueList[keypos].key == _returnValue )
00538                         break;
00539                 else
00540                         ++keypos;
00541         }
00542 
00543         if ( returnValueList[keypos].key == TERMINAL_LIST_ELEMENT )
00544         {
00545                 return "Unknown error code";
00546         }
00547 
00548         return (returnValueList[keypos].data != 0) ? returnValueList[keypos].data : "No message for this error code";
00549 }
00550 
00551 
00552 /*****************************************************************************
00553  *  G L O B A L  M E S S A G E  H A N D L E R                                *
00554  *****************************************************************************/
00555 
00556 
00558 MessageHandling globalMessageHandler( stderr,VS_VISIBLE,VS_VISIBLE,VS_VISIBLE );
00559 
00560 
00561 /*
00562  *      g e t G l o b a l M e s s a g e H a n d l e r
00563  */
00564 MessageHandling* getGlobalMessageHandler( )
00565 {
00566         return &globalMessageHandler;
00567 }
00568 
00569 
00570 END_NAMESPACE_QPOASES
00571 
00572 
00573 /*
00574  *      end of file
00575  */


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:59:20