ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "ci_footbot_proximity_sensor.h" 00008 #include <argos3/core/utility/math/angles.h> 00009 00010 #ifdef ARGOS_WITH_LUA 00011 #include <argos3/core/wrappers/lua/lua_utility.h> 00012 #endif 00013 00014 namespace argos { 00015 00016 /****************************************/ 00017 /****************************************/ 00018 00019 static CRadians SPACING = CRadians(ARGOS_PI / 12.0f); 00020 static CRadians START_ANGLE = SPACING * 0.5f; 00021 00022 /****************************************/ 00023 /****************************************/ 00024 00025 CCI_FootBotProximitySensor::CCI_FootBotProximitySensor() : 00026 m_tReadings(24) { 00027 for(size_t i = 0; i < 24; ++i) { 00028 m_tReadings[i].Angle = START_ANGLE + i * SPACING; 00029 m_tReadings[i].Angle.SignedNormalize(); 00030 } 00031 } 00032 00033 /****************************************/ 00034 /****************************************/ 00035 00036 #ifdef ARGOS_WITH_LUA 00037 void CCI_FootBotProximitySensor::CreateLuaState(lua_State* pt_lua_state) { 00038 CLuaUtility::OpenRobotStateTable(pt_lua_state, "proximity"); 00039 for(size_t i = 0; i < GetReadings().size(); ++i) { 00040 CLuaUtility::StartTable(pt_lua_state, i+1 ); 00041 CLuaUtility::AddToTable(pt_lua_state, "angle", m_tReadings[i].Angle); 00042 CLuaUtility::AddToTable(pt_lua_state, "value", m_tReadings[i].Value); 00043 CLuaUtility::EndTable (pt_lua_state ); 00044 } 00045 CLuaUtility::CloseRobotStateTable(pt_lua_state); 00046 } 00047 #endif 00048 00049 /****************************************/ 00050 /****************************************/ 00051 00052 #ifdef ARGOS_WITH_LUA 00053 void CCI_FootBotProximitySensor::ReadingsToLuaState(lua_State* pt_lua_state) { 00054 lua_getfield(pt_lua_state, -1, "proximity"); 00055 for(size_t i = 0; i < GetReadings().size(); ++i) { 00056 lua_pushnumber(pt_lua_state, i+1 ); 00057 lua_gettable (pt_lua_state, -2 ); 00058 lua_pushnumber(pt_lua_state, m_tReadings[i].Value); 00059 lua_setfield (pt_lua_state, -2, "value" ); 00060 lua_pop(pt_lua_state, 1); 00061 } 00062 lua_pop(pt_lua_state, 1); 00063 } 00064 #endif 00065 00066 00067 /****************************************/ 00068 /****************************************/ 00069 00070 std::ostream& operator<<(std::ostream& c_os, 00071 const CCI_FootBotProximitySensor::SReading& s_reading) { 00072 c_os << "Value=<" << s_reading.Value 00073 << ">, Angle=<" << s_reading.Angle << ">"; 00074 return c_os; 00075 } 00076 00077 /****************************************/ 00078 /****************************************/ 00079 00080 std::ostream& operator<<(std::ostream& c_os, 00081 const CCI_FootBotProximitySensor::TReadings& t_readings) { 00082 if(! t_readings.empty()) { 00083 c_os << "{ " << t_readings[0].Value << " }"; 00084 for(UInt32 i = 1; i < t_readings.size(); ++i) { 00085 c_os << " { " << t_readings[0].Value << " }"; 00086 } 00087 c_os << std::endl; 00088 } 00089 return c_os; 00090 } 00091 00092 /****************************************/ 00093 /****************************************/ 00094 00095 }