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

# /Users/fpy/titan-1-5-MBARI/Mers/Executives/DeductiveController/Rtapi/MR/MR_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 "MR_dispatcher.h"
    00015
    00016 #include <cstdio>
    00017 #include <cassert>
    00018
    00020
    00021 using namespace Mers::Executives::DeductiveController::Rtapi::MR;
    00022 using Mers::Executives::DeductiveController::Rtapi::RTAPIMessage;
    00023 using Mers::Executives::DeductiveController::Rtapi::RTAPICallback;
    00024 using Mers::Executives::DeductiveController::Rtapi::ThreadSafeObjectQueue;
    00025 using Mers::Reconfiguration::Discrete::MR_connection;
    00026 using Mers::Utils::Containers::intList;
    00027 using Mers::Utils::Lang::BaseModelObject;
    00028 using log4cplus::Logger;
    00029
    00031
    00032 Logger MR_dispatcher::m_Logger( Logger::getInstance("Mers.Executives."
    00033         "DeductiveController.Rtapi.MR.MR_dispatcher") );
    00034
    00036
    00043 // constructors and deconstructors
    00044
    00045 MR_dispatcher::MR_dispatcher(ThreadSafeObjectQueue *queue,
    00046         MR_connection *mr_con, RTAPICallback *mr_callback) :
    00047   MultiThreadedObject(), thequeue(queue), goals()
    00048 {
    00049         thequeue->addReference();
    00050         themr_con = mr_con;
    00051         themr_callback = mr_callback;
    00052         start();
    00053 }
    00054
    00055 MR_dispatcher::~MR_dispatcher()
    00056 {
    00057         assert(themr_con != NULL);
    00058         assert(thequeue != NULL);
    00059         delete themr_con;
    00060         // printf("Removing MR_dispatcher Reference\n");
    00061         thequeue->removeReference();
    00062 }
    00063 //-----------------------------------------------------------------------------
    00064
    00065
    00066 // methods
    00067
    00068 //-----------------------------------------------------------------------------
    00069 void
    00070 MR_dispatcher::parse_msg(RTAPIMessage *msg)
    00071 {
    00072         LOG4CPLUS_DEBUG( m_Logger, "Parsing MR message: " << *msg );
    00073
    00074         switch(msg->getMessageType()) {
    00075         case RTAPIMessage::GOAL:
    00076           //printf("Dispatcher: Goal dequeued %d %d \n",msg->args[0],msg->args[1]);
    00077                 goals.push_back(msg->getVariableUID()); // variable
    00078                 goals.push_back(msg->getValueUID()); // value
    00079                 // queue goal
    00080                 break;
    00081         case RTAPIMessage::STATE_UPDATE:
    00082                 themr_con->update_current_state(msg->getVariableUID(), msg->getValueUID());
    00083                 break;
    00084         case RTAPIMessage::MR_REQUEST:
    00085                 {
    00086                         LOG4CPLUS_DEBUG( m_Logger, "Running MR with goals: " << goals );
    00087                         intList next_control_cmd = themr_con->generate_commands(goals);
    00088                         LOG4CPLUS_DEBUG( m_Logger, "MR returned command: " << next_control_cmd );
    00089
    00090                         // Clear the list of goals after use.
    00091                   goals.clear();
    00092
    00093                         // pass control cmd to SCL
    00094                         themr_callback->process_next_control_cmd(next_control_cmd);
    00095                 }
    00096                 break;
    00097         default:;
    00098         }
    00099 }
    00100 //-----------------------------------------------------------------------------
    00101 #define THREAD_RETURN void*
    00102
    00103 THREAD_RETURN
    00104 MR_dispatcher::run()
    00105 {
    00106         pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
    00107         // requires pthread_test_cancel() calls as appropriate
    00108         pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
    00109
    00110         // main loop of message popping
    00111         BaseModelObject * msg;
    00112
    00113   while (1) {
    00114
    00115                 // note this is okay, since if the dispatcher is already suspended,
    00116                 // it's in a condition variable wait, that is a cancellation point
    00117                 // Note also that WIN32 in fact needs something like this, TODO
    00118                 // pthread_testcancel();
    00119
    00120                 // first, wait to retrieve a message from the queue
    00121                 thequeue->dequeue(msg, true);
    00122
    00123                 //cout << "MR_dispatcher::got message from queue" << endl;
    00124
    00125                 // Make sure we got a valid message:
    00126                 if(msg != NULL) {
    00127
    00128                         // check for quiting
    00129                         if(((RTAPIMessage *)msg)->getMessageType() == RTAPIMessage::QUIT)
    00130                                 break;
    00131
    00132                         // now parse the message and invoke
    00133                         parse_msg((RTAPIMessage *) msg);
    00134     } else {
    00135                         fprintf(stderr, "Warning, MR RTAPI received a NULL message\n");
    00136                 }
    00137         }
    00138
    00139         delete this;
    00140
    00141         pthread_exit(NULL);
    00142         return NULL;
    00143 }
    00144
    00145 //-----------------------------------------------------------------------------
    00146