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
00047 CEntity::Init(t_tree);
00048
00049 std::string strControllerId;
00050 GetNodeAttribute(t_tree, "config", strControllerId);
00051
00052 if(NodeExists(t_tree, "params")) {
00053
00054 SetController(strControllerId,
00055 GetNode(t_tree,
00056 "params"));
00057 }
00058 else {
00059
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
00073 m_vecCheckedRays.clear();
00074 m_vecIntersectionPoints.clear();
00075
00076 for(CCI_Sensor::TMap::iterator it = m_pcController->GetAllSensors().begin();
00077 it != m_pcController->GetAllSensors().end(); ++it) {
00078 it->second->Reset();
00079 }
00080
00081 for(CCI_Actuator::TMap::iterator it = m_pcController->GetAllActuators().begin();
00082 it != m_pcController->GetAllActuators().end(); ++it) {
00083 it->second->Reset();
00084 }
00085
00086 m_pcController->Reset();
00087 }
00088
00089
00090
00091
00092 void CControllableEntity::Destroy() {
00093
00094 m_vecCheckedRays.clear();
00095 m_vecIntersectionPoints.clear();
00096 if(m_pcController) {
00097
00098 for(CCI_Sensor::TMap::iterator it = m_pcController->GetAllSensors().begin();
00099 it != m_pcController->GetAllSensors().end(); ++it) {
00100 it->second->Destroy();
00101 }
00102
00103 for(CCI_Actuator::TMap::iterator it = m_pcController->GetAllActuators().begin();
00104 it != m_pcController->GetAllActuators().end(); ++it) {
00105 it->second->Destroy();
00106 }
00107
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
00152 TConfigurationNode& tConfig = CSimulator::GetInstance().GetConfigForController(str_controller_id);
00153
00154 std::string strImpl;
00155
00156 m_pcController = CFactory<CCI_Controller>::New(tConfig.Value());
00157 m_pcController->SetId(GetParent().GetId());
00158
00159 TConfigurationNode& tActuators = GetNode(tConfig, "actuators");
00160 TConfigurationNodeIterator itAct;
00161 for(itAct = itAct.begin(&tActuators);
00162 itAct != itAct.end();
00163 ++itAct) {
00164
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
00177 TConfigurationNode& tSensors = GetNode(tConfig, "sensors");
00178 TConfigurationNodeIterator itSens;
00179 for(itSens = itSens.begin(&tSensors);
00180 itSens != itSens.end();
00181 ++itSens) {
00182
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
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 }