ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/robots/generic/control_interface/ci_range_and_bearing_actuator.cpp
Go to the documentation of this file.
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 }