engine_thread.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00038 static XabslEngineThread *g_xe = NULL;
00039
00040
00041
00042
00043
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
00056
00057
00058
00059
00060
00061
00062
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
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
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
00235
00236
00237 unsigned long int
00238 XabslEngineThread::current_time()
00239 {
00240 return __now->in_msec();
00241 }