ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "light_sensor_equipped_entity.h" 00008 #include <argos3/core/simulator/space/space.h> 00009 00010 namespace argos { 00011 00012 /****************************************/ 00013 /****************************************/ 00014 00015 CLightSensorEquippedEntity::CLightSensorEquippedEntity(CComposableEntity* pc_parent) : 00016 CEntity(pc_parent) { 00017 SetCanBeEnabledIfDisabled(false); 00018 } 00019 00020 /****************************************/ 00021 /****************************************/ 00022 00023 CLightSensorEquippedEntity::CLightSensorEquippedEntity(CComposableEntity* pc_parent, 00024 const std::string& str_id) : 00025 CEntity(pc_parent, str_id) { 00026 SetCanBeEnabledIfDisabled(false); 00027 } 00028 00029 /****************************************/ 00030 /****************************************/ 00031 00032 CLightSensorEquippedEntity::~CLightSensorEquippedEntity() { 00033 while(! m_tSensors.empty()) { 00034 delete m_tSensors.back(); 00035 m_tSensors.pop_back(); 00036 } 00037 } 00038 00039 /****************************************/ 00040 /****************************************/ 00041 00042 void CLightSensorEquippedEntity::Init(TConfigurationNode& t_tree) { 00043 try { 00044 /* 00045 * Parse basic entity stuff 00046 */ 00047 CEntity::Init(t_tree); 00048 /* 00049 * Parse light 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 CVector3 cPos, cDir; 00060 Real fRange; 00061 GetNodeAttribute(*it, "position", cPos); 00062 GetNodeAttribute(*it, "direction", cDir); 00063 GetNodeAttribute(*it, "range", fRange); 00064 AddSensor(cPos, cDir, fRange); 00065 } 00066 else if(it->Value() == "ring") { 00067 CVector3 cRingCenter; 00068 GetNodeAttributeOrDefault(t_tree, "center", cRingCenter, cRingCenter); 00069 Real fRadius; 00070 GetNodeAttribute(t_tree, "radius", fRadius); 00071 CDegrees cRingStartAngleDegrees; 00072 GetNodeAttributeOrDefault(t_tree, "start_angle", cRingStartAngleDegrees, cRingStartAngleDegrees); 00073 CRadians cRingStartAngleRadians = ToRadians(cRingStartAngleDegrees); 00074 Real fRange; 00075 GetNodeAttribute(t_tree, "range", fRange); 00076 UInt32 unNumSensors; 00077 GetNodeAttribute(t_tree, "num_sensors", unNumSensors); 00078 AddSensorRing(cRingCenter, 00079 fRadius, 00080 cRingStartAngleRadians, 00081 fRange, 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 light sensor equipped entity", ex); 00091 } 00092 } 00093 00094 /****************************************/ 00095 /****************************************/ 00096 00097 void CLightSensorEquippedEntity::AddSensor(const CVector3& c_position, 00098 const CVector3& c_direction, 00099 Real f_range) { 00100 m_tSensors.push_back(new SSensor(c_position, c_direction, f_range)); 00101 } 00102 00103 /****************************************/ 00104 /****************************************/ 00105 00106 void CLightSensorEquippedEntity::AddSensorRing(const CVector3& c_center, 00107 Real f_radius, 00108 const CRadians& c_start_angle, 00109 Real f_range, 00110 UInt32 un_num_sensors) { 00111 CRadians cSensorSpacing = CRadians::TWO_PI / un_num_sensors; 00112 CRadians cAngle; 00113 CVector3 cPos, cDir; 00114 for(UInt32 i = 0; i < un_num_sensors; ++i) { 00115 cAngle = c_start_angle + i * cSensorSpacing; 00116 cAngle.SignedNormalize(); 00117 cPos.Set(f_radius, 0.0f, 0.0f); 00118 cPos.RotateZ(cAngle); 00119 cPos += c_center; 00120 cDir.Set(f_range, 0.0f, 0.0f); 00121 cDir.RotateZ(cAngle); 00122 AddSensor(cPos, cDir, f_range); 00123 } 00124 } 00125 00126 /****************************************/ 00127 /****************************************/ 00128 00129 REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CLightSensorEquippedEntity); 00130 00131 /****************************************/ 00132 /****************************************/ 00133 00134 }