ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "ci_range_and_bearing_actuator.h" 00008 00009 #ifdef ARGOS_WITH_LUA 00010 #include <argos3/core/wrappers/lua/lua_utility.h> 00011 #endif 00012 00013 namespace argos { 00014 00015 /****************************************/ 00016 /****************************************/ 00017 00018 #ifdef ARGOS_WITH_LUA 00019 /* 00020 * The stack can have one or two values 00021 * - The case of one value is when you set an entire array 00022 * - In the case of two values, you must have: 00023 * 1. the idx of the data item to set 00024 * 2. the value of the data item in the range [0,255] 00025 */ 00026 int LuaRABSetData(lua_State* pt_lua_state) { 00027 if(lua_gettop(pt_lua_state) == 1) { 00028 /* Check parameters */ 00029 luaL_checktype(pt_lua_state, 1, LUA_TTABLE); 00030 /* Get reference to actuator */ 00031 CCI_RangeAndBearingActuator* pcAct = CLuaUtility::GetDeviceInstance<CCI_RangeAndBearingActuator>(pt_lua_state, "range_and_bearing"); 00032 /* Check whether sizes match */ 00033 if(pcAct->GetSize() != lua_objlen(pt_lua_state, -1)) { 00034 return luaL_error(pt_lua_state, "robot.range_and_bearing.set_data(array) expects an array of %d numbers", pcAct->GetSize()); 00035 } 00036 /* Fill up a byte array, checking that all elements are numbers */ 00037 CByteArray cBuf(pcAct->GetSize()); 00038 for(size_t i = 0; i < pcAct->GetSize(); ++i) { 00039 lua_pushnumber(pt_lua_state, i+1); 00040 lua_gettable(pt_lua_state, -2); 00041 if(lua_type(pt_lua_state, -1) == LUA_TNUMBER) { 00042 cBuf[i] = static_cast<UInt8>(lua_tonumber(pt_lua_state, -1)); 00043 lua_pop(pt_lua_state, 1); 00044 } 00045 else { 00046 return luaL_error(pt_lua_state, "element #%d of the array is not a number", i+1); 00047 } 00048 } 00049 /* Perform action */ 00050 pcAct->SetData(cBuf); 00051 return 0; 00052 } 00053 else if(lua_gettop(pt_lua_state) == 2) { 00054 /* Check parameters */ 00055 luaL_checktype(pt_lua_state, 1, LUA_TNUMBER); 00056 luaL_checktype(pt_lua_state, 2, LUA_TNUMBER); 00057 /* Get reference to actuator */ 00058 CCI_RangeAndBearingActuator* pcAct = CLuaUtility::GetDeviceInstance<CCI_RangeAndBearingActuator>(pt_lua_state, "range_and_bearing"); 00059 /* Check parameter values */ 00060 size_t unIdx = lua_tonumber(pt_lua_state, 1); 00061 UInt8 unData = lua_tonumber(pt_lua_state, 2); 00062 if(unIdx < 1 || unIdx > pcAct->GetSize()) { 00063 return luaL_error(pt_lua_state, "passed index %d out of bounds [1,%d]", unIdx, pcAct->GetSize()); 00064 } 00065 /* Perform action */ 00066 pcAct->SetData(unIdx-1, unData); 00067 return 0; 00068 } 00069 else { 00070 return luaL_error(pt_lua_state, "robot.range_and_bearing.set_data() expects either one or two arguments"); 00071 } 00072 } 00073 #endif 00074 00075 #ifdef ARGOS_WITH_LUA 00076 /* 00077 * The stack must have no values 00078 */ 00079 int LuaRABClearData(lua_State* pt_lua_state) { 00080 /* Check parameters */ 00081 if(lua_gettop(pt_lua_state) != 0) { 00082 return luaL_error(pt_lua_state, "robot.range_and_bearing.clear_data() expects no arguments"); 00083 } 00084 /* Perform action */ 00085 CLuaUtility::GetDeviceInstance<CCI_RangeAndBearingActuator>(pt_lua_state, "range_and_bearing")->ClearData(); 00086 return 0; 00087 } 00088 #endif 00089 00090 /****************************************/ 00091 /****************************************/ 00092 00093 #ifdef ARGOS_WITH_LUA 00094 void CCI_RangeAndBearingActuator::CreateLuaState(lua_State* pt_lua_state) { 00095 CLuaUtility::StartTable(pt_lua_state, "range_and_bearing"); 00096 CLuaUtility::AddToTable(pt_lua_state, "_instance", this); 00097 CLuaUtility::AddToTable(pt_lua_state, "set_data", &LuaRABSetData); 00098 CLuaUtility::AddToTable(pt_lua_state, "clear_data", &LuaRABClearData); 00099 CLuaUtility::EndTable(pt_lua_state); 00100 } 00101 #endif 00102 00103 /****************************************/ 00104 /****************************************/ 00105 00106 void CCI_RangeAndBearingActuator::SetData(const CByteArray& c_data) { 00107 if(m_cData.Size() == c_data.Size()) { 00108 m_cData = c_data; 00109 } 00110 else { 00111 THROW_ARGOSEXCEPTION("CCI_RangeAndBearingActuator::SetData() : data size does not match, expected " << m_cData.Size() << ", got " << c_data.Size()); 00112 } 00113 } 00114 00115 /****************************************/ 00116 /****************************************/ 00117 00118 }