ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "ground_sensor_equipped_entity.h" 00008 #include <argos3/core/simulator/space/space.h> 00009 00010 namespace argos { 00011 00012 /****************************************/ 00013 /****************************************/ 00014 00015 CGroundSensorEquippedEntity::CGroundSensorEquippedEntity(CComposableEntity* pc_parent) : 00016 CEntity(pc_parent) { 00017 SetCanBeEnabledIfDisabled(false); 00018 } 00019 00020 /****************************************/ 00021 /****************************************/ 00022 00023 CGroundSensorEquippedEntity::CGroundSensorEquippedEntity(CComposableEntity* pc_parent, 00024 const std::string& str_id) : 00025 CEntity(pc_parent, str_id) { 00026 SetCanBeEnabledIfDisabled(false); 00027 } 00028 00029 /****************************************/ 00030 /****************************************/ 00031 00032 CGroundSensorEquippedEntity::~CGroundSensorEquippedEntity() { 00033 while(! m_tSensors.empty()) { 00034 delete m_tSensors.back(); 00035 m_tSensors.pop_back(); 00036 } 00037 } 00038 00039 /****************************************/ 00040 /****************************************/ 00041 00042 void CGroundSensorEquippedEntity::Init(TConfigurationNode& t_tree) { 00043 try { 00044 /* 00045 * Parse basic entity stuff 00046 */ 00047 CEntity::Init(t_tree); 00048 /* 00049 * Parse ground sensors 00050 */ 00051 /* Not adding any sensor is a fatal error */ 00052 if(t_tree.NoChildren()) { 00053 THROW_ARGOSEXCEPTION("No sensors defined"); 00054 } 00055 /* Go through children */ 00056 TConfigurationNodeIterator it; 00057 for(it = it.begin(&t_tree); it != it.end(); ++it) { 00058 if(it->Value() == "sensor") { 00059 CVector2 cOffset; 00060 GetNodeAttribute(*it, "offset", cOffset); 00061 std::string strType; 00062 GetNodeAttribute(*it, "type", strType); 00063 AddSensor(cOffset, ParseType(strType)); 00064 } 00065 else if(it->Value() == "ring") { 00066 CVector2 cRingCenter; 00067 GetNodeAttributeOrDefault(t_tree, "center", cRingCenter, cRingCenter); 00068 Real fRadius; 00069 GetNodeAttribute(t_tree, "radius", fRadius); 00070 CDegrees cRingStartAngleDegrees; 00071 GetNodeAttributeOrDefault(t_tree, "start_angle", cRingStartAngleDegrees, cRingStartAngleDegrees); 00072 CRadians cRingStartAngleRadians = ToRadians(cRingStartAngleDegrees); 00073 std::string strType; 00074 GetNodeAttribute(*it, "type", strType); 00075 ESensorType eType = ParseType(strType); 00076 UInt32 unNumSensors; 00077 GetNodeAttribute(t_tree, "num_sensors", unNumSensors); 00078 AddSensorRing(cRingCenter, 00079 fRadius, 00080 cRingStartAngleRadians, 00081 eType, 00082 unNumSensors); 00083 } 00084 else { 00085 THROW_ARGOSEXCEPTION("Unrecognized tag \"" << it->Value() << "\""); 00086 } 00087 } 00088 } 00089 catch(CARGoSException& ex) { 00090 THROW_ARGOSEXCEPTION_NESTED("Initialization error in ground sensor equipped entity", ex); 00091 } 00092 } 00093 00094 /****************************************/ 00095 /****************************************/ 00096 00097 void CGroundSensorEquippedEntity::AddSensor(const CVector2& c_offset, 00098 ESensorType e_type) { 00099 m_tSensors.push_back(new SSensor(c_offset, e_type)); 00100 } 00101 00102 /****************************************/ 00103 /****************************************/ 00104 00105 void CGroundSensorEquippedEntity::AddSensorRing(const CVector2& c_center, 00106 Real f_radius, 00107 const CRadians& c_start_angle, 00108 ESensorType e_type, 00109 UInt32 un_num_sensors) { 00110 CRadians cSensorSpacing = CRadians::TWO_PI / un_num_sensors; 00111 CRadians cAngle; 00112 CVector2 cOffset; 00113 for(UInt32 i = 0; i < un_num_sensors; ++i) { 00114 cAngle = c_start_angle + i * cSensorSpacing; 00115 cAngle.SignedNormalize(); 00116 cOffset.Set(f_radius, 0.0f); 00117 cOffset.Rotate(cAngle); 00118 cOffset += c_center; 00119 AddSensor(cOffset, e_type); 00120 } 00121 } 00122 00123 /****************************************/ 00124 /****************************************/ 00125 00126 CGroundSensorEquippedEntity::ESensorType CGroundSensorEquippedEntity::ParseType(const std::string& str_type) const { 00127 if(str_type == "bw") return TYPE_BLACK_WHITE; 00128 if(str_type == "gray") return TYPE_GRAYSCALE; 00129 THROW_ARGOSEXCEPTION("Unrecognized ground sensor type \"" << str_type << "\""); 00130 } 00131 00132 /****************************************/ 00133 /****************************************/ 00134 00135 REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CGroundSensorEquippedEntity); 00136 00137 /****************************************/ 00138 /****************************************/ 00139 00140 }