controllable_entity.cpp
Go to the documentation of this file.
1 
7 #include "controllable_entity.h"
8 #include <argos3/core/simulator/actuator.h>
9 #include <argos3/core/simulator/sensor.h>
10 #include <argos3/core/simulator/simulator.h>
11 #include <argos3/core/simulator/entity/composable_entity.h>
12 #include <argos3/core/simulator/space/space.h>
13 
14 namespace argos {
15 
16  /****************************************/
17  /****************************************/
18 
20  CEntity(pc_parent),
21  m_pcController(NULL) {}
22 
23  /****************************************/
24  /****************************************/
25 
27  const std::string& str_id) :
28  CEntity(pc_parent, str_id),
29  m_pcController(NULL) {
30  }
31 
32  /****************************************/
33  /****************************************/
34 
36  if(m_pcController != NULL) {
37  delete m_pcController;
38  }
39  }
40 
41  /****************************************/
42  /****************************************/
43 
45  try {
46  /* Init parent */
47  CEntity::Init(t_tree);
48  /* Get the controller id */
49  std::string strControllerId;
50  GetNodeAttribute(t_tree, "config", strControllerId);
51  /* Check if the tree has parameters to pass to the controller */
52  if(NodeExists(t_tree, "params")) {
53  /* Set the controller */
54  SetController(strControllerId,
55  GetNode(t_tree,
56  "params"));
57  }
58  else {
59  /* Set the controller */
60  SetController(strControllerId);
61  }
62  }
63  catch(CARGoSException& ex) {
64  THROW_ARGOSEXCEPTION_NESTED("Failed to initialize controllable entity \"" << GetId() << "\".", ex);
65  }
66  }
67 
68  /****************************************/
69  /****************************************/
70 
72  /* Clear rays */
73  m_vecCheckedRays.clear();
75  /* Reset sensors */
76  for(CCI_Sensor::TMap::iterator it = m_pcController->GetAllSensors().begin();
77  it != m_pcController->GetAllSensors().end(); ++it) {
78  it->second->Reset();
79  }
80  /* Reset actuators */
81  for(CCI_Actuator::TMap::iterator it = m_pcController->GetAllActuators().begin();
82  it != m_pcController->GetAllActuators().end(); ++it) {
83  it->second->Reset();
84  }
85  /* Reset user-defined controller */
87  }
88 
89  /****************************************/
90  /****************************************/
91 
93  /* Clear rays */
94  m_vecCheckedRays.clear();
96  if(m_pcController) {
97  /* Destroy sensors */
98  for(CCI_Sensor::TMap::iterator it = m_pcController->GetAllSensors().begin();
99  it != m_pcController->GetAllSensors().end(); ++it) {
100  it->second->Destroy();
101  }
102  /* Destroy actuators */
103  for(CCI_Actuator::TMap::iterator it = m_pcController->GetAllActuators().begin();
104  it != m_pcController->GetAllActuators().end(); ++it) {
105  it->second->Destroy();
106  }
107  /* Destroy user-defined controller */
109  }
110  }
111 
112  /****************************************/
113  /****************************************/
114 
116  if(m_pcController != NULL) {
117  return *m_pcController;
118  }
119  else {
120  THROW_ARGOSEXCEPTION("Entity " << GetId() << " does not have any controller associated.");
121  }
122  }
123 
124  /****************************************/
125  /****************************************/
126 
128  if(m_pcController != NULL) {
129  return *m_pcController;
130  }
131  else {
132  THROW_ARGOSEXCEPTION("Entity " << GetId() << " does not have any controller associated.");
133  }
134  }
135 
136  /****************************************/
137  /****************************************/
138 
139  void CControllableEntity::SetController(const std::string& str_controller_id) {
140  TConfigurationNode& tConfig = CSimulator::GetInstance().GetConfigForController(str_controller_id);
141  TConfigurationNode& tParams = GetNode(tConfig, "params");
142  SetController(str_controller_id, tParams);
143  }
144 
145  /****************************************/
146  /****************************************/
147 
148  void CControllableEntity::SetController(const std::string& str_controller_id,
149  TConfigurationNode& t_controller_config) {
150  try {
151  /* Look in the map for the parsed XML configuration of the wanted controller */
152  TConfigurationNode& tConfig = CSimulator::GetInstance().GetConfigForController(str_controller_id);
153  /* tConfig is the base of the XML section of the wanted controller */
154  std::string strImpl;
155  /* Create the controller */
158  /* Go through actuators */
159  TConfigurationNode& tActuators = GetNode(tConfig, "actuators");
161  for(itAct = itAct.begin(&tActuators);
162  itAct != itAct.end();
163  ++itAct) {
164  /* itAct->Value() is the name of the current actuator */
165  GetNodeAttribute(*itAct, "implementation", strImpl);
166  CSimulatedActuator* pcAct = CFactory<CSimulatedActuator>::New(itAct->Value() + " (" + strImpl + ")");
167  CCI_Actuator* pcCIAct = dynamic_cast<CCI_Actuator*>(pcAct);
168  if(pcCIAct == NULL) {
169  THROW_ARGOSEXCEPTION("BUG: actuator \"" << itAct->Value() << "\" does not inherit from CCI_Actuator");
170  }
171  pcAct->SetRobot(GetParent());
172  pcCIAct->Init(*itAct);
173  m_mapActuators[itAct->Value()] = pcAct;
174  m_pcController->AddActuator(itAct->Value(), pcCIAct);
175  }
176  /* Go through sensors */
177  TConfigurationNode& tSensors = GetNode(tConfig, "sensors");
179  for(itSens = itSens.begin(&tSensors);
180  itSens != itSens.end();
181  ++itSens) {
182  /* itSens->Value() is the name of the current actuator */
183  GetNodeAttribute(*itSens, "implementation", strImpl);
184  CSimulatedSensor* pcSens = CFactory<CSimulatedSensor>::New(itSens->Value() + " (" + strImpl + ")");
185  CCI_Sensor* pcCISens = dynamic_cast<CCI_Sensor*>(pcSens);
186  if(pcCISens == NULL) {
187  THROW_ARGOSEXCEPTION("BUG: sensor \"" << itSens->Value() << "\" does not inherit from CCI_Sensor");
188  }
189  pcSens->SetRobot(GetParent());
190  pcCISens->Init(*itSens);
191  m_mapSensors[itSens->Value()] = pcSens;
192  m_pcController->AddSensor(itSens->Value(), pcCISens);
193  }
194  /* Configure the controller */
195  m_pcController->Init(t_controller_config);
196  }
197  catch(CARGoSException& ex) {
198  THROW_ARGOSEXCEPTION_NESTED("Can't set controller for controllable entity \"" << GetId() << "\"", ex);
199  }
200  }
201 
202  /****************************************/
203  /****************************************/
204 
206  m_vecCheckedRays.clear();
207  m_vecIntersectionPoints.clear();
208  for(std::map<std::string, CSimulatedSensor*>::iterator it = m_mapSensors.begin();
209  it != m_mapSensors.end(); ++it) {
210  it->second->Update();
211  }
212  }
213 
214  /****************************************/
215  /****************************************/
216 
218  if(m_pcController != NULL) {
220  }
221  else {
222  THROW_ARGOSEXCEPTION("Entity " << GetId() << " does not have any controller associated.");
223  }
224  }
225 
226  /****************************************/
227  /****************************************/
228 
230  for(std::map<std::string, CSimulatedActuator*>::iterator it = m_mapActuators.begin();
231  it != m_mapActuators.end(); ++it) {
232  it->second->Update();
233  }
234  }
235 
236  /****************************************/
237  /****************************************/
238 
242  class CSpaceOperationAddControllableEntity : public CSpaceOperationAddEntity {
243  public:
244  void ApplyTo(CSpace& c_space, CControllableEntity& c_entity) {
245  c_space.AddEntity(c_entity);
246  c_space.AddControllableEntity(c_entity);
247  }
248  };
249  REGISTER_SPACE_OPERATION(CSpaceOperationAddEntity,
250  CSpaceOperationAddControllableEntity,
251  CControllableEntity);
252 
253  class CSpaceOperationRemoveControllableEntity : public CSpaceOperationRemoveEntity {
254  public:
255  void ApplyTo(CSpace& c_space, CControllableEntity& c_entity) {
256  c_space.RemoveControllableEntity(c_entity);
257  c_space.RemoveEntity(c_entity);
258  }
259  };
260  REGISTER_SPACE_OPERATION(CSpaceOperationRemoveEntity,
261  CSpaceOperationRemoveControllableEntity,
262  CControllableEntity);
267  /****************************************/
268  /****************************************/
269 
270 }
void SetId(const std::string &str_id)
Sets the id of the robot associated to this controller.
Definition: ci_controller.h:88
An entity that contains a pointer to the user-defined controller.
void AddEntity(ENTITY &c_entity)
Adds an entity of the given type.
Definition: space.h:253
The basic interface for all actuators.
Definition: ci_actuator.h:34
CCI_Sensor::TMap & GetAllSensors()
Returns a map of the associated sensors.
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
std::vector< std::pair< bool, CRay3 > > m_vecCheckedRays
The list of checked rays.
virtual void AddControllableEntity(CControllableEntity &c_entity)
Definition: space.cpp:144
CControllableEntity(CComposableEntity *pc_parent)
Class constructor.
The basic interface for a simulated actuator.
Definition: actuator.h:22
The basic entity type.
Definition: entity.h:89
TConfigurationNode & GetNode(TConfigurationNode &t_node, const std::string &str_tag)
Given a tree root node, returns the first of its child nodes with the wanted name.
virtual void SetRobot(CComposableEntity &c_entity)=0
Sets the entity associated to this actuator.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
virtual void SetRobot(CComposableEntity &c_entity)=0
Sets the entity associated to this sensor.
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception...
virtual void Sense()
Executes the CSimulatedSensor::Update() method for all associated sensors.
The basic interface for a simulated sensor.
Definition: sensor.h:22
REGISTER_SPACE_OPERATION(CSpaceOperationAddEntity, CSpaceOperationAddCFloorEntity, CFloorEntity)
std::map< std::string, CSimulatedSensor * > m_mapSensors
The map of sensors, indexed by sensor type (not implementation!)
virtual void Reset()
Resets the state of the entity to whatever it was after Init() or one of the standalone constructors ...
void AddSensor(const std::string &str_sensor_type, CCI_Sensor *pc_sensor)
Adds an sensor to this controller.
void SetController(const std::string &str_controller_id)
Creates and assigns a controller with the given id.
virtual ~CControllableEntity()
Class destructor.
Basic class for an entity that contains other entities.
virtual void Destroy()
The default implementation of this method does nothing.
Definition: ci_controller.h:72
static TYPE * New(const std::string &str_label)
Creates a new object of type TYPE
Definition: factory_impl.h:48
std::map< std::string, CSimulatedActuator * > m_mapActuators
The map of actuators, indexed by actuator type (not implementation!)
virtual void ControlStep()
Executes a control step.
Definition: ci_controller.h:55
The basic interface for all sensors.
Definition: ci_sensor.h:34
CCI_Actuator::TMap & GetAllActuators()
Returns a map of the associated actuators.
bool NodeExists(TConfigurationNode &t_node, const std::string &str_tag)
Given a tree root node, returns true if one of its child nodes has the wanted name.
virtual void Reset()
Resets the state of the controller to what it was right after Init() was executed.
Definition: ci_controller.h:65
ticpp::Iterator< ticpp::Element > TConfigurationNodeIterator
The iterator for the ARGoS configuration XML node.
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
The exception that wraps all errors in ARGoS.
virtual void Init(TConfigurationNode &t_node)
Initializes the controller.
Definition: ci_controller.h:48
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: entity.cpp:40
CCI_Controller * m_pcController
The pointer to the associated controller.
virtual void ControlStep()
Executes CCI_Controller::ControlStep().
const CCI_Controller & GetController() const
Returns a reference to the associated controller.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
virtual void Destroy()
Destroys the entity, undoing whatever was done by Init() or by the standalone constructor.
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
CComposableEntity & GetParent()
Returns this entity's parent.
Definition: entity.cpp:91
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
virtual void Act()
Executes the CSimulatedActuator::Update() method for all associated actuators.
std::vector< CVector3 > m_vecIntersectionPoints
The list of intersection points.
void AddActuator(const std::string &str_actuator_type, CCI_Actuator *pc_actuator)
Adds an actuator to this controller.
The basic interface for a robot controller.
Definition: ci_controller.h:30
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
Definition: simulator.cpp:78
TConfigurationNode & GetConfigForController(const std::string &str_id)
Returns the XML portion relative to the controller with the given ID.
Definition: simulator.cpp:95