ARGoS  3
A parallel, multi-engine simulator for swarm robotics
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 }
argos::CControllableEntity::ControlStep
virtual void ControlStep()
Executes CCI_Controller::ControlStep().
Definition: controllable_entity.cpp:217
argos::CSimulator::GetInstance
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
Definition: simulator.cpp:87
argos::CEntity::GetId
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
argos::CCI_Controller::AddSensor
void AddSensor(const std::string &str_sensor_type, CCI_Sensor *pc_sensor)
Adds an sensor to this controller.
Definition: ci_controller.h:205
argos
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
argos::CCI_Controller::Init
virtual void Init(TConfigurationNode &t_node)
Initializes the controller.
Definition: ci_controller.h:48
argos::CSpace
Definition: space.h:30
argos::REGISTER_SPACE_OPERATION
REGISTER_SPACE_OPERATION(CSpaceOperationAddEntity, CSpaceOperationAddCFloorEntity, CFloorEntity)
argos::CCI_Controller::GetAllActuators
CCI_Actuator::TMap & GetAllActuators()
Returns a map of the associated actuators.
Definition: ci_controller.h:174
argos::CComposableEntity
Basic class for an entity that contains other entities.
Definition: composable_entity.h:32
argos::CControllableEntity::m_mapActuators
std::map< std::string, CSimulatedActuator * > m_mapActuators
The map of actuators, indexed by actuator type (not implementation!)
Definition: controllable_entity.h:224
argos::GetNode
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.
Definition: argos_configuration.h:63
argos::CARGoSException
The exception that wraps all errors in ARGoS.
Definition: argos_exception.h:61
argos::CSimulatedActuator::SetRobot
virtual void SetRobot(CComposableEntity &c_entity)=0
Sets the entity associated to this actuator.
argos::CSimulatedActuator
The basic interface for a simulated actuator.
Definition: actuator.h:22
argos::CControllableEntity::m_pcController
CCI_Controller * m_pcController
The pointer to the associated controller.
Definition: controllable_entity.h:221
argos::CCI_Controller::AddActuator
void AddActuator(const std::string &str_actuator_type, CCI_Actuator *pc_actuator)
Adds an actuator to this controller.
Definition: ci_controller.h:193
argos::CCI_Controller::Destroy
virtual void Destroy()
The default implementation of this method does nothing.
Definition: ci_controller.h:72
argos::TConfigurationNode
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Definition: argos_configuration.h:27
argos::CSimulator::GetConfigForController
TConfigurationNode & GetConfigForController(const std::string &str_id)
Returns the XML portion relative to the controller with the given ID.
Definition: simulator.cpp:104
argos::CCI_Sensor
The basic interface for all sensors.
Definition: ci_sensor.h:34
argos::CControllableEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: controllable_entity.cpp:44
argos::CFactory::New
static TYPE * New(const std::string &str_label)
Creates a new object of type TYPE
Definition: factory_impl.h:48
argos::CCI_Controller::GetAllSensors
CCI_Sensor::TMap & GetAllSensors()
Returns a map of the associated sensors.
Definition: ci_controller.h:182
THROW_ARGOSEXCEPTION_NESTED
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception.
Definition: argos_exception.h:115
argos::CControllableEntity::m_vecIntersectionPoints
std::vector< CVector3 > m_vecIntersectionPoints
The list of intersection points.
Definition: controllable_entity.h:233
argos::CSpace::AddEntity
void AddEntity(ENTITY &c_entity)
Adds an entity of the given type.
Definition: space.h:254
argos::TConfigurationNodeIterator
ticpp::Iterator< ticpp::Element > TConfigurationNodeIterator
The iterator for the ARGoS configuration XML node.
Definition: argos_configuration.h:29
THROW_ARGOSEXCEPTION
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Definition: argos_exception.h:111
argos::CControllableEntity::Act
virtual void Act()
Executes the CSimulatedActuator::Update() method for all associated actuators.
Definition: controllable_entity.cpp:229
argos::CSpace::AddControllableEntity
virtual void AddControllableEntity(CControllableEntity &c_entity)
Definition: space.cpp:144
argos::CControllableEntity::~CControllableEntity
virtual ~CControllableEntity()
Class destructor.
Definition: controllable_entity.cpp:35
controllable_entity.h
argos::CCI_Actuator
The basic interface for all actuators.
Definition: ci_actuator.h:34
argos::CControllableEntity::m_mapSensors
std::map< std::string, CSimulatedSensor * > m_mapSensors
The map of sensors, indexed by sensor type (not implementation!)
Definition: controllable_entity.h:227
argos::CControllableEntity::Reset
virtual void Reset()
Resets the state of the entity to whatever it was after Init() or one of the standalone constructors ...
Definition: controllable_entity.cpp:71
argos::NodeExists
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.
Definition: argos_configuration.h:44
argos::CControllableEntity::SetController
void SetController(const std::string &str_controller_id)
Creates and assigns a controller with the given id.
Definition: controllable_entity.cpp:139
argos::CCI_Controller::ControlStep
virtual void ControlStep()
Executes a control step.
Definition: ci_controller.h:55
argos::CControllableEntity::GetController
const CCI_Controller & GetController() const
Returns a reference to the associated controller.
Definition: controllable_entity.cpp:115
argos::CEntity
The basic entity type.
Definition: entity.h:89
argos::CEntity::GetParent
CComposableEntity & GetParent()
Returns this entity's parent.
Definition: entity.cpp:83
argos::CControllableEntity::m_vecCheckedRays
std::vector< std::pair< bool, CRay3 > > m_vecCheckedRays
The list of checked rays.
Definition: controllable_entity.h:230
argos::GetNodeAttribute
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
Definition: argos_configuration.h:208
argos::CControllableEntity::CControllableEntity
CControllableEntity(CComposableEntity *pc_parent)
Class constructor.
Definition: controllable_entity.cpp:19
argos::CCI_Controller::Reset
virtual void Reset()
Resets the state of the controller to what it was right after Init() was executed.
Definition: ci_controller.h:65
argos::CControllableEntity::Sense
virtual void Sense()
Executes the CSimulatedSensor::Update() method for all associated sensors.
Definition: controllable_entity.cpp:205
argos::CCI_Controller::SetId
void SetId(const std::string &str_id)
Sets the id of the robot associated to this controller.
Definition: ci_controller.h:88
argos::CSimulatedSensor
The basic interface for a simulated sensor.
Definition: sensor.h:22
argos::CControllableEntity
An entity that contains a pointer to the user-defined controller.
Definition: controllable_entity.h:26
argos::CCI_Sensor::Init
virtual void Init(TConfigurationNode &t_node)
Initializes the sensor from the XML configuration tree.
Definition: ci_sensor.h:54
argos::CEntity::Init
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: entity.cpp:36
argos::CControllableEntity::Destroy
virtual void Destroy()
Destroys the entity, undoing whatever was done by Init() or by the standalone constructor.
Definition: controllable_entity.cpp:92
argos::CSpaceOperationAddEntity
Definition: space.h:459
argos::CCI_Controller
The basic interface for a robot controller.
Definition: ci_controller.h:30
argos::CCI_Actuator::Init
virtual void Init(TConfigurationNode &t_node)
Initializes the actuator from the XML configuration tree.
Definition: ci_actuator.h:54
argos::CSimulatedSensor::SetRobot
virtual void SetRobot(CComposableEntity &c_entity)=0
Sets the entity associated to this sensor.