ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "led_equipped_entity.h" 00008 #include <argos3/core/simulator/simulator.h> 00009 #include <argos3/core/simulator/space/space.h> 00010 #include <argos3/plugins/simulator/media/led_medium.h> 00011 00012 namespace argos { 00013 00014 /****************************************/ 00015 /****************************************/ 00016 00017 CLEDEquippedEntity::CLEDEquippedEntity(CComposableEntity* pc_parent, 00018 CPositionalEntity* pc_reference) : 00019 CComposableEntity(pc_parent), 00020 m_pcReferenceEntity(pc_reference) { 00021 } 00022 00023 /****************************************/ 00024 /****************************************/ 00025 00026 CLEDEquippedEntity::CLEDEquippedEntity(CComposableEntity* pc_parent, 00027 const std::string& str_id, 00028 CPositionalEntity* pc_reference) : 00029 CComposableEntity(pc_parent, str_id), 00030 m_pcReferenceEntity(pc_reference) { 00031 } 00032 00033 /****************************************/ 00034 /****************************************/ 00035 00036 void CLEDEquippedEntity::Init(TConfigurationNode& t_tree) { 00037 try { 00038 /* Init parent */ 00039 CComposableEntity::Init(t_tree); 00040 /* Go through the led entries */ 00041 CVector3 cPosition; 00042 CColor cColor; 00043 TConfigurationNodeIterator itLED("led"); 00044 for(itLED = itLED.begin(&t_tree); 00045 itLED != itLED.end(); 00046 ++itLED) { 00047 /* Initialise the LED using the XML */ 00048 CLEDEntity* pcLED = new CLEDEntity(this); 00049 pcLED->Init(*itLED); 00050 /* Add the LED to this container */ 00051 m_vecLEDOffsetPositions.push_back(pcLED->GetPosition()); 00052 m_tLEDs.push_back(pcLED); 00053 AddComponent(*pcLED); 00054 } 00055 UpdateComponents(); 00056 } 00057 catch(CARGoSException& ex) { 00058 THROW_ARGOSEXCEPTION_NESTED("Failed to initialize LED equipped entity \"" << GetId() << "\".", ex); 00059 } 00060 } 00061 00062 /****************************************/ 00063 /****************************************/ 00064 00065 void CLEDEquippedEntity::Reset() { 00066 for(CLEDEntity::TList::iterator it = m_tLEDs.begin(); 00067 it != m_tLEDs.end(); 00068 ++it) { 00069 (*it)->Reset(); 00070 } 00071 } 00072 00073 /****************************************/ 00074 /****************************************/ 00075 00076 void CLEDEquippedEntity::AddLED(const CVector3& c_position, 00077 const CColor& c_color) { 00078 CLEDEntity* pcLED = 00079 new CLEDEntity( 00080 this, 00081 std::string("led_") + ToString(m_tLEDs.size()), 00082 c_position, 00083 c_color); 00084 m_tLEDs.push_back(pcLED); 00085 AddComponent(*pcLED); 00086 } 00087 00088 /****************************************/ 00089 /****************************************/ 00090 00091 void CLEDEquippedEntity::AddLEDRing(const CVector3& c_center, 00092 Real f_radius, 00093 const CRadians& c_start_angle, 00094 UInt32 un_num_leds, 00095 const CColor& c_color) { 00096 CRadians cLEDSpacing = CRadians::TWO_PI / un_num_leds; 00097 CRadians cAngle; 00098 CVector3 cPos; 00099 for(UInt32 i = 0; i < un_num_leds; ++i) { 00100 cAngle = c_start_angle + i * cLEDSpacing; 00101 cAngle.SignedNormalize(); 00102 cPos.Set(f_radius, 0.0f, 0.0f); 00103 cPos.RotateZ(cAngle); 00104 cPos += c_center; 00105 AddLED(cPos, c_color); 00106 } 00107 } 00108 00109 /****************************************/ 00110 /****************************************/ 00111 00112 CLEDEntity& CLEDEquippedEntity::GetLED(UInt32 un_index) { 00113 ARGOS_ASSERT(un_index < m_tLEDs.size(), 00114 "CLEDEquippedEntity::GetLED(), id=\"" << 00115 GetId() << 00116 "\": index out of bounds: un_index = " << 00117 un_index << 00118 ", m_tLEDs.size() = " << 00119 m_tLEDs.size()); 00120 return *m_tLEDs[un_index]; 00121 } 00122 00123 /****************************************/ 00124 /****************************************/ 00125 00126 void CLEDEquippedEntity::SetLEDPosition(UInt32 un_index, 00127 const CVector3& c_position) { 00128 ARGOS_ASSERT(un_index < m_tLEDs.size(), 00129 "CLEDEquippedEntity::SetLEDPosition(), id=\"" << 00130 GetId() << 00131 "\": index out of bounds: un_index = " << 00132 un_index << 00133 ", m_tLEDs.size() = " << 00134 m_tLEDs.size()); 00135 m_tLEDs[un_index]->SetPosition(c_position); 00136 } 00137 00138 /****************************************/ 00139 /****************************************/ 00140 00141 void CLEDEquippedEntity::SetLEDColor(UInt32 un_index, 00142 const CColor& c_color) { 00143 ARGOS_ASSERT(un_index < m_tLEDs.size(), 00144 "CLEDEquippedEntity::SetLEDColor(), id=\"" << 00145 GetId() << 00146 "\": index out of bounds: un_index = " << 00147 un_index << 00148 ", m_tLEDs.size() = " << 00149 m_tLEDs.size()); 00150 m_tLEDs[un_index]->SetColor(c_color); 00151 } 00152 00153 /****************************************/ 00154 /****************************************/ 00155 00156 void CLEDEquippedEntity::SetAllLEDsColors(const CColor& c_color) { 00157 for(UInt32 i = 0; i < m_tLEDs.size(); ++i) { 00158 m_tLEDs[i]->SetColor(c_color); 00159 } 00160 } 00161 00162 /****************************************/ 00163 /****************************************/ 00164 00165 void CLEDEquippedEntity::SetAllLEDsColors(const std::vector<CColor>& vec_colors) { 00166 if(vec_colors.size() == m_tLEDs.size()) { 00167 for(UInt32 i = 0; i < vec_colors.size(); ++i) { 00168 m_tLEDs[i]->SetColor(vec_colors[i]); 00169 } 00170 } 00171 else { 00172 THROW_ARGOSEXCEPTION( 00173 "CLEDEquippedEntity::SetAllLEDsColors(), id=\"" << 00174 GetId() << 00175 "\": number of LEDs (" << 00176 m_tLEDs.size() << 00177 ") is lower than the passed color vector size (" << 00178 vec_colors.size() << 00179 ")"); 00180 } 00181 } 00182 00183 /****************************************/ 00184 /****************************************/ 00185 00186 void CLEDEquippedEntity::UpdateComponents() { 00187 if(HasReferenceEntity()) { 00188 /* Set LED position wrt reference */ 00189 CVector3 cLEDPosition; 00190 for(UInt32 i = 0; i < m_tLEDs.size(); ++i) { 00191 cLEDPosition = m_vecLEDOffsetPositions[i]; 00192 cLEDPosition.Rotate(m_pcReferenceEntity->GetOrientation()); 00193 cLEDPosition += m_pcReferenceEntity->GetPosition(); 00194 SetLEDPosition(i, cLEDPosition); 00195 } 00196 } 00197 } 00198 00199 /****************************************/ 00200 /****************************************/ 00201 00202 void CLEDEquippedEntity::AddToMedium(CLEDMedium& c_medium) { 00203 for(UInt32 i = 0; i < m_tLEDs.size(); ++i) { 00204 m_tLEDs[i]->AddToMedium(c_medium); 00205 } 00206 } 00207 00208 /****************************************/ 00209 /****************************************/ 00210 00211 void CLEDEquippedEntity::RemoveFromMedium(CLEDMedium& c_medium) { 00212 for(UInt32 i = 0; i < m_tLEDs.size(); ++i) { 00213 m_tLEDs[i]->RemoveFromMedium(c_medium); 00214 } 00215 } 00216 00217 /****************************************/ 00218 /****************************************/ 00219 00220 REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CLEDEquippedEntity); 00221 00222 /****************************************/ 00223 /****************************************/ 00224 00225 }