ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "lua_utility.h" 00008 #include <argos3/core/utility/configuration/argos_exception.h> 00009 #include <argos3/core/utility/math/angles.h> 00010 #include <argos3/core/utility/math/vector2.h> 00011 #include <argos3/core/utility/math/vector3.h> 00012 #include <argos3/core/utility/math/quaternion.h> 00013 #include <argos3/core/utility/datatypes/color.h> 00014 00015 namespace argos { 00016 00017 /****************************************/ 00018 /****************************************/ 00019 00020 static const CRange<Real> UNIT(0.0f, 1.0f); 00021 00022 int LuaRNGBernoulli(lua_State* pt_state) { 00023 /* Check number of parameters */ 00024 if(lua_gettop(pt_state) > 1) { 00025 return luaL_error(pt_state, "robot.random.bernoulli() expects 0 or 1 arguments"); 00026 } 00027 /* Get RNG instance */ 00028 CRandom::CRNG* pcRNG = CLuaUtility::GetDeviceInstance<CRandom::CRNG>(pt_state, "random"); 00029 /* Perform wanted action */ 00030 if(lua_gettop(pt_state) == 0) { 00031 /* Return random number */ 00032 lua_pushnumber(pt_state, pcRNG->Bernoulli()); 00033 return 1; 00034 } 00035 else { 00036 /* Check parameter */ 00037 luaL_checktype(pt_state, 1, LUA_TNUMBER); 00038 /* Return random number */ 00039 lua_pushnumber(pt_state, 00040 pcRNG->Bernoulli(lua_tonumber(pt_state, 1))); 00041 return 1; 00042 } 00043 /* Can't reach this point */ 00044 return 0; 00045 } 00046 00047 int LuaRNGUniform(lua_State* pt_state) { 00048 /* Check number of parameters */ 00049 if(lua_gettop(pt_state) > 2) { 00050 return luaL_error(pt_state, "robot.random.uniform() expects 0, 1, or 2 arguments"); 00051 } 00052 /* Get RNG instance */ 00053 CRandom::CRNG* pcRNG = CLuaUtility::GetDeviceInstance<CRandom::CRNG>(pt_state, "random"); 00054 /* Perform wanted action */ 00055 if(lua_gettop(pt_state) == 0) { 00056 /* Return a number between 0 and 1 */ 00057 lua_pushnumber(pt_state, pcRNG->Uniform(UNIT)); 00058 return 1; 00059 } 00060 else if(lua_gettop(pt_state) == 1) { 00061 /* Check parameter */ 00062 luaL_checktype(pt_state, 1, LUA_TNUMBER); 00063 /* Return a number between 0 and the max */ 00064 lua_pushnumber(pt_state, 00065 pcRNG->Uniform(CRange<Real>(0, 00066 lua_tonumber(pt_state, 1)))); 00067 return 1; 00068 } 00069 else { 00070 /* Check parameters */ 00071 luaL_checktype(pt_state, 1, LUA_TNUMBER); 00072 luaL_checktype(pt_state, 2, LUA_TNUMBER); 00073 /* Return a number between min and max */ 00074 lua_pushnumber(pt_state, 00075 pcRNG->Uniform(CRange<Real>(lua_tonumber(pt_state, 1), 00076 lua_tonumber(pt_state, 2)))); 00077 return 1; 00078 } 00079 /* Can't reach this point */ 00080 return 0; 00081 } 00082 00083 int LuaRNGUniformInt(lua_State* pt_state) { 00084 /* Check number of parameters */ 00085 if(lua_gettop(pt_state) > 2) { 00086 return luaL_error(pt_state, "robot.random.uniform_int() expects 1 or 2 arguments"); 00087 } 00088 /* Get RNG instance */ 00089 CRandom::CRNG* pcRNG = CLuaUtility::GetDeviceInstance<CRandom::CRNG>(pt_state, "random"); 00090 /* Perform wanted action */ 00091 if(lua_gettop(pt_state) == 1) { 00092 /* Check parameter */ 00093 luaL_checktype(pt_state, 1, LUA_TNUMBER); 00094 /* Return a number between 0 and the max */ 00095 lua_pushnumber(pt_state, 00096 pcRNG->Uniform(CRange<UInt32>(0, 00097 Floor(lua_tonumber(pt_state, 1))))); 00098 return 1; 00099 } 00100 else { 00101 /* Check parameters */ 00102 luaL_checktype(pt_state, 1, LUA_TNUMBER); 00103 luaL_checktype(pt_state, 2, LUA_TNUMBER); 00104 /* Return a number between min and max */ 00105 lua_pushnumber(pt_state, 00106 pcRNG->Uniform(CRange<SInt32>(Floor(lua_tonumber(pt_state, 1)), 00107 Floor(lua_tonumber(pt_state, 2))))); 00108 return 1; 00109 } 00110 /* Can't reach this point */ 00111 return 0; 00112 } 00113 00114 int LuaRNGExponential(lua_State* pt_state) { 00115 /* Check number of parameters */ 00116 if(lua_gettop(pt_state) != 1) { 00117 return luaL_error(pt_state, "robot.random.exponential() expects 1 argument"); 00118 } 00119 /* Get RNG instance */ 00120 CRandom::CRNG* pcRNG = CLuaUtility::GetDeviceInstance<CRandom::CRNG>(pt_state, "random"); 00121 /* Check parameter */ 00122 luaL_checktype(pt_state, 1, LUA_TNUMBER); 00123 /* Return random number */ 00124 lua_pushnumber(pt_state, 00125 pcRNG->Exponential(lua_tonumber(pt_state, 1))); 00126 return 1; 00127 } 00128 00129 int LuaRNGGaussian(lua_State* pt_state) { 00130 /* Check number of parameters */ 00131 if(lua_gettop(pt_state) != 1 && lua_gettop(pt_state) != 2) { 00132 return luaL_error(pt_state, "robot.random.gaussian() expects 1 or 2 arguments"); 00133 } 00134 /* Get RNG instance */ 00135 CRandom::CRNG* pcRNG = CLuaUtility::GetDeviceInstance<CRandom::CRNG>(pt_state, "random"); 00136 /* Perform wanted action */ 00137 if(lua_gettop(pt_state) == 1) { 00138 /* Check parameter */ 00139 luaL_checktype(pt_state, 1, LUA_TNUMBER); 00140 /* Return random number */ 00141 lua_pushnumber(pt_state, pcRNG->Gaussian(lua_tonumber(pt_state, 1))); 00142 return 1; 00143 } 00144 else { 00145 /* Check parameters */ 00146 luaL_checktype(pt_state, 1, LUA_TNUMBER); 00147 luaL_checktype(pt_state, 2, LUA_TNUMBER); 00148 /* Return random number */ 00149 lua_pushnumber(pt_state, 00150 pcRNG->Gaussian(lua_tonumber(pt_state, 1), 00151 lua_tonumber(pt_state, 2))); 00152 return 1; 00153 } 00154 /* Can't reach this point */ 00155 return 0; 00156 } 00157 00158 /****************************************/ 00159 /****************************************/ 00160 00161 bool CLuaUtility::LoadScript(lua_State* pt_state, 00162 const std::string& str_filename) { 00163 if(luaL_loadfile(pt_state, str_filename.c_str())) { 00164 return false; 00165 } 00166 if(lua_pcall(pt_state, 0, 0, 0)) { 00167 return false; 00168 } 00169 return true; 00170 } 00171 00172 /****************************************/ 00173 /****************************************/ 00174 00175 bool CLuaUtility::CallLuaFunction(lua_State* pt_state, 00176 const std::string& str_function) { 00177 lua_getglobal(pt_state, str_function.c_str()); 00178 if(lua_pcall(pt_state, 0, 0, 0)) { 00179 return false; 00180 } 00181 return true; 00182 } 00183 00184 /****************************************/ 00185 /****************************************/ 00186 00187 void PrintStackEntry(CARGoSLog& c_log, lua_State* pt_state, SInt32 n_index) { 00188 switch(lua_type(pt_state, n_index)) { 00189 case LUA_TBOOLEAN: c_log << lua_toboolean(pt_state, n_index); break; 00190 case LUA_TNUMBER: c_log << lua_tonumber(pt_state, n_index); break; 00191 case LUA_TSTRING: c_log << lua_tostring(pt_state, n_index); break; 00192 default: c_log << lua_topointer(pt_state, n_index); break; 00193 } 00194 } 00195 00196 void RecursivePrintGlobals(CARGoSLog& c_log, 00197 lua_State* pt_state, 00198 size_t un_depth) { 00199 for(size_t i = 0; i < un_depth; ++i) { 00200 c_log << " "; 00201 } 00202 PrintStackEntry(c_log, pt_state, -2); 00203 c_log << " [" << lua_typename(pt_state, lua_type(pt_state, -1)) << "] "; 00204 if(lua_istable(pt_state, -1)) { 00205 c_log << std::endl; 00206 lua_pushnil(pt_state); 00207 while(lua_next(pt_state, -2)) { 00208 if(lua_type(pt_state, -1) != LUA_TFUNCTION && 00209 (lua_type(pt_state, -2) != LUA_TSTRING || 00210 (std::string(lua_tostring(pt_state, -2)) != "_G" && 00211 std::string(lua_tostring(pt_state, -2)) != "_VERSION" && 00212 std::string(lua_tostring(pt_state, -2)) != "package" && 00213 std::string(lua_tostring(pt_state, -2)) != "string" && 00214 std::string(lua_tostring(pt_state, -2)) != "os" && 00215 std::string(lua_tostring(pt_state, -2)) != "io" && 00216 std::string(lua_tostring(pt_state, -2)) != "math" && 00217 std::string(lua_tostring(pt_state, -2)) != "debug" && 00218 std::string(lua_tostring(pt_state, -2)) != "coroutine" && 00219 std::string(lua_tostring(pt_state, -2)) != "table"))) { 00220 RecursivePrintGlobals(c_log, pt_state, un_depth+1); 00221 } 00222 lua_pop(pt_state, 1); 00223 } 00224 } 00225 else { 00226 PrintStackEntry(c_log, pt_state, -1); 00227 c_log << std::endl; 00228 } 00229 } 00230 00231 void CLuaUtility::PrintGlobals(CARGoSLog& c_log, 00232 lua_State* pt_state) { 00233 c_log << "*** LUA GLOBALS START ***" << std::endl; 00234 lua_getglobal(pt_state, "_G"); 00235 RecursivePrintGlobals(c_log, pt_state, 0); 00236 lua_pop(pt_state, 1); 00237 c_log << "*** LUA GLOBALS END ***" << std::endl; 00238 #ifdef ARGOS_THREADSAFE_LOG 00239 c_log.Flush(); 00240 #endif 00241 } 00242 00243 /****************************************/ 00244 /****************************************/ 00245 00246 void CLuaUtility::PrintStack(CARGoSLog& c_log, 00247 lua_State* pt_state) { 00248 c_log << "*** LUA STACK START ***" << std::endl; 00249 size_t unTop = lua_gettop(pt_state); 00250 c_log << "Elements in stack: " << unTop << std::endl; 00251 for(size_t i = unTop; i > 0; --i) { 00252 c_log << "#" << i << " [" << lua_typename(pt_state, lua_type(pt_state, i)) << "] "; 00253 PrintStackEntry(c_log, pt_state, i); 00254 c_log << std::endl; 00255 } 00256 c_log << "*** LUA STACK END ***" << std::endl; 00257 #ifdef ARGOS_THREADSAFE_LOG 00258 c_log.Flush(); 00259 #endif 00260 } 00261 00262 /****************************************/ 00263 /****************************************/ 00264 00265 void CLuaUtility::RegisterLoggerWrapper(lua_State* pt_state) { 00266 lua_register(pt_state, "log", LOGWrapper); 00267 lua_register(pt_state, "logerr", LOGERRWrapper); 00268 } 00269 00270 /****************************************/ 00271 /****************************************/ 00272 00273 void CLuaUtility::RegisterRNG(lua_State* pt_state, 00274 CRandom::CRNG* pc_rng) { 00275 pc_rng->Reset(); 00276 OpenRobotStateTable(pt_state, "random"); 00277 AddToTable(pt_state, "_instance", pc_rng); 00278 AddToTable(pt_state, "bernoulli", &LuaRNGBernoulli); 00279 AddToTable(pt_state, "uniform", &LuaRNGUniform); 00280 AddToTable(pt_state, "uniform_int", &LuaRNGUniformInt); 00281 AddToTable(pt_state, "exponential", &LuaRNGExponential); 00282 AddToTable(pt_state, "gaussian", &LuaRNGGaussian); 00283 CloseRobotStateTable(pt_state); 00284 } 00285 00286 /****************************************/ 00287 /****************************************/ 00288 00289 void CLuaUtility::OpenRobotStateTable(lua_State* pt_state, 00290 const std::string& str_key) { 00291 lua_pushstring(pt_state, str_key.c_str()); 00292 lua_rawget(pt_state, -2); 00293 if(lua_isnil(pt_state, -1)) { 00294 lua_pop(pt_state, 1); 00295 StartTable(pt_state, str_key); 00296 EndTable(pt_state); 00297 lua_pushstring(pt_state, str_key.c_str()); 00298 lua_rawget(pt_state, -2); 00299 } 00300 } 00301 00302 /****************************************/ 00303 /****************************************/ 00304 00305 void CLuaUtility::CloseRobotStateTable(lua_State* pt_state) { 00306 lua_pop(pt_state, 1); 00307 } 00308 00309 /****************************************/ 00310 /****************************************/ 00311 00312 void CLuaUtility::StartTable(lua_State* pt_state, 00313 const std::string& str_key) { 00314 lua_pushstring(pt_state, str_key.c_str()); 00315 lua_newtable (pt_state); 00316 } 00317 00318 /****************************************/ 00319 /****************************************/ 00320 00321 void CLuaUtility::StartTable(lua_State* pt_state, 00322 int n_key) { 00323 lua_pushnumber(pt_state, n_key); 00324 lua_newtable (pt_state); 00325 } 00326 00327 /****************************************/ 00328 /****************************************/ 00329 00330 void CLuaUtility::EndTable(lua_State* pt_state) { 00331 lua_settable(pt_state, -3); 00332 } 00333 00334 /****************************************/ 00335 /****************************************/ 00336 00337 void CLuaUtility::AddToTable(lua_State* pt_state, 00338 const std::string& str_key, 00339 void* pt_data) { 00340 lua_pushstring (pt_state, str_key.c_str()); 00341 lua_pushlightuserdata(pt_state, pt_data ); 00342 lua_settable (pt_state, -3 ); 00343 } 00344 00345 /****************************************/ 00346 /****************************************/ 00347 00348 void CLuaUtility::AddToTable(lua_State* pt_state, 00349 const std::string& str_key, 00350 lua_CFunction t_data) { 00351 lua_pushstring (pt_state, str_key.c_str()); 00352 lua_pushcfunction(pt_state, t_data ); 00353 lua_settable (pt_state, -3 ); 00354 } 00355 00356 /****************************************/ 00357 /****************************************/ 00358 00359 void CLuaUtility::AddToTable(lua_State* pt_state, 00360 const std::string& str_key, 00361 Real f_data) { 00362 lua_pushstring(pt_state, str_key.c_str()); 00363 lua_pushnumber(pt_state, f_data ); 00364 lua_settable (pt_state, -3 ); 00365 } 00366 00367 /****************************************/ 00368 /****************************************/ 00369 00370 void CLuaUtility::AddToTable(lua_State* pt_state, 00371 int n_key, 00372 Real f_data) { 00373 lua_pushnumber(pt_state, n_key ); 00374 lua_pushnumber(pt_state, f_data); 00375 lua_settable (pt_state, -3 ); 00376 } 00377 00378 /****************************************/ 00379 /****************************************/ 00380 00381 void CLuaUtility::AddToTable(lua_State* pt_state, 00382 const std::string& str_key, 00383 const CRadians& c_data) { 00384 lua_pushstring(pt_state, str_key.c_str() ); 00385 lua_pushnumber(pt_state, c_data.GetValue()); 00386 lua_settable (pt_state, -3 ); 00387 } 00388 00389 /****************************************/ 00390 /****************************************/ 00391 00392 void CLuaUtility::AddToTable(lua_State* pt_state, 00393 int n_key, 00394 const CRadians& c_data) { 00395 lua_pushnumber(pt_state, n_key ); 00396 lua_pushnumber(pt_state, c_data.GetValue()); 00397 lua_settable (pt_state, -3 ); 00398 } 00399 00400 /****************************************/ 00401 /****************************************/ 00402 00403 void CLuaUtility::AddToTable(lua_State* pt_state, 00404 const std::string& str_key, 00405 const CVector2& c_data) { 00406 StartTable(pt_state, str_key ); 00407 AddToTable(pt_state, "_type", TYPE_VECTOR2 ); 00408 AddToTable(pt_state, "x", c_data.GetX()); 00409 AddToTable(pt_state, "y", c_data.GetY()); 00410 EndTable (pt_state ); 00411 } 00412 00413 /****************************************/ 00414 /****************************************/ 00415 00416 void CLuaUtility::AddToTable(lua_State* pt_state, 00417 int n_key, 00418 const CVector2& c_data) { 00419 StartTable(pt_state, n_key ); 00420 AddToTable(pt_state, "_type", TYPE_VECTOR2 ); 00421 AddToTable(pt_state, "x", c_data.GetX()); 00422 AddToTable(pt_state, "y", c_data.GetY()); 00423 EndTable (pt_state ); 00424 } 00425 00426 /****************************************/ 00427 /****************************************/ 00428 00429 void CLuaUtility::AddToTable(lua_State* pt_state, 00430 const std::string& str_key, 00431 const CVector3& c_data) { 00432 StartTable(pt_state, str_key ); 00433 AddToTable(pt_state, "_type", TYPE_VECTOR3 ); 00434 AddToTable(pt_state, "x", c_data.GetX()); 00435 AddToTable(pt_state, "y", c_data.GetY()); 00436 AddToTable(pt_state, "z", c_data.GetZ()); 00437 EndTable (pt_state ); 00438 } 00439 00440 /****************************************/ 00441 /****************************************/ 00442 00443 void CLuaUtility::AddToTable(lua_State* pt_state, 00444 int n_key, 00445 const CVector3& c_data) { 00446 StartTable(pt_state, n_key ); 00447 AddToTable(pt_state, "_type", TYPE_VECTOR3 ); 00448 AddToTable(pt_state, "x", c_data.GetX()); 00449 AddToTable(pt_state, "y", c_data.GetY()); 00450 AddToTable(pt_state, "z", c_data.GetZ()); 00451 EndTable (pt_state ); 00452 } 00453 00454 /****************************************/ 00455 /****************************************/ 00456 00457 void CLuaUtility::AddToTable(lua_State* pt_state, 00458 const std::string& str_key, 00459 const CQuaternion& c_data) { 00460 CRadians cAngle; 00461 CVector3 cAxis; 00462 c_data.ToAngleAxis(cAngle, cAxis); 00463 StartTable(pt_state, str_key ); 00464 AddToTable(pt_state, "_type", TYPE_QUATERNION); 00465 AddToTable(pt_state, "angle", cAngle ); 00466 AddToTable(pt_state, "axis", cAxis ); 00467 EndTable (pt_state ); 00468 } 00469 00470 /****************************************/ 00471 /****************************************/ 00472 00473 void CLuaUtility::AddToTable(lua_State* pt_state, 00474 int n_key, 00475 const CQuaternion& c_data) { 00476 CRadians cAngle; 00477 CVector3 cAxis; 00478 c_data.ToAngleAxis(cAngle, cAxis); 00479 StartTable(pt_state, n_key ); 00480 AddToTable(pt_state, "_type", TYPE_QUATERNION); 00481 AddToTable(pt_state, "angle", cAngle ); 00482 AddToTable(pt_state, "axis", cAxis ); 00483 EndTable (pt_state ); 00484 } 00485 00486 /****************************************/ 00487 /****************************************/ 00488 00489 void CLuaUtility::AddToTable(lua_State* pt_state, 00490 const std::string& str_key, 00491 const CColor& c_data) { 00492 StartTable(pt_state, str_key ); 00493 AddToTable(pt_state, "_type", TYPE_COLOR ); 00494 AddToTable(pt_state, "red", c_data.GetRed() ); 00495 AddToTable(pt_state, "green", c_data.GetGreen()); 00496 AddToTable(pt_state, "blue", c_data.GetBlue() ); 00497 EndTable (pt_state ); 00498 } 00499 00500 /****************************************/ 00501 /****************************************/ 00502 00503 void CLuaUtility::AddToTable(lua_State* pt_state, 00504 int n_key, 00505 const CColor& c_data) { 00506 StartTable(pt_state, n_key ); 00507 AddToTable(pt_state, "_type", TYPE_COLOR ); 00508 AddToTable(pt_state, "red", c_data.GetRed() ); 00509 AddToTable(pt_state, "green", c_data.GetGreen()); 00510 AddToTable(pt_state, "blue", c_data.GetBlue() ); 00511 EndTable (pt_state ); 00512 } 00513 00514 /****************************************/ 00515 /****************************************/ 00516 00517 int CLuaUtility::LOGWrapper(lua_State* pt_state) { 00518 return LoggerWrapper(LOG, pt_state); 00519 } 00520 00521 /****************************************/ 00522 /****************************************/ 00523 00524 int CLuaUtility::LOGERRWrapper(lua_State* pt_state) { 00525 return LoggerWrapper(LOGERR, pt_state); 00526 } 00527 00528 /****************************************/ 00529 /****************************************/ 00530 00531 int CLuaUtility::LoggerWrapper(CARGoSLog& c_log, 00532 lua_State* pt_state) { 00533 /* Get number of arguments */ 00534 UInt32 unArgc = lua_gettop(pt_state); 00535 /* Send arguments to log one by one */ 00536 UInt32 unType; 00537 for(UInt32 i = 1; i <= unArgc; ++i) { 00538 unType = lua_type(pt_state, i); 00539 switch(unType) { 00540 case LUA_TBOOLEAN: c_log << lua_toboolean(pt_state, i); break; 00541 case LUA_TNUMBER: c_log << lua_tonumber (pt_state, i); break; 00542 case LUA_TSTRING: c_log << lua_tostring (pt_state, i); break; 00543 default: c_log << lua_typename (pt_state, unType); break; 00544 } 00545 } 00546 c_log << std::endl; 00547 /* No result is calculated */ 00548 return 0; 00549 } 00550 00551 /****************************************/ 00552 /****************************************/ 00553 00554 }