11 #include <argos3/core/utility/string_utilities.h>
12 #include <argos3/core/utility/math/range.h>
13 #include <argos3/core/utility/logging/argos_log.h>
14 #include <argos3/core/utility/math/rng.h>
15 #include <argos3/core/simulator/simulator.h>
16 #include <argos3/core/simulator/entity/composable_entity.h>
17 #include <argos3/core/simulator/entity/positional_entity.h>
18 #include <argos3/core/simulator/loop_functions.h>
29 m_unSimulationClock(0),
30 m_pcFloorEntity(NULL),
31 m_ptPhysicsEngines(NULL),
51 for(itArenaItem = itArenaItem.begin(&t_tree);
52 itArenaItem != itArenaItem.end();
54 if(itArenaItem->Value() !=
"distribute") {
56 pcEntity->
Init(*itArenaItem);
57 CallEntityOperation<CSpaceOperationAddEntity, CSpace, void>(*
this, *pcEntity);
61 for(itArenaItem = itArenaItem.begin(&t_tree);
62 itArenaItem != itArenaItem.end();
64 if(itArenaItem->Value() ==
"distribute") {
88 CallEntityOperation<CSpaceOperationRemoveEntity, CSpace, void>(*
this, *
m_vecRootEntities.back());
96 const std::string& str_pattern) {
100 t_buffer.push_back(*it);
111 return itEntities->second;
176 if(vecPotentialEngines.empty()) {
182 for(
size_t i = 0; i < vecPotentialEngines.size(); ++i) {
183 bAdded |= vecPotentialEngines[i]->AddEntity(*pcToAdd);
190 else if(vecPotentialEngines.size() == 1) {
192 if(!vecPotentialEngines[0]->
AddEntity(*pcToAdd)) {
198 for(
size_t i = 0; i < vecPotentialEngines.size(); ++i) {
199 if(vecPotentialEngines[i]->
AddEntity(*pcToAdd))
return;
247 return CVector3(fRandX, fRandY, fRandZ);
259 m_cStdDev(c_std_dev) {}
276 m_cDistances(c_distances),
277 m_unNumEntityPlaced(0) {
278 m_unLayout[0] = un_layout[0];
279 m_unLayout[1] = un_layout[1];
280 m_unLayout[2] = un_layout[2];
282 if( m_unLayout[0] == 0 || m_unLayout[1] == 0 || m_unLayout[2] == 0 ) {
283 THROW_ARGOSEXCEPTION(
"'layout' values (distribute position, method 'grid') must all be different than 0");
292 if(m_unNumEntityPlaced < m_unLayout[0] * m_unLayout[1] * m_unLayout[2]) {
293 cReturn.
SetX(m_cCenter.
GetX() + ( m_unLayout[0] - 1 ) * m_cDistances.
GetX() * 0.5 - ( m_unNumEntityPlaced % m_unLayout[0] ) * m_cDistances.
GetX());
294 cReturn.
SetY(m_cCenter.
GetY() + ( m_unLayout[1] - 1 ) * m_cDistances.
GetY() * 0.5 - ( m_unNumEntityPlaced / m_unLayout[0] ) % m_unLayout[1] * m_cDistances.
GetY());
295 cReturn.
SetZ(m_cCenter.
GetZ() + ( m_unLayout[2] - 1 ) * m_cDistances.
GetZ() * 0.5 - ( m_unNumEntityPlaced / ( m_unLayout[0] * m_unLayout[1] ) ) * m_cDistances.
GetZ());
296 ++m_unNumEntityPlaced;
299 THROW_ARGOSEXCEPTION(
"Distribute position, method 'grid': trying to place more entities than allowed "
300 "by the 'layout', check your 'quantity' tag");
309 UInt32 m_unNumEntityPlaced;
316 std::string strMethod;
318 if(strMethod ==
"uniform") {
322 if(! (cMin <= cMax)) {
323 THROW_ARGOSEXCEPTION(
"Uniform generator: the min is not less than or equal to max: " << cMin <<
" / " << cMax);
327 else if(strMethod ==
"gaussian") {
333 else if(strMethod ==
"constant") {
338 else if(strMethod ==
"grid") {
343 std::string strLayout;
345 ParseValues<UInt32> (strLayout, 3, unLayout,
',');
356 static CEmbodiedEntity* GetEmbodiedEntity(CEntity* pc_entity) {
358 CEmbodiedEntity* pcEmbodiedTest =
dynamic_cast<CEmbodiedEntity*
>(pc_entity);
359 if(pcEmbodiedTest != NULL) {
360 return pcEmbodiedTest;
363 CComposableEntity* pcComposableTest =
dynamic_cast<CComposableEntity*
>(pc_entity);
364 if(pcComposableTest != NULL) {
365 if(pcComposableTest->HasComponent(
"body")) {
366 return &(pcComposableTest->GetComponent<CEmbodiedEntity>(
"body"));
376 static CPositionalEntity* GetPositionalEntity(CEntity* pc_entity) {
378 CPositionalEntity* pcPositionalTest =
dynamic_cast<CPositionalEntity*
>(pc_entity);
379 if(pcPositionalTest != NULL) {
380 return pcPositionalTest;
383 CComposableEntity* pcComposableTest =
dynamic_cast<CComposableEntity*
>(pc_entity);
384 if(pcComposableTest != NULL) {
385 if(pcComposableTest->HasComponent(
"position")) {
386 return &(pcComposableTest->GetComponent<CPositionalEntity>(
"position"));
400 cPositionNode =
GetNode(t_tree,
"position");
402 cOrientationNode =
GetNode(t_tree,
"orientation");
404 cEntityNode =
GetNode(t_tree,
"entity");
419 itEntity = itEntity.begin(&cEntityNode);
420 if(itEntity == itEntity.end()) {
424 std::string strBaseId;
427 for(
UInt32 i = 0; i < unQuantity; ++i) {
461 if(pcPositionalEntity != NULL) {
465 SetNodeAttribute(tEntityTree,
"orientation", (*pcOrientationGenerator)(bRetry));
467 pcEntity->
Init(tEntityTree);
469 CallEntityOperation<CSpaceOperationAddEntity, CSpace, void>(*
this, *pcEntity);
484 SetNodeAttribute(tBodyNode,
"orientation", (*pcOrientationGenerator)(bRetry));
486 pcEntity->
Init(tEntityTree);
489 if(pcEmbodiedEntity != NULL) {
492 CallEntityOperation<CSpaceOperationAddEntity, CSpace, void>(*
this, *pcEntity);
498 CallEntityOperation<CSpaceOperationRemoveEntity, CSpace, void>(*
this, *pcEntity);
502 if(unTrials > unMaxTrials) {
505 tEntityTree.Value() <<
" with base id \"" <<
506 strBaseId <<
"\". I managed to place only " << i <<
" objects.");
516 THROW_ARGOSEXCEPTION(
"Cannot distribute entities that are not positional nor embodied, and \"" << tEntityTree.Value() <<
"\" is neither.");
523 delete pcPositionGenerator;
524 delete pcOrientationGenerator;
CVector3 m_cArenaCenter
Arena center.
virtual CVector3 operator()(bool b_is_retry)
void IncreaseSimulationClock(UInt32 un_increase=1)
Increases the simulation clock by the wanted value.
std::vector< CEntity * > TVector
A vector of entities.
An entity that contains a pointer to the user-defined controller.
void AddEntity(ENTITY &c_entity)
Adds an entity of the given type.
bool MatchPattern(const std::string &str_input, const std::string &str_pattern)
Returns true if str_pattern is matched by str_input.
virtual void RemoveControllableEntity(CControllableEntity &c_entity)
CPhysicsEngine::TVector & GetPhysicsEngines()
Returns the list of currently existing physics engines.
void GetNodeAttributeOrDefault(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer, const T &t_default)
Returns the value of a node's attribute, or the passed default value.
virtual CVector3 operator()(bool b_is_retry)
CMedium::TVector * m_ptMedia
A pointer to the list of media.
virtual CVector3 operator()(bool b_is_retry)=0
void Distribute(TConfigurationNode &t_tree)
float Real
Collects all ARGoS code.
CARGoSLog LOG(std::cout, SLogColor(ARGOS_LOG_ATTRIBUTE_BRIGHT, ARGOS_LOG_COLOR_GREEN))
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
CLoopFunctions & GetLoopFunctions()
Returns a reference to the loop functions associated to the current experiment.
virtual void AddControllableEntity(CControllableEntity &c_entity)
CEntity::TVector m_vecEntities
A vector of entities.
virtual CVector3 operator()(bool b_is_retry)
Real GetX() const
Returns the x coordinate of this vector.
CRandom::CRNG * GetRNG()
Returns the random generator of the "argos" category.
TMapPerTypePerId m_mapEntitiesPerTypePerId
A map of maps of all the simulated entities.
Real GetY() const
Returns the y coordinate of this vector.
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.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
virtual ~RealNumberGenerator()
CEntity::TVector m_vecRootEntities
A vector of all the entities without a parent.
virtual void Destroy()
Destroys the space and all its entities.
This entity is a link to a body in the physics engine.
TMapPerType & GetEntitiesByType(const std::string &str_type)
Returns a map containing all the objects of a given type.
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception...
CSimulator & m_cSimulator
CEntity & GetRootEntity()
Returns the root entity containing this entity.
GridGenerator(const CVector3 c_center, const UInt32 un_layout[], const CVector3 c_distances)
GaussianGenerator(const CVector3 &c_mean, const CVector3 &c_std_dev)
UniformGenerator(const CVector3 &c_min, const CVector3 &c_max)
void GetEntitiesMatching(CEntity::TVector &t_buffer, const std::string &str_pattern)
Returns the entities matching a given pattern.
std::vector< CPhysicsEngine * > TVector
virtual void PostStep()
Executes user-defined logic right after a control step is executed.
CRange< CVector3 > m_cArenaLimits
Arena limits.
unsigned int UInt32
32-bit unsigned integer.
CRadians Uniform(const CRange< CRadians > &c_range)
Returns a random value from a uniform distribution.
void AddChildNode(TConfigurationNode &t_parent_node, TConfigurationNode &t_child_node)
Adds an XML node as child of another XML node.
virtual CVector3 operator()(bool b_is_retry)
static TYPE * New(const std::string &str_label)
Creates a new object of type TYPE
void SetX(const Real f_x)
Sets the x coordinate of this vector.
std::map< std::string, CAny, std::less< std::string > > TMapPerType
A map of entities indexed by type description.
CPhysicsEngine::TVector * m_ptPhysicsEngines
A pointer to the list of physics engines.
CMedium::TVector & GetMedia()
Returns the list of currently existing media.
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 Init(TConfigurationNode &t_tree)
Initializes the space using the
section of the XML configuration file.
CARGoSLog LOGERR(std::cerr, SLogColor(ARGOS_LOG_ATTRIBUTE_BRIGHT, ARGOS_LOG_COLOR_RED))
ticpp::Iterator< ticpp::Element > TConfigurationNodeIterator
The iterator for the ARGoS configuration XML node.
std::string ToString(const T &t_value)
Converts the given parameter to a std::string.
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.
void SetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, const T &t_value)
Sets the value of the wanted node's attribute.
virtual void PreStep()
Executes user-defined logic right before a control step is executed.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
virtual void UpdateMedia()=0
void SetY(const Real f_y)
Sets the y coordinate of this vector.
virtual bool IsCollidingWithSomething() const
Returns true if this entity is colliding with another object.
CSpace()
Class constructor.
virtual void UpdateControllableEntitiesAct()=0
CVector3 m_cArenaSize
Arena size.
Real Gaussian(Real f_std_dev, Real f_mean=0.0f)
Returns a random value from a Gaussian distribution.
CVector3 Position
The position of the anchor wrt the global coordinate system.
unsigned long long UInt64
64-bit unsigned integer.
virtual void Update()
Updates the space.
virtual void AddEntityToPhysicsEngine(CEmbodiedEntity &c_entity)
CControllableEntity::TVector m_vecControllableEntities
A vector of controllable entities.
const std::string & GetId() const
Returns the id of this entity.
bool IsMovable() const
Returns true if the entity is movable.
virtual void Reset()
Reset the space and all its entities.
The namespace containing all the ARGoS related code.
Real GetZ() const
Returns the z coordinate of this vector.
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
virtual void UpdatePhysics()=0
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
RealNumberGenerator * CreateGenerator(TConfigurationNode &t_tree)
ConstantGenerator(const CVector3 &c_value)
UInt32 m_unSimulationClock
The current simulation clock.
virtual void UpdateControllableEntitiesSenseStep()=0
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.