ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "rab_equipped_entity.h" 00008 #include <argos3/core/utility/string_utilities.h> 00009 #include <argos3/core/simulator/simulator.h> 00010 #include <argos3/core/simulator/space/space.h> 00011 00012 namespace argos { 00013 00014 /****************************************/ 00015 /****************************************/ 00016 00017 CRABEquippedEntity::CRABEquippedEntity(CComposableEntity* pc_parent) : 00018 CPositionalEntity(pc_parent), 00019 m_pcReference(NULL), 00020 m_fRange(0.0f) { 00021 Disable(); 00022 SetCanBeEnabledIfDisabled(false); 00023 } 00024 00025 /****************************************/ 00026 /****************************************/ 00027 00028 CRABEquippedEntity::CRABEquippedEntity(CComposableEntity* pc_parent, 00029 const std::string& str_id, 00030 size_t un_msg_size, 00031 Real f_range, 00032 CEmbodiedEntity& c_reference, 00033 const CVector3& c_pos_offset, 00034 const CQuaternion& c_rot_offset) : 00035 CPositionalEntity(pc_parent, 00036 str_id, 00037 c_reference.GetInitPosition() + c_pos_offset, 00038 c_reference.GetInitOrientation() * c_rot_offset), 00039 m_pcReference(&c_reference), 00040 m_cPosOffset(c_pos_offset), 00041 m_cRotOffset(c_rot_offset), 00042 m_cData(un_msg_size), 00043 m_fRange(f_range) { 00044 Disable(); 00045 SetCanBeEnabledIfDisabled(false); 00046 } 00047 00048 /****************************************/ 00049 /****************************************/ 00050 00051 void CRABEquippedEntity::Init(TConfigurationNode& t_tree) { 00052 try { 00053 /* 00054 * Init entity. 00055 * Here we explicitly avoid to call CPositionalEntity::Init() because that 00056 * would also initialize position and orientation, which, instead, must 00057 * be calculated from reference entity and offsets. 00058 */ 00059 CEntity::Init(t_tree); 00060 /* Get reference entity */ 00061 std::string strReference; 00062 GetNodeAttribute(t_tree, "reference", strReference); 00063 m_pcReference = dynamic_cast<CEmbodiedEntity*>(&CSimulator::GetInstance().GetSpace().GetEntity(strReference)); 00064 if(m_pcReference == NULL) { 00065 THROW_ARGOSEXCEPTION("Entity \"" << strReference << "\" can't be used as a reference for range and bearing entity \"" << GetId() << "\""); 00066 } 00067 /* Get offsets */ 00068 GetNodeAttributeOrDefault(t_tree, "pos_offset", m_cPosOffset, m_cPosOffset); 00069 std::string strRotOffset; 00070 GetNodeAttributeOrDefault(t_tree, "rot_offset", strRotOffset, strRotOffset); 00071 if(strRotOffset != "") { 00072 CDegrees cRotOffsetEuler[3]; 00073 ParseValues(strRotOffset, 3, cRotOffsetEuler, ','); 00074 m_cRotOffset.FromEulerAngles(ToRadians(cRotOffsetEuler[0]), 00075 ToRadians(cRotOffsetEuler[1]), 00076 ToRadians(cRotOffsetEuler[2])); 00077 } 00078 /* Set init position and orientation */ 00079 SetInitPosition(m_pcReference->GetInitPosition() + m_cPosOffset); 00080 SetInitOrientation(m_pcReference->GetInitOrientation() * m_cRotOffset); 00081 SetPosition(GetInitPosition()); 00082 SetOrientation(GetInitOrientation()); 00083 /* Get message size */ 00084 size_t unMsgSize; 00085 GetNodeAttribute(t_tree, "msg_size", unMsgSize); 00086 m_cData.Resize(unMsgSize); 00087 /* Get transmission range */ 00088 GetNodeAttribute(t_tree, "range", m_fRange); 00089 } 00090 catch(CARGoSException& ex) { 00091 THROW_ARGOSEXCEPTION_NESTED("Error initializing a range and bearing entity \"" << GetId() << "\"", ex); 00092 } 00093 } 00094 00095 /****************************************/ 00096 /****************************************/ 00097 00098 void CRABEquippedEntity::Update() { 00099 SetPosition(m_pcReference->GetPosition() + m_cPosOffset); 00100 SetOrientation(m_pcReference->GetOrientation() * m_cRotOffset); 00101 } 00102 00103 /****************************************/ 00104 /****************************************/ 00105 00106 void CRABEquippedEntity::Reset() { 00107 m_cData.Zero(); 00108 } 00109 00110 /****************************************/ 00111 /****************************************/ 00112 00113 void CRABEquippedEntity::SetData(const CByteArray& c_data) { 00114 if(m_cData.Size() == c_data.Size()) { 00115 m_cData = c_data; 00116 } 00117 else { 00118 THROW_ARGOSEXCEPTION("CRABEquippedEntity::SetData() : data size does not match, expected " << m_cData.Size() << ", got " << c_data.Size()); 00119 } 00120 } 00121 00122 /****************************************/ 00123 /****************************************/ 00124 00125 void CRABEquippedEntity::ClearData() { 00126 m_cData.Zero(); 00127 } 00128 00129 /****************************************/ 00130 /****************************************/ 00131 00132 void CRABEquippedEntitySpaceHashUpdater::operator()(CAbstractSpaceHash<CRABEquippedEntity>& c_space_hash, 00133 CRABEquippedEntity& c_element) { 00134 /* Calculate the position of the center of the RAB equipped entity in the space hash */ 00135 c_space_hash.SpaceToHashTable(m_nCenterI, 00136 m_nCenterJ, 00137 m_nCenterK, 00138 c_element.GetPosition()); 00139 /* Update the cells in a sphere around it */ 00140 SInt32 nRangeI = c_space_hash.SpaceToHashTable(c_element.GetRange(), 0); 00141 SInt32 nRangeJ; 00142 SInt32 nRangeK; 00143 for(SInt32 i = 0; i <= nRangeI; ++i) { 00144 nRangeJ = 00145 c_space_hash.SpaceToHashTable( 00146 ::sqrt( 00147 Square(c_element.GetRange()) - 00148 Square(c_space_hash.HashTableToSpace(i, 0)) 00149 ), 00150 1); 00151 for(SInt32 j = 0; j <= nRangeJ; ++j) { 00152 nRangeK = 00153 c_space_hash.SpaceToHashTable( 00154 ::sqrt( 00155 Square(c_element.GetRange()) - 00156 Square(c_space_hash.HashTableToSpace(j, 1)) 00157 ), 00158 2); 00159 for(SInt32 k = 0; k <= nRangeK; ++k) { 00160 if(i > 0) { 00161 /* 00162 * i > 0 00163 */ 00164 if(j > 0) { 00165 /* 00166 * i > 0 00167 * j > 0 00168 */ 00169 if(k > 0) { 00170 /* 00171 * i > 0 00172 * j > 0 00173 * k > 0 00174 */ 00175 c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ + j, m_nCenterK + k, c_element); 00176 c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ + j, m_nCenterK - k, c_element); 00177 c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ - j, m_nCenterK + k, c_element); 00178 c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ - j, m_nCenterK - k, c_element); 00179 c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ + j, m_nCenterK + k, c_element); 00180 c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ + j, m_nCenterK - k, c_element); 00181 c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ - j, m_nCenterK + k, c_element); 00182 c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ - j, m_nCenterK - k, c_element); 00183 } 00184 else { 00185 /* 00186 * i > 0 00187 * j > 0 00188 * k == 0 00189 */ 00190 c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ + j, m_nCenterK, c_element); 00191 c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ - j, m_nCenterK, c_element); 00192 c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ + j, m_nCenterK, c_element); 00193 c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ - j, m_nCenterK, c_element); 00194 } 00195 } 00196 else { 00197 /* 00198 * i > 0 00199 * j == 0 00200 */ 00201 if(k > 0) { 00202 /* 00203 * i > 0 00204 * j == 0 00205 * k > 0 00206 */ 00207 c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ, m_nCenterK + k, c_element); 00208 c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ, m_nCenterK - k, c_element); 00209 c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ, m_nCenterK + k, c_element); 00210 c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ, m_nCenterK - k, c_element); 00211 } 00212 else { 00213 /* 00214 * i > 0 00215 * j == 0 00216 * k == 0 00217 */ 00218 c_space_hash.UpdateCell(m_nCenterI + i, m_nCenterJ, m_nCenterK, c_element); 00219 c_space_hash.UpdateCell(m_nCenterI - i, m_nCenterJ, m_nCenterK, c_element); 00220 } 00221 } 00222 } 00223 else { 00224 /* 00225 * i == 0 00226 */ 00227 if(j > 0) { 00228 /* 00229 * i == 0 00230 * j > 0 00231 */ 00232 if(k > 0) { 00233 /* 00234 * i == 0 00235 * j > 0 00236 * k > 0 00237 */ 00238 c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ + j, m_nCenterK + k, c_element); 00239 c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ + j, m_nCenterK - k, c_element); 00240 c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ - j, m_nCenterK + k, c_element); 00241 c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ - j, m_nCenterK - k, c_element); 00242 } 00243 else { 00244 /* 00245 * i == 0 00246 * j > 0 00247 * k == 0 00248 */ 00249 c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ + j, m_nCenterK, c_element); 00250 c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ - j, m_nCenterK, c_element); 00251 } 00252 } 00253 else { 00254 /* 00255 * i == 0 00256 * j == 0 00257 */ 00258 if(k > 0) { 00259 /* 00260 * i == 0 00261 * j == 0 00262 * k > 0 00263 */ 00264 c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ, m_nCenterK + k, c_element); 00265 c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ, m_nCenterK - k, c_element); 00266 } 00267 else { 00268 /* 00269 * i == 0 00270 * j == 0 00271 * k == 0 00272 */ 00273 c_space_hash.UpdateCell(m_nCenterI, m_nCenterJ, m_nCenterK, c_element); 00274 } 00275 } 00276 } 00277 } 00278 } 00279 } 00280 } 00281 00282 /****************************************/ 00283 /****************************************/ 00284 00285 REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CRABEquippedEntity); 00286 00287 /****************************************/ 00288 /****************************************/ 00289 00290 CRABEquippedEntityGridCellUpdater::CRABEquippedEntityGridCellUpdater(CGrid<CRABEquippedEntity>& c_grid) : 00291 m_cGrid(c_grid) {} 00292 00293 bool CRABEquippedEntityGridCellUpdater::operator()(SInt32 n_i, 00294 SInt32 n_j, 00295 SInt32 n_k, 00296 CGrid<CRABEquippedEntity>::SCell& s_cell) { 00297 /* Update cell */ 00298 m_cGrid.UpdateCell(n_i, n_j, n_k, *m_pcEntity); 00299 /* Continue with other cells */ 00300 return true; 00301 } 00302 00303 void CRABEquippedEntityGridCellUpdater::SetEntity(CRABEquippedEntity& c_entity) { 00304 m_pcEntity = &c_entity; 00305 } 00306 00307 CRABEquippedEntityGridEntityUpdater::CRABEquippedEntityGridEntityUpdater(CGrid<CRABEquippedEntity>& c_grid) : 00308 m_cGrid(c_grid), 00309 m_cCellUpdater(c_grid) {} 00310 00311 bool CRABEquippedEntityGridEntityUpdater::operator()(CRABEquippedEntity& c_entity) { 00312 try { 00313 m_cCellUpdater.SetEntity(c_entity); 00314 m_cGrid.ForCellsInBoxRange(c_entity.GetPosition(), 00315 CVector3(c_entity.GetRange(), 00316 c_entity.GetRange(), 00317 c_entity.GetRange()), 00318 m_cCellUpdater); 00319 /* Continue with the other entities */ 00320 return true; 00321 } 00322 catch(CARGoSException& ex) { 00323 THROW_ARGOSEXCEPTION_NESTED("While updating the RAB entity grid for RAB entity \"" << c_entity.GetContext() << c_entity.GetId() << "\"", ex); 00324 } 00325 } 00326 00327 /****************************************/ 00328 /****************************************/ 00329 00330 }