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);
}
