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

# /Users/fpy/titan-1-5-MBARI/Mers/Executives/DeductiveController/Rtapi/ME/ME_dispatcher.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_dispatcher.h"
    00015
    00016 #include <cassert>
    00017 #include <Mers/Utils/Containers/intList.h>
    00018
    00020
    00021 using namespace Mers::Executives::DeductiveController::Rtapi::ME;
    00022 using Mers::Executives::DeductiveController::Rtapi::ThreadSafeObjectQueue;
    00023 using Mers::Executives::DeductiveController::Rtapi::RTAPICallback;
    00024 using Mers::Executives::DeductiveController::Rtapi::RTAPIMessage;
    00025 using Mers::Estimation::Discrete::ME_connection;
    00026 using Mers::Utils::Containers::intList;
    00027 using Mers::Utils::Lang::BaseModelObject;
    00028
    00029 //using Mers::Executives::DeductiveController::Rtapi::
    00030
    00032
    00038 ME_dispatcher::ME_dispatcher(ThreadSafeObjectQueue * queue, ME_connection * syst,
    00039                              RTAPICallback * inrep) :
    00040 MultiThreadedObject(), meCallback(inrep), thequeue(queue), thesystem(syst)
    00041 {
    00042         assert(queue);
    00043         assert(syst);
    00044         assert(inrep);
    00045         thequeue->addReference();
    00046         // cout << "Starting ME Dispatcher" << endl;
    00047         start();
    00048 }
    00049
    00050 ME_dispatcher::~ME_dispatcher()
    00051 {
    00052         assert(thesystem != NULL);
    00053         assert(thequeue != NULL);
    00054         delete thesystem;
    00055         // printf("Removing ME_dispatcher Reference\n");
    00056         thequeue->removeReference();
    00057 }
    00058
    00059 // parse_msg gets a message popped off the queue and calls the Livingstone
    00060 // system at the back-end of the L2 RT-API. The function called depends
    00061 // on the type of the message.
    00062 int ME_dispatcher::parse_msg(RTAPIMessage *msg)
    00063 {
    00064         assert(msg);
    00065         intList const * meData;
    00066
    00067         // printf("ME Message\n");
    00068
    00069         // switch on the function specified, parse arguments accordingly
    00070         switch(msg->getMessageType()) {
    00071         case RTAPIMessage::COMMAND:
    00072                 thesystem->progress(msg->getVariableUID(), msg->getValueUID());
    00073                 meCallback->post_observation(msg->getVariableUID(), msg->getValueUID());
    00074                 break;
    00075
    00076         case RTAPIMessage::OBSERVATION:
    00077                 thesystem->observe(msg->getVariableUID(), msg->getValueUID());
    00078                 meCallback->post_observation(msg->getVariableUID(), msg->getValueUID());
    00079                 break;
    00080
    00081         case RTAPIMessage::FIND_MODE_ESTIMATE:
    00082
    00083                 meData = thesystem->find_mode_estimate();
    00084                 meCallback->post_mode_estimate(*meData);
    00085                 delete meData;
    00086
    00087                 // Note that the progress_to_estimate is currently handled by the MEInterface!
    00088                 // That is a bit dangerous, but we definitely should not do it twice.
    00089                 //thesystem->progress_to_estimate();  // make the estimate so
    00090                 break;
    00091
    00092         case RTAPIMessage::ABORT_COMMAND:
    00093                 break;
    00094
    00095         case RTAPIMessage::REPORT_FULL_STATE:
    00096
    00097                 // TODO: previously, the return value from post_full_state was
    00098                 //       not being deleted.  Was this a memory leak?
    00099                 meData = thesystem->get_full_state();
    00100                 meCallback->post_full_state(*meData);
    00101                 delete meData;
    00102
    00103                 // need to call the user's function here
    00104                 // assume it was a function object passed upon api creation
    00105                 // this function can do whatever the user at the front end chooses to
    00106                 // in the implementation of the ReporterInterface.
    00107                 break;
    00108                 // mode reconfig interface
    00109         case RTAPIMessage::QUIT:
    00110                 return 1;
    00111         case RTAPIMessage::INITIALIZE_STATE:
    00112                 thesystem->set_initial_state(msg->getVariableUID(), msg->getValueUID());
    00113                 break;
    00114         default:
    00115                 break;
    00116         }
    00117         return 0;
    00118 }
    00119
    00120 #define THREAD_RETURN void*
    00121 // This is the main dispatching functions that in an infinite loop pops
    00122 // off messages from the queue and parses them to the Livingstone system.
    00123
    00124 THREAD_RETURN ME_dispatcher::run()
    00125 {
    00126         //meCallback->open(); // opens stream for output
    00127         // pthread_cleanup_push(cleanup_reporting,  &meCallback);
    00128
    00129         // pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
    00130         // requires pthread_test_cancel() calls as appropriate
    00131
    00132         // pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
    00133
    00134         // this is where it all takes place
    00135         BaseModelObject * msg;
    00136
    00137         // printf("ME Dispatcher Running\n");
    00138
    00139         while (1) {
    00140
    00141                 // note this is okay, since if the dispatcher is already suspended, it's
    00142                 // in a condition variable wait, that is a cancellation point
    00143                 // Note also that WIN32 in fact needs something like this, TODO
    00144                 // pthread_testcancel();
    00145
    00146                 // first, wait to retrieve a message from the queue
    00147                 if(FAILED(thequeue->dequeue(msg, true)))
    00148                         break;
    00149
    00150                 // now parse the message and invoke
    00151                 if(parse_msg((RTAPIMessage *)msg) != 0)
    00152                         break;
    00153
    00154                 delete msg;
    00155         }
    00156
    00157         m_ThreadIsRunning = false;
    00158
    00159         //pthread_cleanup_pop(1);
    00160         delete this;
    00161
    00162         pthread_exit(NULL);
    00163         return NULL;
    00164 }
    00165
    00166 void ME_dispatcher::cleanup_reporting(void *arg)
    00167 {
    00168         assert(arg);
    00169   //ReporterInterface *rptr = (ReporterInterface *) arg;
    00170   //rptr->close();
    00171 }
    00172