engine_thread.cpp

00001 
00002 /***************************************************************************
00003  *  engine_thread.cpp - Thread driving the XABSL Engine
00004  *
00005  *  Created: Thu Aug 07 17:01:29 2008
00006  *  Copyright  2006-2008  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "engine_thread.h"
00024 #include "xabsl_tools.h"
00025 #include "skill_wrapper.h"
00026 
00027 #include <core/exceptions/software.h>
00028 #include <utils/time/time.h>
00029 #include <interfaces/SkillerInterface.h>
00030 #include <interfaces/ObjectPositionInterface.h>
00031 
00032 #include <XabslEngine/XabslEngine.h>
00033 
00034 using namespace fawkes;
00035 
00036 
00037 /** Global XabslEngineThread required for xet_current_time(). */
00038 static XabslEngineThread *g_xe = NULL;
00039 
00040 /** Get current time.
00041  * Uses a globally set XabslEngineThread instance to determine the current
00042  * time, may be simulated time!
00043  * @return continuous time in miliseconds
00044  */
00045 static unsigned long int
00046 xet_current_time()
00047 {
00048   if ( ! g_xe) {
00049     throw NullPointerException("No XabslEngineThread instance exists");
00050   }
00051 
00052   return g_xe->current_time();
00053 }
00054 
00055 /** @class XabslEngineThread "engine_thread.h"
00056  * Xabsl Engine Thread.
00057  * This thread drives the Xabsl engine.
00058  * @author Tim Niemueller
00059  */
00060 
00061 
00062 /** Constructor. */
00063 XabslEngineThread::XabslEngineThread()
00064   : Thread("XabslEngineThread", Thread::OPMODE_WAITFORWAKEUP),
00065     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_THINK)
00066 {
00067 }
00068 
00069 
00070 void
00071 XabslEngineThread::init()
00072 {
00073   if ( g_xe ) {
00074     throw Exception("Global XabslEngineThread has already been set.");
00075   }
00076   g_xe   = this;
00077 
00078   __xe         = NULL;
00079   __xleh       = NULL;
00080   __now        = NULL;
00081   __ball_rx    = NULL;
00082   __ball_ry    = NULL;
00083   __skiller_if = NULL;
00084   __wm_ball_if = NULL;
00085 
00086   __now  = new Time(clock);
00087   __xleh = new XabslLoggingErrorHandler(logger);
00088   __xe   = new xabsl::Engine(*__xleh, &xet_current_time);
00089 
00090   __wm_ball_if = blackboard->open_for_reading<ObjectPositionInterface>("WM Ball");
00091   __skiller_if = blackboard->open_for_reading<SkillerInterface>("Skiller");
00092 
00093   XabslSkillWrapper::ParameterList params;
00094   params.push_back(std::make_pair("x", "double"));
00095   params.push_back(std::make_pair("y", "double"));
00096   params.push_back(std::make_pair("ori", "double"));
00097   XabslSkillWrapper *sw = new XabslSkillWrapper("relgoto", *__xleh, params);
00098   __wrappers[sw->name()] = sw;
00099   __xe->registerBasicBehavior(*sw);
00100 
00101   __ball_ry = __ball_rx = NULL;
00102   for (Interface::FieldIterator i = __wm_ball_if->fields(); i != __wm_ball_if->fields_end(); ++i) {
00103     if ( strcmp(i.get_name(), "relative_x") == 0 ) {
00104       __ball_rx = new XabslInterfaceFieldWrapper<double, float>(i.get_type(), i.get_name(), (float *)i.get_value());
00105       __xe->registerDecimalInputSymbol("ball.relative_x", __ball_rx,
00106                                        (double (xabsl::FunctionProvider::*)())&XabslInterfaceFieldWrapper<double, float>::get_value);
00107     } else if ( strcmp(i.get_name(), "relative_y") == 0 ) {
00108       __ball_ry = new XabslInterfaceFieldWrapper<double, float>(i.get_type(), i.get_name(), (float *)i.get_value());
00109       __xe->registerDecimalInputSymbol("ball.relative_y", __ball_ry,
00110                                        (double (xabsl::FunctionProvider::*)())&XabslInterfaceFieldWrapper<double, float>::get_value);
00111     }
00112   }
00113 
00114   XabslFileInputSource xinput(XABSLDIR"agent.xabslc");
00115   __xe->createOptionGraph(xinput);
00116 
00117   if ( __xleh->errorsOccurred ) {
00118     finalize();
00119     throw Exception("Error while creating XABSL engine, see log for details");
00120   }
00121 
00122   /* Test code, exporting interfaces to allow for real skill-level programming
00123    * is an overly complex and error prone task.
00124    * Since C++ methods for basic behaviors for sending a message cannot be
00125    * created on-the-fly wrappers would need to be written or generated for each
00126    * possible message type.
00127 
00128   __navi_if = blackboard->open_for_reading<NavigatorInterface>("Navigator");
00129 
00130   std::string base_name = "navi_";
00131   Interface::FieldIterator i;
00132   for (i = __navi_if->fields(); i != __navi_if->fields_end(); ++i) {
00133     switch (i.get_type()) {
00134     case Interface::IFT_BOOL:
00135       {
00136         XabslInterfaceFieldWrapper<bool> *ifw = new XabslInterfaceFieldWrapper<bool>(new InterfaceFieldPointer<bool>(i.get_type(), i.get_name(), (bool *)i.get_value()));
00137         __xe->registerBooleanInputSymbol((base_name + ifw->get_name()).c_str(),
00138                                          ifw,
00139                                          (bool (xabsl::FunctionProvider::*)())&XabslInterfaceFieldWrapper<bool>::get_value);
00140         __xe->registerBooleanOutputSymbol((base_name + ifw->get_name()).c_str(),
00141                                           ifw,
00142                                           (void (xabsl::FunctionProvider::*)(bool))&XabslInterfaceFieldWrapper<bool>::set_value,
00143                                           (bool (xabsl::FunctionProvider::*)())&XabslInterfaceFieldWrapper<bool>::get_value);
00144       }
00145       break;
00146     case Interface::IFT_INT:
00147     case Interface::IFT_UINT:
00148     case Interface::IFT_LONGINT:
00149     case Interface::IFT_LONGUINT:
00150     case Interface::IFT_FLOAT:
00151       {
00152         XabslInterfaceFieldWrapper<double> *ifw = new XabslInterfaceFieldWrapper<double>(new InterfaceFieldPointer<double>(i.get_type(), i.get_name(), (double *)i.get_value()));
00153         __xe->registerDecimalInputSymbol((base_name + ifw->get_name()).c_str(),
00154                                          ifw,
00155                                          (double (xabsl::FunctionProvider::*)())&XabslInterfaceFieldWrapper<double>::get_value);
00156         __xe->registerDecimalOutputSymbol((base_name + ifw->get_name()).c_str(),
00157                                           ifw,
00158                                           (void (xabsl::FunctionProvider::*)(double))&XabslInterfaceFieldWrapper<double>::set_value,
00159                                           (double (xabsl::FunctionProvider::*)())&XabslInterfaceFieldWrapper<double>::get_value);
00160       }
00161       break;
00162     case Interface::IFT_STRING:
00163       // ignored, XABSL can't handle that
00164       break;
00165     }
00166   }
00167   */
00168 }
00169 
00170 
00171 void
00172 XabslEngineThread::finalize()
00173 {
00174   g_xe = NULL;
00175 
00176   for (__wit = __wrappers.begin(); __wit != __wrappers.end(); ++__wit) {
00177     delete __wit->second;
00178   }
00179   __wrappers.clear();
00180 
00181   delete __xe;
00182   delete __xleh;
00183   delete __now;
00184   delete __ball_rx;
00185   delete __ball_ry;
00186 
00187   if (__skiller_if)  blackboard->close(__skiller_if);
00188   if (__wm_ball_if)  blackboard->close(__wm_ball_if);
00189 }
00190 
00191 
00192 void
00193 XabslEngineThread::once()
00194 {
00195   try { 
00196     __skiller_if->msgq_enqueue(new SkillerInterface::AcquireControlMessage());
00197   } catch (Exception &e) {
00198     logger->log_error("XabslEngineThread", "Cannot aquire exclusive skiller "
00199                       "control, exception follows");
00200     logger->log_error("XabslEngineThread", e);
00201   }
00202 }
00203 
00204 void
00205 XabslEngineThread::loop()
00206 {
00207   __now->stamp();
00208 
00209   __wm_ball_if->read();
00210   __skiller_if->read();
00211 
00212   __xe->execute();
00213 
00214   std::string skill_string = "";
00215   for (__wit = __wrappers.begin(); __wit != __wrappers.end(); ++__wit) {
00216     std::string css = __wit->second->skill_string();
00217     if ( css != "" ) {
00218       skill_string += css + "; ";
00219     }
00220   }
00221   if ( skill_string != "" ) {
00222     logger->log_debug(name(), "Skill string: %s", skill_string.c_str());
00223   }
00224 
00225   try { 
00226     __skiller_if->msgq_enqueue(new SkillerInterface::ExecSkillMessage(skill_string.c_str()));
00227   } catch (Exception &e) {
00228     logger->log_warn("XabslEngineThread", "Executing skill failed, exception follows");
00229     logger->log_warn("XabslEngineThread", e);
00230   }
00231 }
00232 
00233 
00234 /** Get current time.
00235  * @return continuous time in miliseconds
00236  */
00237 unsigned long int
00238 XabslEngineThread::current_time()
00239 {
00240   return __now->in_msec();
00241 }

Generated on Tue Feb 22 13:32:31 2011 for Fawkes API by  doxygen 1.4.7