- Main Page
- Namespaces
- Classes
- Files
- Related Pages

# /Users/fpy/titan-1-5-MBARI/Mers/Executives/DeductiveController/Rtapi/ME/ME_rtapi.cpp

    00001
    00002 //
    00003 // Copyright (c) 2004           Model-based Embedded and Robotic Systems Group
    00004 // All Rights Reserved          Massachusetts Institute of Technology
    00005 //
    00006 // This software is provided as is. Model-based Embedded and Robotic Systems
    00007 // (MERS) group does not assume any responsibility.
    00008 //
    00009 // Organization:        Model-based Embedded and Robotic Systems Group
    00010 //                                      Massachusetts Institute of Technology
    00011 //
    00013
    00014 #include "ME_rtapi.h"
    00015
    00016 #include <cassert>
    00017
    00018
    00019 #include "ME_dispatcher.h"
    00020 #include <Mers/Executives/DeductiveController/Rtapi/RTAPIMessage.h>
    00021 #include <Mers/Estimation/Discrete/Stub/ME_stub.h>
    00022 #include <Mers/Utils/Lang/Sleep.h>
    00023 //#include <test/time_delays.h>
    00024 #include <stdio.h>
    00025
    00027
    00028 using namespace Mers::Executives::DeductiveController::Rtapi::ME;
    00029 using namespace Mers::Estimation::Discrete::Stub;
    00030 using Mers::Executives::DeductiveController::Rtapi::RTAPICallback;
    00031 using Mers::Executives::DeductiveController::Rtapi::CommandCallback;
    00032 using Mers::Executives::DeductiveController::Rtapi::RTAPIMessage;
    00033 using Mers::Estimation::Discrete::ME_connection;
    00034 using Mers::Utils::Containers::ObjectArray;
    00035 using Mers::Utils::Lang::sleep;
    00036
    00038
    00039 typedef struct{
    00040         ME_rtapi * rtapi;
    00041         RTAPIMessage * msg;
    00042 }RTITimerPacket;
    00043
    00044 /*
    00045  * this is the function called to create a timer, then invoke an appropriate
    00046  * handler after the specified timeout has elapsed.
    00047  * @param rtiTimePckt pointer to a RTITimerPacket object which has been
    00048  *        initialized to the message being processed as well as the
    00049  *        api object calling this timer routine
    00050  */
    00051 void * rti_timer(void * rtiTimePckt)
    00052 {
    00053         assert(rtiTimePckt);
    00054         RTITimerPacket * pkt = ((RTITimerPacket *)rtiTimePckt);
    00055         int sleeptime = 0;
    00056
    00057         assert(pkt->msg);
    00058         switch (pkt->msg->getMessageType())
    00059         {
    00060         case RTAPIMessage::OBSERVATION:
    00061                 sleeptime = get_observation_timeout(pkt->msg->getVariableUID());
    00062
    00063                 // make sure we sleep 'sleeptime', sleeping again if we're interrupted
    00064                 while (sleeptime > 0)
    00065                         sleeptime = sleep(sleeptime);
    00066                 assert(pkt->rtapi);
    00067                 pkt->rtapi->observation_timeout();
    00068                 break;
    00069         case RTAPIMessage::COMMAND:
    00070                 sleeptime = get_command_timeout(pkt->msg->getVariableUID());
    00071
    00072                 // make sure we sleep 'sleeptime', sleeping again if we're interrupted
    00073                 while (sleeptime > 0)
    00074                         sleeptime = sleep(sleeptime);
    00075                 pkt->rtapi->command_timeout();
    00076                 break;
    00077         default:
    00078                 break;
    00079         }
    00080         // msg was created for us, rtapi is global (a reference)
    00081         // so delete msg but not rtapi
    00082         delete pkt->msg;
    00083         return 0;
    00084 }
    00085
    00087 //  Constructor/Destructor
    00089
    00090 /*
    00091 * Creates the object with a handle to the ME subsystem as well as a
    00092 * call back function for results from the ME subsystem
    00093 * @param syst handle to ME
    00094 * @param meCallback handle to the callback object
    00095 */
    00096 ME_rtapi::ME_rtapi(ME_connection * syst, RTAPICallback * meCallback)
    00097         : thequeue(*(new ThreadSafeObjectQueue())), listeners(true)
    00098 {
    00099         thequeue.addReference();
    00100
    00101         assert(syst);
    00102         assert(meCallback);
    00103         mec = syst;
    00104
    00105         dispatcher = new ME_dispatcher(&thequeue, mec, meCallback);
    00106         assert(dispatcher);
    00107
    00108         int err = pthread_attr_init(&timer_attr);
    00109         assert(!err);
    00110
    00111         pthread_attr_setdetachstate(&timer_attr, PTHREAD_CREATE_DETACHED);
    00112         pthread_attr_setscope(&timer_attr, PTHREAD_SCOPE_SYSTEM);
    00113
    00114         err = pthread_mutex_init(&timersMutex, NULL);
    00115         assert(!err);
    00116         activeTimers = 0;
    00117
    00118         active = true;
    00119 }
    00120
    00121 /*
    00122 * Destroys this object and kills the dispatcher
    00123 */
    00124 ME_rtapi::~ME_rtapi()
    00125 {
    00126         // all future calls to this api will report that api is going away and
    00127         // just return
    00128         active = false;
    00129
    00130         RTAPIMessage * msg = new RTAPIMessage(RTAPIMessage::QUIT);
    00131
    00132         thequeue.enqueue(msg);
    00133
    00134         // normal behavior is to wait until the queue is emptied
    00135         thequeue.waitForEmpty();
    00136
    00137         // printf("Removing ME_rtapi Reference\n");
    00138         thequeue.removeReference();
    00139
    00140         // kill dispatcher process and delete object
    00141         // the dispatcher object may or may not be suspended when arriving here
    00142
    00143         // we have the option of posting a stop quest to the other thread
    00144         // instead of posting a message in the queue (the latter will tend
    00145         // to take longer to finish), but we would potentially have to wake
    00146         // up the other thread which is complicated...
    00147
    00148         // dispatcher->stop();
    00149         // delete dispatcher;
    00150
    00151         pthread_attr_destroy(&timer_attr);
    00152         pthread_mutex_destroy(&timersMutex);
    00153
    00154         listeners.makeEmpty();
    00155 }
    00156
    00157
    00158 /*
    00159 * adds command and its assignment to the message queue
    00160 */
    00161 int ME_rtapi::queue_command(unsigned int cmd_index,
    00162                                                          unsigned int cmd_value)
    00163 {
    00164         if(!active)
    00165                 return -1;
    00166         RTAPIMessage * msg = new RTAPIMessage(RTAPIMessage::COMMAND, cmd_index,
    00167                 cmd_value);
    00168         assert(msg);
    00169
    00170         int err = thequeue.enqueue(msg);
    00171         if(!FAILED(err) && listeners.getLength() > cmd_index &&
    00172                 listeners[cmd_index] != NULL){
    00173                 unsigned int x;
    00174                 ObjectArray * objs = (ObjectArray *) (listeners[cmd_index]);
    00175
    00176                 for(x = 0; x < objs->getLength(); x++){
    00177                         CommandCallback * cc = (CommandCallback *) objs->operator [](x);
    00178
    00179                         cc->issueCommand(cmd_index, cmd_value);
    00180                 }
    00181         }
    00182         return err;
    00183 }
    00184
    00185 // adds monitor observed values to the message queue for assignment
    00186 int ME_rtapi::queue_observation(unsigned int obs_index,
    00187                                                                  unsigned int value_index)
    00188 {
    00189         if(!active)
    00190                 return -1;
    00191         RTAPIMessage * msg = new RTAPIMessage(RTAPIMessage::OBSERVATION, obs_index,
    00192                 value_index);
    00193         assert(msg);
    00194         return thequeue.enqueue(msg);
    00195 }
    00196
    00197 // adds monitor state values to the message queue for assignment
    00198 int ME_rtapi::queue_initial_state(unsigned int state_index,
    00199                                                                  unsigned int value_index)
    00200 {
    00201         if(!active)
    00202                 return -1;
    00203         RTAPIMessage * msg = new RTAPIMessage(RTAPIMessage::INITIALIZE_STATE, state_index,
    00204                 value_index);
    00205         assert(msg);
    00206         return thequeue.enqueue(msg);
    00207 }
    00208
    00209 /*
    00210 * adds a list of observed values to the message queue
    00211 */
    00212 int ME_rtapi::queue_observations(int number,
    00213                                                                   unsigned int obs[], unsigned int values[])
    00214 {
    00215         if(!active)
    00216                 return -1;
    00217         int ret;
    00218         for(int i = 0; i < number; i++){
    00219                 ret = queue_observation(obs[i], values[i]);
    00220                 if(FAILED(ret))
    00221                         return ret;
    00222         }
    00223         return 0;
    00224 }
    00225
    00226 /*
    00227 * queues a message requesting a new mode estimate, performs a statisfaction
    00228 * verification
    00229 */
    00230 int ME_rtapi::queue_find_mode_estimate()
    00231 {
    00232         if(!active)
    00233                 return -1;
    00234         RTAPIMessage * msg = new RTAPIMessage(RTAPIMessage::FIND_MODE_ESTIMATE);
    00235         assert(msg);
    00236         return thequeue.enqueue(msg);
    00237 }
    00238
    00239 /*************************************************************************/
    00240
    00241 int ME_rtapi::queue_start_command_and_time(unsigned int cmd_index,
    00242                                                                                         unsigned int cmd_value)
    00243 {
    00244         if(!active)
    00245                 return -1;
    00246         pthread_t tid;
    00247         int err;
    00248
    00249         RTAPIMessage * msg =
    00250                 new RTAPIMessage(RTAPIMessage::COMMAND, cmd_index, cmd_value);
    00251         assert(msg);
    00252
    00253         // start timeout task
    00254         // create a thread and jump to the 'rti_timer' function
    00255         // with an RTITimerPacket as argument.
    00256         RTITimerPacket * pkt = new RTITimerPacket;
    00257
    00258         // this is a problem - the queue will probably delete the message
    00259         // while the timer is running, we should probably create two messages
    00260         // but it works right now since we use the msg first in the new thread
    00261         // but it's possible for the message to be processed first on certain
    00262         // machines
    00263         pkt->msg =
    00264                 new RTAPIMessage(msg);
    00265         assert(pkt->msg);
    00266         pkt->rtapi = this;
    00267
    00268         err = pthread_create(&tid, &timer_attr, rti_timer, pkt);
    00269         assert(!err);
    00270
    00271         incrementTimeouts();
    00272
    00273         //add the command to the queue as well
    00274         err = thequeue.enqueue(msg);
    00275         if(!FAILED(err) && listeners.getLength() > cmd_index &&
    00276                 listeners[cmd_index] != NULL){
    00277                 unsigned int x;
    00278                 ObjectArray * objs = (ObjectArray *) (listeners[cmd_index]);
    00279
    00280                 for(x = 0; x < objs->getLength(); x++){
    00281                         CommandCallback * cc = (CommandCallback *) objs->operator [](x);
    00282
    00283                         cc->issueCommand(cmd_index, cmd_value);
    00284                 }
    00285         }
    00286         return err;
    00287 }
    00288
    00289 /*************************************************************************/
    00290
    00291 int ME_rtapi::queue_observations_and_time(unsigned int obs_index,
    00292                                                                                    unsigned int value_index)
    00293 {
    00294         if(!active)
    00295                 return -1;
    00296         pthread_t tid;
    00297         int err;
    00298
    00299         RTAPIMessage *msg =
    00300                 new RTAPIMessage(RTAPIMessage::OBSERVATION, obs_index, value_index);
    00301         assert(msg);
    00302
    00303         // if there isn't anything already running, start a timer
    00304         if(getTimeouts() == 0)
    00305         {
    00306                 // start timeout task
    00307                 // create a thread and jump to the 'rti_timer' function
    00308                 // with an RTITimerPacket as argument.
    00309                 RTITimerPacket * pkt = new RTITimerPacket;
    00310
    00311                 // this is a problem - the queue will probably delete the message
    00312                 // while the timer is running, we should probably create two messages
    00313                 // but it works right now since we use the msg first in the new thread
    00314                 // but it's possible for the message to be processed first on certain
    00315                 // machines
    00316                 pkt->msg =
    00317                         new RTAPIMessage(RTAPIMessage::OBSERVATION, obs_index, value_index);
    00318                 assert(pkt->msg);
    00319                 pkt->rtapi = this;
    00320
    00321                 err = pthread_create(&tid, &timer_attr, rti_timer, pkt);
    00322                 assert(!err);
    00323                 incrementTimeouts();
    00324         }
    00325
    00326         //add the command to the queue as well
    00327         return thequeue.enqueue(msg);
    00328 }
    00329
    00330 int ME_rtapi::queue_abort_command()
    00331 {
    00332         if(!active)
    00333                 return -1;
    00334         // once a command is queued it's not possible to abort it
    00335         return 1;
    00336 }
    00337
    00338 int ME_rtapi::queue_report_full_state()
    00339 {
    00340         if(!active)
    00341                 return -1;
    00342         RTAPIMessage *msg = new RTAPIMessage(RTAPIMessage::REPORT_FULL_STATE);
    00343         assert(msg);
    00344         return thequeue.enqueue(msg);
    00345 }
    00346
    00347 /*************************************************************************/
    00348
    00349 int ME_rtapi::registerCommandListener(unsigned int command, CommandCallback * listener)
    00350 {
    00351         // check to see if listeners is large enough to index with the command UID
    00352         if(listeners.getLength() < command){
    00353                 if(FAILED(listeners.resize(command + 1)))
    00354                         return 1;
    00355         }
    00356         // check to see if we've got an array at the command UID
    00357         if(listeners[command] == NULL){
    00358                 listeners[command] = new ObjectArray(true);
    00359                 if(!listeners[command]){
    00360                         fprintf(stderr, "Could not create a new listener array\n");
    00361                         return 2;
    00362                 }
    00363         }
    00364         if(FAILED(((ObjectArray *)(listeners[command]))->pushBack(listener)))
    00365                 return 3;
    00366         return 0;
    00367 }
    00368
    00369 /*************************************************************************/
    00370
    00371 // this function is called by default when a command or observation
    00372 // timer times out
    00373 int ME_rtapi::observation_timeout()
    00374 {
    00375         bool findME;
    00376         assert(this);
    00377
    00378         findME = false;
    00379
    00380         // we only call queue_find_mode_estimate when all the active
    00381         // commands/observations are done
    00382         pthread_mutex_lock(&timersMutex);
    00383         activeTimers--;
    00384         findME = (activeTimers == 0);
    00385         pthread_mutex_unlock(&timersMutex);
    00386
    00387         if(findME)
    00388                 return queue_find_mode_estimate();
    00389         return 0;
    00390 }
    00391
    00392 // this function is called by default when a command or observation
    00393 // timer times out
    00394 int ME_rtapi::command_timeout()
    00395 {
    00396         bool findME;
    00397         assert(this);
    00398
    00399         findME = false;
    00400
    00401         // we only call queue_find_mode_estimate when all the active
    00402         // commands/observations are done
    00403         pthread_mutex_lock(&timersMutex);
    00404         activeTimers--;
    00405         findME = (activeTimers == 0);
    00406         pthread_mutex_unlock(&timersMutex);
    00407
    00408         if(findME)
    00409                 return queue_find_mode_estimate();
    00410         return 0;
    00411 }
    00412
    00413 void ME_rtapi::incrementTimeouts()
    00414 {
    00415         // on many machines you can do something like
    00416         // add [this + activeTimers], 1
    00417         // or
    00418         // inc [this + activeTimers]
    00419         // which are unit operations, so this mutex would be unnecessary
    00420         pthread_mutex_lock(&timersMutex);
    00421         activeTimers++;
    00422         pthread_mutex_unlock(&timersMutex);
    00423 }
    00424
    00425 void ME_rtapi::decrementTimeouts()
    00426 {
    00427         pthread_mutex_lock(&timersMutex);
    00428         activeTimers--;
    00429         pthread_mutex_unlock(&timersMutex);
    00430 }
    00431
    00432 unsigned int ME_rtapi::getTimeouts()
    00433 {
    00434         unsigned int aTimers;
    00435         pthread_mutex_lock(&timersMutex);
    00436         aTimers = activeTimers;
    00437         pthread_mutex_unlock(&timersMutex);
    00438         return aTimers;
    00439 }