void CFootBotUN::initLocalNavigation(TConfigurationNode& t_tree) { hlAgent.Init(t_tree); hlAgent.axisLength = axisLength; orcaAgent.Init(t_tree); orcaAgent.axisLength = axisLength; hrvoAgent.Init(t_tree); hrvoAgent.axisLength = axisLength; localNavigationType = "HL"; if (NodeExists(t_tree, "local_navigation")) { TConfigurationNode node = GetNode(t_tree, "local_navigation"); if (node.HasAttribute("type")) GetNodeAttribute(node, "type", localNavigationType); } if (localNavigationType == "HL") { localNavigationIndex = 2; setAgent(hlAgent); } else if (localNavigationType == "ORCA") { localNavigationIndex = 0; setAgent(orcaAgent); } else if (localNavigationType == "HRVO") { localNavigationIndex = 0; setAgent(hrvoAgent); } else { throw "Navigation type not defined!"; return; } printf("INIT local nav %.3f %.3f", axisLength, agent->axisLength); }
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 CProximitySensorEquippedEntity::Init(TConfigurationNode& t_tree) { try { /* * Parse basic entity stuff */ CEntity::Init(t_tree); /* * Parse proximity sensors */ /* Not adding any sensor is a fatal error */ if(t_tree.NoChildren()) { THROW_ARGOSEXCEPTION("No sensors defined"); } /* Go through children */ TConfigurationNodeIterator it; for(it = it.begin(&t_tree); it != it.end(); ++it) { std::string strAnchorId; GetNodeAttribute(*it, "anchor", strAnchorId); /* * NOTE: here we get a reference to the embodied entity * This line works under the assumption that: * 1. the ProximitySensorEquippedEntity has a parent; * 2. the parent has a child whose id is "body" * 3. the "body" is an embodied entity * If any of the above is false, this line will bomb out. */ CEmbodiedEntity& cBody = GetParent().GetComponent<CEmbodiedEntity>("body"); if(it->Value() == "sensor") { CVector3 cOff, cDir; Real fRange; GetNodeAttribute(*it, "offset", cOff); GetNodeAttribute(*it, "direction", cDir); GetNodeAttribute(*it, "range", fRange); AddSensor(cOff, cDir, fRange, cBody.GetAnchor(strAnchorId)); } else if(it->Value() == "ring") { CVector3 cRingCenter; GetNodeAttributeOrDefault(t_tree, "center", cRingCenter, cRingCenter); Real fRadius; GetNodeAttribute(t_tree, "radius", fRadius); CDegrees cRingStartAngleDegrees; GetNodeAttributeOrDefault(t_tree, "start_angle", cRingStartAngleDegrees, cRingStartAngleDegrees); CRadians cRingStartAngleRadians = ToRadians(cRingStartAngleDegrees); Real fRange; GetNodeAttribute(t_tree, "range", fRange); UInt32 unNumSensors; GetNodeAttribute(t_tree, "num_sensors", unNumSensors); AddSensorRing(cRingCenter, fRadius, cRingStartAngleRadians, fRange, unNumSensors, cBody.GetAnchor(strAnchorId)); } else { THROW_ARGOSEXCEPTION("Unrecognized tag \"" << it->Value() << "\""); } } } catch(CARGoSException& ex) { THROW_ARGOSEXCEPTION_NESTED("Initialization error in proximity sensor equipped entity", ex); } }
void Agent::Init(TConfigurationNode& t_tree) { maxSpeed = MAX_SPEED; leftWheelSpeed = 0.0; rightWheelSpeed = 0.0; linearSpeedIsContinuos = DEFAULT_LINEAR_SPEED_CONTINUOUS; maxRotationSpeed = DEFAULT_MAX_ANGULAR_SPEED; radius = RADIUS; optimalSpeed = DEFAULT_OPTIMAL_SPEED; rotationTau = DEFAULT_ROTATION_TAU; socialRadius[HUMAN] = DEFAULT_HUMAN_SOCIAL_RADIUS; socialRadius[FOOTBOT] = DEFAULT_FOOTBOT_SOCIAL_RADIUS; socialRadius[OBSTACLE] = DEFAULT_OBSTACLE_SOCIAL_RADIUS; safetyMargin = 0.1; socialMargin = 0.1; ratioOfSocialRadiusForSensing = DEFAULT_SOCIAL_SENSING_RATIO; horizon = DEFAULT_HORIZON; if (NodeExists(t_tree, "mobility")) { TConfigurationNode node = GetNode(t_tree, "mobility"); if (node.HasAttribute("continuous")) GetNodeAttribute(node, "continuous", linearSpeedIsContinuos); if (node.HasAttribute("rotation_max_speed")) GetNodeAttribute(node, "rotation_max_speed", maxRotationSpeed); } std::string text; if (NodeExists(t_tree, "horizon")) { GetNodeText(GetNode(t_tree, "horizon"), text); sscanf(text.c_str(), "%f", &horizon); } if (NodeExists(t_tree, "optimalSpeed")) { GetNodeText(GetNode(t_tree, "optimalSpeed"), text); sscanf(text.c_str(), "%f", &optimalSpeed); } if (NodeExists(t_tree, "maxSpeed")) { GetNodeText(GetNode(t_tree, "maxSpeed"), text); sscanf(text.c_str(), "%f", &maxSpeed); } if (NodeExists(t_tree, "safetyMargin")) { GetNodeText(GetNode(t_tree, "safetyMargin"), text); sscanf(text.c_str(), "%f", &safetyMargin); safetyMargin = fmax(0.01, safetyMargin); } if (NodeExists(t_tree, "socialMargin")) { GetNodeText(GetNode(t_tree, "socialMargin"), text); sscanf(text.c_str(), "%f", &socialMargin); //safetyMargin=fmax(safetyMargin,socialMargin); } if (NodeExists(t_tree, "socialRadius")) { TConfigurationNode node = GetNode(t_tree, "socialRadius"); std::string text = ""; if (node.HasAttribute("footbot")) { GetNodeAttribute(node, "foobot", text); Real d; scanf(text.c_str(), "%f", &d); socialRadius[FOOTBOT] = d; } if (node.HasAttribute("human")) { GetNodeAttribute(node, "human", text); Real d; scanf(text.c_str(), "%f", &d); socialRadius[HUMAN] = d; } if (node.HasAttribute("obstacle")) { GetNodeAttribute(node, "obstacle", text); Real d; scanf(text.c_str(), "%f", &d); socialRadius[OBSTACLE] = d; } } if (NodeExists(t_tree, "rotationTau")) { GetNodeText(GetNode(t_tree, "rotationTau"), text); sscanf(text.c_str(), "%f", &rotationTau); rotationTau = fmax(0.1, rotationTau); } printf("Agent Init: sM %.2f, SM %.2f, horizon %.2f optimal speed %.2f max speed %.2f rotation tau %.2f max rotation speed %.2f\n", safetyMargin, socialMargin, horizon, optimalSpeed, maxSpeed, rotationTau, maxRotationSpeed); //DEBUG_CONTROLLER("Mobility Settings: maxRotationSpeed %.2f, linearSpeedIsContinuos %d \r\n",maxRotationSpeed,linearSpeedIsContinuos); }