ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
Go to the documentation of this file.
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) {
451 SetNodeAttribute(tBodyNode,
"orientation", (*pcOrientationGenerator)(bRetry));
453 pcEntity->
Init(tEntityTree);
465 if(pcEmbodiedEntity == NULL) {
468 if(pcPositionalEntity == NULL) {
469 THROW_ARGOSEXCEPTION(
"Cannot distribute entities that are not positional nor embodied, and \"" << tEntityTree.Value() <<
"\" is neither.");
473 CallEntityOperation<CSpaceOperationAddEntity, CSpace, void>(*
this, *pcEntity);
479 CallEntityOperation<CSpaceOperationAddEntity, CSpace, void>(*
this, *pcEntity);
485 CallEntityOperation<CSpaceOperationRemoveEntity, CSpace, void>(*
this, *pcEntity);
489 if(unTrials > unMaxTrials) {
492 tEntityTree.Value() <<
" with base id \"" <<
493 strBaseId <<
"\". I managed to place only " << i <<
" objects.");
506 delete pcPositionGenerator;
507 delete pcOrientationGenerator;
Real Gaussian(Real f_std_dev, Real f_mean=0.0f)
Returns a random value from a Gaussian distribution.
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.
Real GetZ() const
Returns the z coordinate of this vector.
CRadians Uniform(const CRange< CRadians > &c_range)
Returns a random value from a uniform distribution.
CControllableEntity::TVector m_vecControllableEntities
A vector of controllable entities.
TMapPerType & GetEntitiesByType(const std::string &str_type)
Returns a map containing all the objects of a given type.
virtual void RemoveControllableEntity(CControllableEntity &c_entity)
CRange< CVector3 > m_cArenaLimits
Arena limits.
void Distribute(TConfigurationNode &t_tree)
virtual void UpdateMedia()=0
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
const std::string & GetId() const
Returns the id of this entity.
CARGoSLog LOG(std::cout, SLogColor(ARGOS_LOG_ATTRIBUTE_BRIGHT, ARGOS_LOG_COLOR_GREEN))
virtual CVector3 operator()(bool b_is_retry)=0
void SetX(const Real f_x)
Sets the x coordinate of this vector.
void AddChildNode(TConfigurationNode &t_parent_node, TConfigurationNode &t_child_node)
Adds an XML node as child of another XML node.
The namespace containing all the ARGoS related code.
virtual void Destroy()
Destroys the space and all its entities.
virtual void UpdateControllableEntitiesAct()=0
bool IsMovable() const
Returns true if the entity is movable.
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.
The exception that wraps all errors in ARGoS.
virtual void UpdatePhysics()=0
RealNumberGenerator * CreateGenerator(TConfigurationNode &t_tree)
CVector3 m_cArenaSize
Arena size.
This entity is a link to a body in the physics engine.
CARGoSLog LOGERR(std::cerr, SLogColor(ARGOS_LOG_ATTRIBUTE_BRIGHT, ARGOS_LOG_COLOR_RED))
CSimulator & m_cSimulator
UniformGenerator(const CVector3 &c_min, const CVector3 &c_max)
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
CPhysicsEngine::TVector * m_ptPhysicsEngines
A pointer to the list of physics engines.
CEntity & GetRootEntity()
Returns the root entity containing this entity.
Real GetX() const
Returns the x coordinate of this vector.
virtual void UpdateControllableEntitiesSenseStep()=0
virtual void PreStep()
Executes user-defined logic right before a control step is executed.
static TYPE * New(const std::string &str_label)
Creates a new object of type TYPE
CSpace()
Class constructor.
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
bool MatchPattern(const std::string &str_input, const std::string &str_pattern)
Returns true if str_pattern is matched by str_input.
virtual bool IsCollidingWithSomething() const
Returns true if this entity is colliding with another object.
ConstantGenerator(const CVector3 &c_value)
virtual CVector3 operator()(bool b_is_retry)
void IncreaseSimulationClock(UInt32 un_increase=1)
Increases the simulation clock by the wanted value.
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception.
void AddEntity(ENTITY &c_entity)
Adds an entity of the given type.
ticpp::Iterator< ticpp::Element > TConfigurationNodeIterator
The iterator for the ARGoS configuration XML node.
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
virtual void AddControllableEntity(CControllableEntity &c_entity)
CMedium::TVector & GetMedia()
Returns the list of currently existing media.
virtual CVector3 operator()(bool b_is_retry)
virtual void PostStep()
Executes user-defined logic right after a control step is executed.
CPhysicsEngine::TVector & GetPhysicsEngines()
Returns the list of currently existing physics engines.
CRandom::CRNG * GetRNG()
Returns the random generator of the "argos" category.
GaussianGenerator(const CVector3 &c_mean, const CVector3 &c_std_dev)
CLoopFunctions & GetLoopFunctions()
Returns a reference to the loop functions associated to the current experiment.
void SetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, const T &t_value)
Sets the value of the wanted node's attribute.
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.
std::map< std::string, CAny, std::less< std::string > > TMapPerType
A map of entities indexed by type description.
CMedium::TVector * m_ptMedia
A pointer to the list of media.
std::string ToString(const T &t_value)
Converts the given parameter to a std::string.
unsigned int UInt32
32-bit unsigned integer.
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.
CEntity::TVector m_vecEntities
A vector of entities.
GridGenerator(const CVector3 c_center, const UInt32 un_layout[], const CVector3 c_distances)
CVector3 m_cArenaCenter
Arena center.
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
std::vector< CPhysicsEngine * > TVector
TMapPerTypePerId m_mapEntitiesPerTypePerId
A map of maps of all the simulated entities.
CEntity::TVector m_vecRootEntities
A vector of all the entities without a parent.
virtual void Update()
Updates the space.
virtual void AddEntityToPhysicsEngine(CEmbodiedEntity &c_entity)
Real GetY() const
Returns the y coordinate of this vector.
virtual void Reset()
Reset the space and all its entities.
An entity that contains a pointer to the user-defined controller.
CVector3 Position
The position of the anchor wrt the global coordinate system.
virtual CVector3 operator()(bool b_is_retry)
float Real
Collects all ARGoS code.
std::vector< CEntity * > TVector
A vector of entities.
unsigned long long UInt64
64-bit unsigned integer.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
virtual ~RealNumberGenerator()
UInt32 m_unSimulationClock
The current simulation clock.
virtual void Init(TConfigurationNode &t_tree)
Initializes the space using the <arena> section of the XML configuration file.
virtual CVector3 operator()(bool b_is_retry)
void GetEntitiesMatching(CEntity::TVector &t_buffer, const std::string &str_pattern)
Returns the entities matching a given pattern.
void SetY(const Real f_y)
Sets the y coordinate of this vector.