ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "controllable_entity.h" 00008 #include <argos3/core/simulator/actuator.h> 00009 #include <argos3/core/simulator/sensor.h> 00010 #include <argos3/core/simulator/simulator.h> 00011 #include <argos3/core/simulator/entity/composable_entity.h> 00012 #include <argos3/core/simulator/space/space.h> 00013 00014 namespace argos { 00015 00016 /****************************************/ 00017 /****************************************/ 00018 00019 CControllableEntity::CControllableEntity(CComposableEntity* pc_parent) : 00020 CEntity(pc_parent), 00021 m_pcController(NULL) {} 00022 00023 /****************************************/ 00024 /****************************************/ 00025 00026 CControllableEntity::CControllableEntity(CComposableEntity* pc_parent, 00027 const std::string& str_id) : 00028 CEntity(pc_parent, str_id), 00029 m_pcController(NULL) { 00030 } 00031 00032 /****************************************/ 00033 /****************************************/ 00034 00035 CControllableEntity::~CControllableEntity() { 00036 if(m_pcController != NULL) { 00037 delete m_pcController; 00038 } 00039 } 00040 00041 /****************************************/ 00042 /****************************************/ 00043 00044 void CControllableEntity::Init(TConfigurationNode& t_tree) { 00045 try { 00046 /* Init parent */ 00047 CEntity::Init(t_tree); 00048 /* Get the controller id */ 00049 std::string strControllerId; 00050 GetNodeAttribute(t_tree, "config", strControllerId); 00051 /* Check if the tree has parameters to pass to the controller */ 00052 if(NodeExists(t_tree, "params")) { 00053 /* Set the controller */ 00054 SetController(strControllerId, 00055 GetNode(t_tree, 00056 "params")); 00057 } 00058 else { 00059 /* Set the controller */ 00060 SetController(strControllerId); 00061 } 00062 } 00063 catch(CARGoSException& ex) { 00064 THROW_ARGOSEXCEPTION_NESTED("Failed to initialize controllable entity \"" << GetId() << "\".", ex); 00065 } 00066 } 00067 00068 /****************************************/ 00069 /****************************************/ 00070 00071 void CControllableEntity::Reset() { 00072 /* Clear rays */ 00073 m_vecCheckedRays.clear(); 00074 m_vecIntersectionPoints.clear(); 00075 /* Reset sensors */ 00076 for(CCI_Sensor::TMap::iterator it = m_pcController->GetAllSensors().begin(); 00077 it != m_pcController->GetAllSensors().end(); ++it) { 00078 it->second->Reset(); 00079 } 00080 /* Reset actuators */ 00081 for(CCI_Actuator::TMap::iterator it = m_pcController->GetAllActuators().begin(); 00082 it != m_pcController->GetAllActuators().end(); ++it) { 00083 it->second->Reset(); 00084 } 00085 /* Reset user-defined controller */ 00086 m_pcController->Reset(); 00087 } 00088 00089 /****************************************/ 00090 /****************************************/ 00091 00092 void CControllableEntity::Destroy() { 00093 /* Clear rays */ 00094 m_vecCheckedRays.clear(); 00095 m_vecIntersectionPoints.clear(); 00096 if(m_pcController) { 00097 /* Destroy sensors */ 00098 for(CCI_Sensor::TMap::iterator it = m_pcController->GetAllSensors().begin(); 00099 it != m_pcController->GetAllSensors().end(); ++it) { 00100 it->second->Destroy(); 00101 } 00102 /* Destroy actuators */ 00103 for(CCI_Actuator::TMap::iterator it = m_pcController->GetAllActuators().begin(); 00104 it != m_pcController->GetAllActuators().end(); ++it) { 00105 it->second->Destroy(); 00106 } 00107 /* Destroy user-defined controller */ 00108 m_pcController->Destroy(); 00109 } 00110 } 00111 00112 /****************************************/ 00113 /****************************************/ 00114 00115 const CCI_Controller& CControllableEntity::GetController() const { 00116 if(m_pcController != NULL) { 00117 return *m_pcController; 00118 } 00119 else { 00120 THROW_ARGOSEXCEPTION("Entity " << GetId() << " does not have any controller associated."); 00121 } 00122 } 00123 00124 /****************************************/ 00125 /****************************************/ 00126 00127 CCI_Controller& CControllableEntity::GetController() { 00128 if(m_pcController != NULL) { 00129 return *m_pcController; 00130 } 00131 else { 00132 THROW_ARGOSEXCEPTION("Entity " << GetId() << " does not have any controller associated."); 00133 } 00134 } 00135 00136 /****************************************/ 00137 /****************************************/ 00138 00139 void CControllableEntity::SetController(const std::string& str_controller_id) { 00140 TConfigurationNode& tConfig = CSimulator::GetInstance().GetConfigForController(str_controller_id); 00141 TConfigurationNode& tParams = GetNode(tConfig, "params"); 00142 SetController(str_controller_id, tParams); 00143 } 00144 00145 /****************************************/ 00146 /****************************************/ 00147 00148 void CControllableEntity::SetController(const std::string& str_controller_id, 00149 TConfigurationNode& t_controller_config) { 00150 try { 00151 /* Look in the map for the parsed XML configuration of the wanted controller */ 00152 TConfigurationNode& tConfig = CSimulator::GetInstance().GetConfigForController(str_controller_id); 00153 /* tConfig is the base of the XML section of the wanted controller */ 00154 std::string strImpl; 00155 /* Create the controller */ 00156 m_pcController = CFactory<CCI_Controller>::New(tConfig.Value()); 00157 m_pcController->SetId(GetParent().GetId()); 00158 /* Go through actuators */ 00159 TConfigurationNode& tActuators = GetNode(tConfig, "actuators"); 00160 TConfigurationNodeIterator itAct; 00161 for(itAct = itAct.begin(&tActuators); 00162 itAct != itAct.end(); 00163 ++itAct) { 00164 /* itAct->Value() is the name of the current actuator */ 00165 GetNodeAttribute(*itAct, "implementation", strImpl); 00166 CSimulatedActuator* pcAct = CFactory<CSimulatedActuator>::New(itAct->Value() + " (" + strImpl + ")"); 00167 CCI_Actuator* pcCIAct = dynamic_cast<CCI_Actuator*>(pcAct); 00168 if(pcCIAct == NULL) { 00169 THROW_ARGOSEXCEPTION("BUG: actuator \"" << itAct->Value() << "\" does not inherit from CCI_Actuator"); 00170 } 00171 pcAct->SetRobot(GetParent()); 00172 pcCIAct->Init(*itAct); 00173 m_mapActuators[itAct->Value()] = pcAct; 00174 m_pcController->AddActuator(itAct->Value(), pcCIAct); 00175 } 00176 /* Go through sensors */ 00177 TConfigurationNode& tSensors = GetNode(tConfig, "sensors"); 00178 TConfigurationNodeIterator itSens; 00179 for(itSens = itSens.begin(&tSensors); 00180 itSens != itSens.end(); 00181 ++itSens) { 00182 /* itSens->Value() is the name of the current actuator */ 00183 GetNodeAttribute(*itSens, "implementation", strImpl); 00184 CSimulatedSensor* pcSens = CFactory<CSimulatedSensor>::New(itSens->Value() + " (" + strImpl + ")"); 00185 CCI_Sensor* pcCISens = dynamic_cast<CCI_Sensor*>(pcSens); 00186 if(pcCISens == NULL) { 00187 THROW_ARGOSEXCEPTION("BUG: sensor \"" << itSens->Value() << "\" does not inherit from CCI_Sensor"); 00188 } 00189 pcSens->SetRobot(GetParent()); 00190 pcCISens->Init(*itSens); 00191 m_mapSensors[itSens->Value()] = pcSens; 00192 m_pcController->AddSensor(itSens->Value(), pcCISens); 00193 } 00194 /* Configure the controller */ 00195 m_pcController->Init(t_controller_config); 00196 } 00197 catch(CARGoSException& ex) { 00198 THROW_ARGOSEXCEPTION_NESTED("Can't set controller for controllable entity \"" << GetId() << "\"", ex); 00199 } 00200 } 00201 00202 /****************************************/ 00203 /****************************************/ 00204 00205 void CControllableEntity::Sense() { 00206 m_vecCheckedRays.clear(); 00207 m_vecIntersectionPoints.clear(); 00208 for(std::map<std::string, CSimulatedSensor*>::iterator it = m_mapSensors.begin(); 00209 it != m_mapSensors.end(); ++it) { 00210 it->second->Update(); 00211 } 00212 } 00213 00214 /****************************************/ 00215 /****************************************/ 00216 00217 void CControllableEntity::ControlStep() { 00218 if(m_pcController != NULL) { 00219 m_pcController->ControlStep(); 00220 } 00221 else { 00222 THROW_ARGOSEXCEPTION("Entity " << GetId() << " does not have any controller associated."); 00223 } 00224 } 00225 00226 /****************************************/ 00227 /****************************************/ 00228 00229 void CControllableEntity::Act() { 00230 for(std::map<std::string, CSimulatedActuator*>::iterator it = m_mapActuators.begin(); 00231 it != m_mapActuators.end(); ++it) { 00232 it->second->Update(); 00233 } 00234 } 00235 00236 /****************************************/ 00237 /****************************************/ 00238 00242 class CSpaceOperationAddControllableEntity : public CSpaceOperationAddEntity { 00243 public: 00244 void ApplyTo(CSpace& c_space, CControllableEntity& c_entity) { 00245 c_space.AddEntity(c_entity); 00246 c_space.AddControllableEntity(c_entity); 00247 } 00248 }; 00249 REGISTER_SPACE_OPERATION(CSpaceOperationAddEntity, 00250 CSpaceOperationAddControllableEntity, 00251 CControllableEntity); 00252 00253 class CSpaceOperationRemoveControllableEntity : public CSpaceOperationRemoveEntity { 00254 public: 00255 void ApplyTo(CSpace& c_space, CControllableEntity& c_entity) { 00256 c_space.RemoveControllableEntity(c_entity); 00257 c_space.RemoveEntity(c_entity); 00258 } 00259 }; 00260 REGISTER_SPACE_OPERATION(CSpaceOperationRemoveEntity, 00261 CSpaceOperationRemoveControllableEntity, 00262 CControllableEntity); 00267 /****************************************/ 00268 /****************************************/ 00269 00270 }