ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "composable_entity.h" 00008 00009 #include <argos3/core/utility/string_utilities.h> 00010 00011 namespace argos { 00012 00013 /****************************************/ 00014 /****************************************/ 00015 00016 CComposableEntity::CComposableEntity(CComposableEntity* pc_parent) : 00017 CEntity(pc_parent) {} 00018 00019 /****************************************/ 00020 /****************************************/ 00021 00022 CComposableEntity::CComposableEntity(CComposableEntity* pc_parent, 00023 const std::string& str_id) : 00024 CEntity(pc_parent, str_id) {} 00025 00026 /****************************************/ 00027 /****************************************/ 00028 00029 void CComposableEntity::Reset() { 00030 for(CEntity::TMultiMap::iterator it = m_mapComponents.begin(); 00031 it != m_mapComponents.end(); 00032 ++it) { 00033 it->second->Reset(); 00034 } 00035 } 00036 00037 /****************************************/ 00038 /****************************************/ 00039 00040 void CComposableEntity::Update() { 00041 UpdateComponents(); 00042 } 00043 00044 /****************************************/ 00045 /****************************************/ 00046 00047 void CComposableEntity::SetEnabled(bool b_enabled) { 00048 CEntity::SetEnabled(b_enabled); 00049 for(CEntity::TMultiMap::iterator it = m_mapComponents.begin(); 00050 it != m_mapComponents.end(); 00051 ++it) { 00052 it->second->SetEnabled(b_enabled); 00053 } 00054 } 00055 00056 /****************************************/ 00057 /****************************************/ 00058 00059 void CComposableEntity::UpdateComponents() { 00060 for(CEntity::TMultiMap::iterator it = m_mapComponents.begin(); 00061 it != m_mapComponents.end(); 00062 ++it) { 00063 if(it->second->IsEnabled()) { 00064 it->second->Update(); 00065 } 00066 } 00067 } 00068 00069 /****************************************/ 00070 /****************************************/ 00071 00072 void CComposableEntity::AddComponent(CEntity& c_component) { 00073 m_mapComponents.insert( 00074 std::pair<std::string, CEntity*>( 00075 c_component.GetTypeDescription(), 00076 &c_component)); 00077 m_vecComponents.push_back(&c_component); 00078 } 00079 00080 /****************************************/ 00081 /****************************************/ 00082 00083 CEntity& CComposableEntity::RemoveComponent(const std::string& str_component) { 00084 try { 00085 CEntity::TMultiMap::iterator it = FindComponent(str_component); 00086 if(it == m_mapComponents.end()) { 00087 THROW_ARGOSEXCEPTION("Element \"" << str_component << "\" not found in the component map."); 00088 } 00089 CEntity& cRetVal = *(it->second); 00090 m_mapComponents.erase(it); 00091 size_t i; 00092 for(i = 0; i < m_vecComponents.size() && m_vecComponents[i] != &cRetVal; ++i); 00093 if(i < m_vecComponents.size()) { 00094 m_vecComponents.erase(m_vecComponents.begin() + i); 00095 } 00096 else { 00097 THROW_ARGOSEXCEPTION("Element \"" << str_component << "\" not found in the component vector, but present in the map. BUG!"); 00098 } 00099 return cRetVal; 00100 } 00101 catch(CARGoSException& ex) { 00102 THROW_ARGOSEXCEPTION_NESTED("While removing component \"" << str_component << "\" from the composable entity \"" << GetContext() << GetId() << "\"", ex); 00103 } 00104 } 00105 00106 /****************************************/ 00107 /****************************************/ 00108 00109 CEntity& CComposableEntity::GetComponent(const std::string& str_path) { 00110 try { 00111 /* Search for the path separator character and take the first path segment */ 00112 size_t unFirstSeparatorIdx = str_path.find("."); 00113 std::string strFrontIdentifier = str_path.substr(0, unFirstSeparatorIdx); 00114 /* Try to find the relevant component in this context */ 00115 CEntity::TMultiMap::iterator itComponent = FindComponent(strFrontIdentifier); 00116 if(itComponent != m_mapComponents.end()) { 00117 if(unFirstSeparatorIdx == std::string::npos) { 00118 /* Path separator not found, found component in the current context is the one we want */ 00119 return *(itComponent->second); 00120 } 00121 /* Path separator found, try to cast the found component to a composable entity */ 00122 else { 00123 CComposableEntity* pcComposableEntity = dynamic_cast<CComposableEntity*>(itComponent->second); 00124 if(pcComposableEntity != NULL) { 00125 /* Dynamic cast of component to composable entity was successful, re-execute this function in the new context */ 00126 return pcComposableEntity->GetComponent(str_path.substr(unFirstSeparatorIdx + 1, std::string::npos)); 00127 } 00128 else { 00129 /* Dynamic cast failed, user is requesting an entity from an entity which is not composable -> error */ 00130 THROW_ARGOSEXCEPTION("Component \"" << strFrontIdentifier << "\" of \"" << GetContext() + GetId() 00131 << "\" is not a composable entity"); 00132 } 00133 } 00134 } 00135 else { 00136 THROW_ARGOSEXCEPTION("Component \"" << strFrontIdentifier << "\" does not exist in \"" 00137 << GetContext() + GetId() << "\""); 00138 } 00139 } 00140 catch(CARGoSException& ex) { 00141 THROW_ARGOSEXCEPTION_NESTED("While getting a component from a composable entity", ex); 00142 } 00143 } 00144 00145 /****************************************/ 00146 /****************************************/ 00147 00148 bool CComposableEntity::HasComponent(const std::string& str_path) { 00149 /* Search for the path separator character and take the first path segement */ 00150 size_t unFirstSeparatorIdx = str_path.find("."); 00151 std::string strFrontIdentifier = str_path.substr(0, unFirstSeparatorIdx); 00152 /* Try to find the relevant component in this context */ 00153 CEntity::TMultiMap::iterator itComponent = FindComponent(strFrontIdentifier); 00154 if(itComponent != m_mapComponents.end()) { 00155 if(unFirstSeparatorIdx == std::string::npos) { 00156 /* Path separator not found, found component in the current context is the one we want */ 00157 return true; 00158 } 00159 else { 00160 /* Path separator found, try to cast the found component to a composable entity */ 00161 CComposableEntity* pcComposableEntity = dynamic_cast<CComposableEntity*>(itComponent->second); 00162 if(pcComposableEntity != NULL) { 00163 /* Dynamic cast of component to composable entity was sucessful, re-execute this function in the new context */ 00164 return pcComposableEntity->HasComponent(str_path.substr(unFirstSeparatorIdx + 1, std::string::npos)); 00165 } 00166 else { 00167 /* Could not cast to a composable entity, the queried component cannot exist in the specified context */ 00168 return false; 00169 } 00170 } 00171 } 00172 else { 00173 /* Could not find the queried component in this context */ 00174 return false; 00175 } 00176 } 00177 00178 /****************************************/ 00179 /****************************************/ 00180 00181 CEntity::TMultiMap::iterator CComposableEntity::FindComponent(const std::string& str_component) { 00182 /* Check for the presence of [ */ 00183 std::string::size_type unIdentifierStart = str_component.find('['); 00184 if(unIdentifierStart != std::string::npos) { 00185 /* Found, now check for the presence of ] after [ */ 00186 std::string::size_type unIdentifierEnd = str_component.rfind(']'); 00187 if(unIdentifierEnd != std::string::npos && 00188 unIdentifierEnd > unIdentifierStart) { 00189 /* Use the string between [ and ] as an index and whatever comes before as base id */ 00190 /* Count how many components there are for the base type */ 00191 std::string strBaseType = str_component.substr(0, unIdentifierStart); 00192 if(m_mapComponents.count(strBaseType) == 0) { 00193 /* No components of this base type, return an iterator to the end of the collection */ 00194 return m_mapComponents.end(); 00195 } 00196 else { 00197 /* Components of base type found - extract the uid and search for it */ 00198 std::string strComponentId = str_component.substr(unIdentifierStart + 1, unIdentifierEnd - unIdentifierStart - 1); 00199 /* Create a pair of iterators which mark the beginning and the end of the components that match the base type */ 00200 std::pair<CEntity::TMultiMap::iterator, 00201 CEntity::TMultiMap::iterator> cRange = m_mapComponents.equal_range(strBaseType); 00202 /* Create an iterator to hold the component we are trying to locate */ 00203 CEntity::TMultiMap::iterator itComponent; 00204 /* Search through components of base type and try find a match for the specified Id */ 00205 for(itComponent = cRange.first; 00206 (itComponent != cRange.second) && (itComponent->second->GetId() != strComponentId); 00207 ++itComponent); 00208 /* If the iterator itComponent is not equal to cRange.second, then we have found our component */ 00209 if(itComponent != cRange.second) { 00210 return itComponent; 00211 } 00212 else { 00213 /* Identifer not found in the collection of components with the specified base type, 00214 * return an iterator to the end of the collect to show this */ 00215 return m_mapComponents.end(); 00216 } 00217 } 00218 } 00219 else { 00220 THROW_ARGOSEXCEPTION("Syntax error in entity id \"" << str_component << "\""); 00221 } 00222 } 00223 else { 00224 /* Identifier syntax not used, return an iterator to the first element or the end of collection if 00225 * no elements are found */ 00226 return m_mapComponents.find(str_component); 00227 } 00228 } 00229 00230 /****************************************/ 00231 /****************************************/ 00232 00233 REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CComposableEntity); 00234 00235 /****************************************/ 00236 /****************************************/ 00237 00238 }