Fawkes API Fawkes Development Version
|
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 }