void CSimulator::InitMedia(TConfigurationNode& t_tree) {
try {
/* Cycle through the media */
TConfigurationNodeIterator itMedia;
for(itMedia = itMedia.begin(&t_tree);
itMedia != itMedia.end();
++itMedia) {
/* Create the medium */
CMedium* pcMedium = CFactory<CMedium>::New(itMedia->Value());
try {
/* Initialize the medium */
pcMedium->Init(*itMedia);
/* Check that an medium with that ID does not exist yet */
if(m_mapMedia.find(pcMedium->GetId()) == m_mapMedia.end()) {
/* Add it to the lists */
m_mapMedia[pcMedium->GetId()] = pcMedium;
m_vecMedia.push_back(pcMedium);
}
else {
/* Duplicate id -> error */
THROW_ARGOSEXCEPTION("A medium with id \"" << pcMedium->GetId() << "\" exists already. The ids must be unique!");
}
}
catch(CARGoSException& ex) {
/* Error while executing medium init, destroy what done to prevent memory leaks */
pcMedium->Destroy();
delete pcMedium;
THROW_ARGOSEXCEPTION_NESTED("Error initializing medium type \"" << itMedia->Value() << "\"", ex);
}
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize the media. Parse error in the <media> subtree.", ex);
}
}
void CSimulator::InitPhysics(TConfigurationNode& t_tree) {
try {
/* Cycle through the physics engines */
TConfigurationNodeIterator itEngines;
for(itEngines = itEngines.begin(&t_tree);
itEngines != itEngines.end();
++itEngines) {
/* Create the physics engine */
CPhysicsEngine* pcEngine = CFactory<CPhysicsEngine>::New(itEngines->Value());
try {
/* Initialize the engine */
pcEngine->Init(*itEngines);
/* Check that an engine with that ID does not exist yet */
if(m_mapPhysicsEngines.find(pcEngine->GetId()) == m_mapPhysicsEngines.end()) {
/* Add it to the lists */
m_mapPhysicsEngines[pcEngine->GetId()] = pcEngine;
m_vecPhysicsEngines.push_back(pcEngine);
}
else {
/* Duplicate id -> error */
THROW_ARGOSEXCEPTION("A physics engine with id \"" << pcEngine->GetId() << "\" exists already. The ids must be unique!");
}
}
catch(CARGoSException& ex) {
/* Error while executing engine init, destroy what done to prevent memory leaks */
pcEngine->Destroy();
delete pcEngine;
THROW_ARGOSEXCEPTION_NESTED("Error initializing physics engine type \"" << itEngines->Value() << "\"", ex);
}
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize the physics engines. Parse error in the <physics_engines> subtree.", ex);
}
}
void CRecruitmentLoopFunctions::Init(TConfigurationNode& t_node) {
try {
/* Get a pointer to the floor entity */
m_pcFloor = &(GetSpace().GetFloorEntity());
//m_pcFloor = &m_cSpace.GetFloorEntity();
/* Create a new RNG */
m_pcRNG = CRandom::CreateRNG("argos");
/* Get the number of food items we want to be scattered from XML */
TConfigurationNode& tForaging = GetNode(t_node, "foraging");
GetNodeAttribute(tForaging, "output", m_strOutput);
UInt32 NbFoodItems;
GetNodeAttribute(tForaging, "items", NbFoodItems);
GetNodeAttribute(tForaging, "radius", m_fFoodSquareRadius);
m_fFoodSquareRadius *= m_fFoodSquareRadius;
/* Distribute uniformly the items in the environment */
for(UInt32 i = 0; i < NbFoodItems; ++i) {
m_cFoodPos.push_back(
CVector2(m_pcRNG->Uniform(m_cForagingArenaSideX),
m_pcRNG->Uniform(m_cForagingArenaSideY)));
}
for(UInt32 i = 0; i < NbFoodItems; ++i) {
m_cFoodPos.push_back(
CVector2(m_pcRNG->Uniform(m_cForagingArena2SideX),
m_pcRNG->Uniform(m_cForagingArena2SideY)));
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error parsing loop functions!", ex);
}
m_cOutput.open(m_strOutput.c_str(), std::ios_base::trunc | std::ios_base::out);
m_cOutput << "# clock\tcollected_food\tavarage per 100 steps" << std::endl;
}
void CEntity::Init(TConfigurationNode& t_tree) {
try {
/*
* Set the id of the entity from XML or type description
*/
/* Was an id specified explicitly? */
if(NodeAttributeExists(t_tree, "id")) {
/* Yes, use that */
GetNodeAttribute(t_tree, "id", m_strId);
}
else {
/* No, derive it from the parent */
if(m_pcParent != NULL) {
UInt32 unIdCount = 0;
while(GetParent().HasComponent(GetTypeDescription() +
"[" + GetTypeDescription() +
"_" + ToString(unIdCount) +
"]")) {
++unIdCount;
}
m_strId = GetTypeDescription() + "_" + ToString(unIdCount);
}
else {
THROW_ARGOSEXCEPTION("Root entities must provide the identifier tag");
}
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize an entity.", ex);
}
}
void CEmbodiedEntity::Init(TConfigurationNode& t_tree) {
try {
/* Initialize base entity */
CEntity::Init(t_tree);
/* Get the position of the entity */
GetNodeAttributeOrDefault(t_tree, "position", m_cInitOriginPosition, CVector3());
/* Get the orientation of the entity */
GetNodeAttributeOrDefault(t_tree, "orientation", m_cInitOriginOrientation, CQuaternion());
/* Create origin anchor */
m_psOriginAnchor = new SAnchor("origin",
0,
CVector3(),
CQuaternion(),
m_cInitOriginPosition,
m_cInitOriginOrientation);
/* Add anchor to map and enable it */
m_mapAnchors[m_psOriginAnchor->Id] = m_psOriginAnchor;
EnableAnchor("origin");
/* Embodied entities are movable by default */
m_bMovable = true;
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize embodied entity \"" << GetContext() << GetId() << "\".", ex);
}
}
void CEntity::Init(TConfigurationNode& t_tree) {
/*
* Set an ID if it has not been set yet
* In this way, entities that are part of a composable can have an ID set by the parent
*/
if(m_strId == "") {
if(! HasParent()) {
/*
* Root-level entity
*/
try {
/* Get the id of the entity */
GetNodeAttribute(t_tree, "id", m_strId);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize an entity.", ex);
}
}
else {
/*
* Part of a component
*/
m_strId = GetParent().GetId() + "." + GetTypeDescription();
}
}
}
void CLuaController::Init(TConfigurationNode& t_tree) {
try {
/* Create RNG */
m_pcRNG = CRandom::CreateRNG("argos");
/* Load script */
std::string strScriptFileName;
GetNodeAttributeOrDefault(t_tree, "script", strScriptFileName, strScriptFileName);
if(strScriptFileName != "") {
SetLuaScript(strScriptFileName, t_tree);
if(! m_bIsOK) {
THROW_ARGOSEXCEPTION("Error loading Lua script \"" << strScriptFileName << "\": " << lua_tostring(m_ptLuaState, -1));
}
}
else {
/* Create a new Lua stack */
m_ptLuaState = luaL_newstate();
/* Load the Lua libraries */
luaL_openlibs(m_ptLuaState);
/* Create and set Lua state */
CreateLuaState();
SensorReadingsToLuaState();
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing Lua controller", ex);
}
}
void CBuzzControllerEFootBot::Init(TConfigurationNode& t_node) {
try {
/* Get pointers to devices */
try {
m_pcWheels = GetActuator<CCI_DifferentialSteeringActuator>("differential_steering");
m_sWheelTurningParams.Init(GetNode(t_node, "wheel_turning"));
}
catch(CARGoSException& ex) {}
try {
m_pcLEDs = GetActuator<CCI_LEDsActuator>("leds");
}
catch(CARGoSException& ex) {}
try {
m_pcProximity = GetSensor<CCI_EFootBotProximitySensor>("efootbot_proximity");
}
catch(CARGoSException& ex) {}
try {
m_batterySensor = GetSensor<CCI_BatterySensor>("battery");
}
catch(CARGoSException& ex) {}
/* Initialize the rest */
CBuzzController::Init(t_node);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing the Buzz controller for the foot-bot", ex);
}
}
void CFootBotForaging::Init(TConfigurationNode& t_node) {
try {
/*
* Initialize sensors/actuators
*/
m_pcWheels = GetActuator<CCI_DifferentialSteeringActuator>("differential_steering");
m_pcLEDs = GetActuator<CCI_LEDsActuator >("leds" );
m_pcRABA = GetActuator<CCI_RangeAndBearingActuator >("range_and_bearing" );
m_pcRABS = GetSensor <CCI_RangeAndBearingSensor >("range_and_bearing" );
m_pcProximity = GetSensor <CCI_FootBotProximitySensor >("footbot_proximity" );
m_pcLight = GetSensor <CCI_FootBotLightSensor >("footbot_light" );
m_pcGround = GetSensor <CCI_FootBotMotorGroundSensor >("footbot_motor_ground" );
/*
* Parse XML parameters
*/
/* Diffusion algorithm */
m_sDiffusionParams.Init(GetNode(t_node, "diffusion"));
/* Wheel turning */
m_sWheelTurningParams.Init(GetNode(t_node, "wheel_turning"));
/* Controller state */
m_sStateData.Init(GetNode(t_node, "state"));
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing the foot-bot foraging controller for robot \"" << GetId() << "\"", ex);
}
/*
* Initialize other stuff
*/
/* Create a random number generator. We use the 'argos' category so
that creation, reset, seeding and cleanup are managed by ARGoS. */
m_pcRNG = CRandom::CreateRNG("argos");
Reset();
}
void CFootBotProximityDefaultSensor::SetRobot(CComposableEntity& c_entity) {
try {
m_pcProximityImpl->SetRobot(c_entity);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Can't set robot for the foot-bot proximity default sensor", ex);
}
}
void CSimulator::InitSpace(TConfigurationNode& t_tree) {
try {
m_pcSpace->Init(t_tree);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize the space.", ex);
}
}
void CQuadRotorSpeedDefaultActuator::Init(TConfigurationNode& t_tree) {
try {
CCI_QuadRotorSpeedActuator::Init(t_tree);
Reset();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Initialization error in quadrotor speed actuator.", ex);
}
}
void CRadiosDefaultActuator::Init(TConfigurationNode& t_tree) {
try {
/* Parent class init */
CCI_RadiosActuator::Init(t_tree);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing the radios default actuator", ex);
}
}
void CLEDEntity::RemoveFromMedium() {
try {
GetMedium().RemoveEntity(*this);
m_pcMedium = NULL;
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Can't remove LED entity \"" << GetId() << "\" from medium.", ex);
}
}
void CFootBotFlocking::SFlockingInteractionParams::Init(TConfigurationNode& t_node) {
try {
GetNodeAttribute(t_node, "target_distance", TargetDistance);
GetNodeAttribute(t_node, "gain", Gain);
GetNodeAttribute(t_node, "exponent", Exponent);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing controller flocking parameters.", ex);
}
}
void CEntity::Init(TConfigurationNode& t_tree)
{
try {
/* Get the id of the entity */
GetNodeAttribute(t_tree, "id", m_strId);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize an entity.", ex);
}
}
void CFootBotLightRotZOnlySensor::SetRobot(CComposableEntity& c_entity) {
try {
m_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body"));
m_pcControllableEntity = &(c_entity.GetComponent<CControllableEntity>("controller"));
m_pcLightEntity = &(c_entity.GetComponent<CLightSensorEquippedEntity>("light_sensors"));
m_pcLightEntity->Enable();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Can't set robot for the foot-bot light default sensor", ex);
}
}
void CEPuckRangeAndBearingSensor::Init(TConfigurationNode& t_tree) {
try {
CCI_EPuckRangeAndBearingSensor::Init(t_tree);
/* Show rays? */
GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
GetNodeAttributeOrDefault(t_tree, "check_occlusions", m_bCheckOcclusions, m_bCheckOcclusions);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Initialization error in e-puck range and bearing sensor.", ex);
}
}
void CLEDEntity::Init(TConfigurationNode& t_tree) {
try {
/* Parse XML */
CPositionalEntity::Init(t_tree);
GetNodeAttribute(t_tree, "color", m_cInitColor);
m_cColor = m_cInitColor;
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error while initializing led entity", ex);
}
}
void CSimulator::InitControllers(TConfigurationNode& t_tree) {
/*
* Go through controllers, loading the library of each of them
* and storing type, id and XML tree of each of them for later use
*/
if(! t_tree.NoChildren()) {
try {
std::string strLibrary;
std::string strId;
TConfigurationNodeIterator it;
for(it = it.begin(&t_tree);
it != it.end(); ++it) {
/* Get controller id */
try {
GetNodeAttribute(*it, "id", strId);
}
catch(CARGoSException& ex) {
std::string strValue;
it->GetValue(&strValue);
THROW_ARGOSEXCEPTION_NESTED("Controller type \"" << strValue << "\" has no assigned id.", ex);
}
/* Bomb out if id is already in map */
if(m_mapControllerConfig.find(strId) != m_mapControllerConfig.end()) {
THROW_ARGOSEXCEPTION("Controller id \"" << strId << "\" duplicated");
}
/* Optionally, process "library" attribute if present */
if(NodeAttributeExists(*it, "library")) {
/* Get library name */
GetNodeAttribute(*it, "library", strLibrary);
/* Load library */
CDynamicLoading::LoadLibrary(strLibrary);
}
/* Store XML info in map by id */
m_mapControllerConfig.insert(std::pair<std::string, TConfigurationNode*>(strId, &(*it)));
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing controllers", ex);
}
}
}
void CLEDsDefaultActuator::Init(TConfigurationNode& t_tree) {
try {
CCI_LEDsActuator::Init(t_tree);
std::string strMedium;
GetNodeAttribute(t_tree, "medium", strMedium);
m_pcLEDMedium = &CSimulator::GetInstance().GetMedium<CLEDMedium>(strMedium);
m_pcLEDEquippedEntity->AddToMedium(*m_pcLEDMedium);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing the LEDs default actuator", ex);
}
}
void CFootBotForaging::SDiffusionParams::Init(TConfigurationNode& t_node) {
try {
CRange<CDegrees> cGoStraightAngleRangeDegrees(CDegrees(-10.0f), CDegrees(10.0f));
GetNodeAttribute(t_node, "go_straight_angle_range", cGoStraightAngleRangeDegrees);
GoStraightAngleRange.Set(ToRadians(cGoStraightAngleRangeDegrees.GetMin()),
ToRadians(cGoStraightAngleRangeDegrees.GetMax()));
GetNodeAttribute(t_node, "delta", Delta);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing controller diffusion parameters.", ex);
}
}
void CBuzzController::Init(TConfigurationNode& t_node) {
try {
/* Get pointers to devices */
m_pcRABA = GetActuator<CCI_RangeAndBearingActuator>("range_and_bearing");
m_pcRABS = GetSensor <CCI_RangeAndBearingSensor >("range_and_bearing");
try {
m_pcPos = GetSensor <CCI_PositioningSensor>("positioning");
}
catch(CARGoSException& ex) {}
/* Get the script name */
std::string strBCFName;
GetNodeAttributeOrDefault(t_node, "bytecode_file", strBCFName, strBCFName);
/* Get the script name */
std::string strDbgFName;
GetNodeAttributeOrDefault(t_node, "debug_file", strDbgFName, strDbgFName);
/* Initialize the rest */
bool bIDSuccess = false;
m_unRobotId = 0;
/* Find Buzz ID */
size_t tStartPos = GetId().find_last_of("_");
if(tStartPos != std::string::npos){
/* Checks for ID after last "_" ie. footbot_group3_10 -> 10 */
m_unRobotId = FromString<UInt16>(GetId().substr(tStartPos+1));
bIDSuccess = true;
}
/* FromString() returns 0 if passed an invalid string */
if(!m_unRobotId || !bIDSuccess){
/* Checks for ID after first number footbot_simulated10 -> 10 */
tStartPos = GetId().find_first_of("0123456789");
if(tStartPos != std::string::npos){
m_unRobotId = FromString<UInt16>(GetId().substr(tStartPos));
bIDSuccess = true;
}
}
if(!bIDSuccess) {
THROW_ARGOSEXCEPTION("Error in finding Buzz ID from name \"" << GetId() << "\"");
}
if(strBCFName != "" && strDbgFName != "")
SetBytecode(strBCFName, strDbgFName);
else
m_tBuzzVM = buzzvm_new(m_unRobotId);
UpdateSensors();
/* Set initial robot message (id and then all zeros) */
CByteArray cData;
cData << m_tBuzzVM->robot;
while(cData.Size() < m_pcRABA->GetSize()) cData << static_cast<UInt8>(0);
m_pcRABA->SetData(cData);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing the Buzz controller", ex);
}
}
bool CDirectionalLEDEntityGridUpdater::operator()(CDirectionalLEDEntity& c_entity) {
try {
/* Calculate the position of the LED in the space hash */
m_cGrid.PositionToCell(m_nI, m_nJ, m_nK, c_entity.GetPosition());
/* Update the corresponding cell */
m_cGrid.UpdateCell(m_nI, m_nJ, m_nK, c_entity);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("While updating the directional LED grid for LED \"" <<
c_entity.GetContext() + c_entity.GetId() << "\"", ex);
}
/* Continue with the other entities */
return true;
}
void CDirectionalLEDEntity::Init(TConfigurationNode& t_tree) {
try {
/* Parse XML */
CPositionalEntity::Init(t_tree);
GetNodeAttributeOrDefault(t_tree, "color", m_cInitColor, m_cInitColor);
m_cColor = m_cInitColor;
CDegrees cObservableAngle;
GetNodeAttribute(t_tree, "observable_angle", cObservableAngle);
m_cObservableAngle = ToRadians(cObservableAngle);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error while initializing directional led entity", ex);
}
}
void CSimulator::InitLoopFunctions(TConfigurationNode& t_tree) {
try {
std::string strLibrary, strLabel;
GetNodeAttributeOrDefault(t_tree, "library", strLibrary, strLibrary);
GetNodeAttribute(t_tree, "label", strLabel);
if(! strLibrary.empty()) {
CDynamicLoading::LoadLibrary(strLibrary);
}
m_pcLoopFunctions = CFactory<CLoopFunctions>::New(strLabel);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing loop functions", ex);
}
}
void CSimulator::InitVisualization(TConfigurationNode& t_tree) {
try {
/* Consider only the first visualization */
TConfigurationNodeIterator itVisualization;
itVisualization = itVisualization.begin(&t_tree);
/* Create the visualization */
m_pcVisualization = CFactory<CVisualization>::New(itVisualization->Value());
/* Initialize the visualization */
m_pcVisualization->Init(*itVisualization);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize the visualization. Parse error in the <visualization> subtree.", ex);
}
}
void CSimulator::InitMedia2() {
try {
/* Cycle through the media */
CMedium::TMap::iterator it;
for(it = m_mapMedia.begin(); it != m_mapMedia.end(); ++it) {
CMedium& cMedium = *(it->second);
try {
/* Initialize the medium */
cMedium.PostSpaceInit();
}
catch(CARGoSException& ex) {
/* Error while executing medium post space init, destroy what done to prevent memory leaks */
std::ostringstream ossMsg;
ossMsg << "Error executing post-space initialization of medium \"" << cMedium.GetId() << "\"";
cMedium.Destroy();
THROW_ARGOSEXCEPTION_NESTED(ossMsg.str(), ex);
}
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize the media. Parse error in the <media> subtree.", ex);
}
}
void CBluebotEntity::Init(TConfigurationNode& t_tree) {
try {
/* Init parent */
CEntity::Init(t_tree);
/* Init components */
m_pcEmbodiedEntity->Init(t_tree);
m_pcControllableEntity->Init(t_tree);
m_pcWheeledEntity->Init(t_tree);
UpdateComponents();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
}
}
void CSimulator::InitPhysics2() {
try {
/* Cycle through the physics engines */
CPhysicsEngine::TMap::iterator it;
for(it = m_mapPhysicsEngines.begin(); it != m_mapPhysicsEngines.end(); ++it) {
CPhysicsEngine& cPhysicsEngine = *(it->second);
try {
/* Initialize the physicsengine */
cPhysicsEngine.PostSpaceInit();
}
catch(CARGoSException& ex) {
/* Error while executing physicsengine post space init, destroy what done to prevent memory leaks */
std::ostringstream ossMsg;
ossMsg << "Error executing post-space initialization of physics engine \"" << cPhysicsEngine.GetId() << "\"";
cPhysicsEngine.Destroy();
THROW_ARGOSEXCEPTION_NESTED(ossMsg.str(), ex);
}
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize the physics engines. Parse error in the <physics_engines> subtree.", ex);
}
}
