ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "lua_controller.h" 00008 #include <argos3/core/utility/logging/argos_log.h> 00009 00010 #include <argos3/core/wrappers/lua/lua_utility.h> 00011 00012 namespace argos { 00013 00014 /****************************************/ 00015 /****************************************/ 00016 00017 CLuaController::CLuaController() : 00018 m_ptLuaState(NULL), 00019 m_bScriptActive(false), 00020 m_bIsOK(true), 00021 m_pcRNG(NULL) { 00022 } 00023 00024 /****************************************/ 00025 /****************************************/ 00026 00027 CLuaController::~CLuaController() { 00028 } 00029 00030 /****************************************/ 00031 /****************************************/ 00032 00033 void CLuaController::Init(TConfigurationNode& t_tree) { 00034 try { 00035 /* Create RNG */ 00036 m_pcRNG = CRandom::CreateRNG("argos"); 00037 /* Load script */ 00038 std::string strScriptFileName; 00039 GetNodeAttributeOrDefault(t_tree, "script", strScriptFileName, strScriptFileName); 00040 if(strScriptFileName != "") { 00041 SetLuaScript(strScriptFileName); 00042 if(! m_bIsOK) { 00043 THROW_ARGOSEXCEPTION("Error loading Lua script \"" << strScriptFileName << "\": " << lua_tostring(m_ptLuaState, -1)); 00044 } 00045 } 00046 else { 00047 /* Create a new Lua stack */ 00048 m_ptLuaState = luaL_newstate(); 00049 /* Load the Lua libraries */ 00050 luaL_openlibs(m_ptLuaState); 00051 /* Create and set Lua state */ 00052 CreateLuaState(); 00053 SensorReadingsToLuaState(); 00054 } 00055 } 00056 catch(CARGoSException& ex) { 00057 THROW_ARGOSEXCEPTION_NESTED("Error initializing Lua controller", ex); 00058 } 00059 } 00060 00061 /****************************************/ 00062 /****************************************/ 00063 00064 void CLuaController::ControlStep() { 00065 if(m_bScriptActive && m_bIsOK) { 00066 /* Update Lua state through sensor readings */ 00067 SensorReadingsToLuaState(); 00068 /* Execute script step function */ 00069 if(! CLuaUtility::CallLuaFunction(m_ptLuaState, "step")) { 00070 m_bIsOK = false; 00071 } 00072 } 00073 } 00074 00075 /****************************************/ 00076 /****************************************/ 00077 00078 void CLuaController::Reset() { 00079 if(m_bScriptActive) { 00080 if(m_bIsOK) { 00081 m_bIsOK = CLuaUtility::CallLuaFunction(m_ptLuaState, "reset"); 00082 } 00083 else { 00084 SetLuaScript(m_strScriptFileName); 00085 } 00086 } 00087 } 00088 00089 /****************************************/ 00090 /****************************************/ 00091 00092 void CLuaController::Destroy() { 00093 if(m_bScriptActive && m_bIsOK) { 00094 /* Execute script destroy function */ 00095 CLuaUtility::CallLuaFunction(m_ptLuaState, "destroy"); 00096 } 00097 /* Close Lua */ 00098 lua_close(m_ptLuaState); 00099 } 00100 00101 /****************************************/ 00102 /****************************************/ 00103 00104 void CLuaController::SetLuaScript(const std::string& str_script) { 00105 /* First, delete old script */ 00106 if(m_bScriptActive) { 00107 lua_close(m_ptLuaState); 00108 m_bScriptActive = false; 00109 m_strScriptFileName = ""; 00110 } 00111 /* Create a new Lua stack */ 00112 m_ptLuaState = luaL_newstate(); 00113 /* Load the Lua libraries */ 00114 luaL_openlibs(m_ptLuaState); 00115 /* Load script */ 00116 if(!CLuaUtility::LoadScript(m_ptLuaState, str_script)) { 00117 m_bIsOK = false; 00118 return; 00119 } 00120 m_strScriptFileName = str_script; 00121 /* Create and set variables */ 00122 CreateLuaState(); 00123 SensorReadingsToLuaState(); 00124 /* Execute script init function */ 00125 if(!CLuaUtility::CallLuaFunction(m_ptLuaState, "init")) { 00126 m_bIsOK = false; 00127 return; 00128 } 00129 m_bIsOK = true; 00130 m_bScriptActive = true; 00131 } 00132 00133 /****************************************/ 00134 /****************************************/ 00135 00136 void CLuaController::CreateLuaState() { 00137 /* Register functions */ 00138 CLuaUtility::RegisterLoggerWrapper(m_ptLuaState); 00139 /* Create a table that will contain the state of the robot */ 00140 lua_newtable(m_ptLuaState); 00141 /* Set the id of the robot */ 00142 lua_pushstring(m_ptLuaState, "id"); 00143 lua_pushstring(m_ptLuaState, GetId().c_str()); 00144 lua_settable(m_ptLuaState, -3); 00145 /* Register RNG */ 00146 CLuaUtility::RegisterRNG(m_ptLuaState, m_pcRNG); 00147 /* Go through devices and add the necessary items to the table */ 00148 for(CCI_Actuator::TMap::iterator it = m_mapActuators.begin(); 00149 it != m_mapActuators.end(); 00150 ++it) { 00151 it->second->CreateLuaState(m_ptLuaState); 00152 } 00153 for(CCI_Sensor::TMap::iterator it = m_mapSensors.begin(); 00154 it != m_mapSensors.end(); 00155 ++it) { 00156 it->second->CreateLuaState(m_ptLuaState); 00157 } 00158 /* Set the name of the table */ 00159 lua_setglobal(m_ptLuaState, "robot"); 00160 } 00161 00162 /****************************************/ 00163 /****************************************/ 00164 00165 void CLuaController::SensorReadingsToLuaState() { 00166 /* Put the robot state table on top */ 00167 lua_getglobal(m_ptLuaState, "robot"); 00168 /* Go through the sensors */ 00169 for(CCI_Sensor::TMap::iterator it = m_mapSensors.begin(); 00170 it != m_mapSensors.end(); 00171 ++it) { 00172 it->second->ReadingsToLuaState(m_ptLuaState); 00173 } 00174 /* Pop the robot state table */ 00175 lua_pop(m_ptLuaState, 1); 00176 } 00177 00178 /****************************************/ 00179 /****************************************/ 00180 00181 std::string CLuaController::GetErrorMessage() { 00182 if(m_bIsOK) { 00183 return "OK"; 00184 } 00185 else { 00186 return lua_tostring(m_ptLuaState, -1); 00187 } 00188 } 00189 00190 /****************************************/ 00191 /****************************************/ 00192 00193 REGISTER_CONTROLLER(CLuaController, "lua_controller"); 00194 00195 }