CQTOpenGLUserFunctions* CQTOpenGLMainWindow::CreateUserFunctions(TConfigurationNode& t_tree) {
/* Parse XML for user functions */
if(NodeExists(t_tree, "user_functions")) {
/* Use the passed user functions */
/* Get data from XML */
TConfigurationNode tNode = GetNode(t_tree, "user_functions");
std::string strLabel, strLibrary;
GetNodeAttribute(tNode, "label", strLabel);
GetNodeAttribute(tNode, "library", strLibrary);
/* Load the library taking care of the $ARGOSINSTALLDIR variable */
void* ptUserFunctions = ::dlopen(ExpandARGoSInstallDir(strLibrary).c_str(),
RTLD_LOCAL | RTLD_LAZY);
if(ptUserFunctions == NULL) {
THROW_ARGOSEXCEPTION("Failed opening QTOpenGL user function library \""
<< strLibrary << "\": " << dlerror()
<< std::endl);
}
/* Create the user functions */
if(mapQTOpenGLUserFunctionFactory.find(strLabel) == mapQTOpenGLUserFunctionFactory.end()) {
THROW_ARGOSEXCEPTION("Unknown QTOpenGL user function type \"" << strLabel
<< "\", probably your user functions have been registered with a different name."
<< std::endl);
}
return mapQTOpenGLUserFunctionFactory[strLabel]();
}
else {
/* Use standard (empty) user functions */
return new CQTOpenGLUserFunctions;
}
}
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::Init() {
/* General configuration */
InitFramework(GetNode(m_tConfigurationRoot, "framework"));
/* Initialize controllers */
InitControllers(GetNode(m_tConfigurationRoot, "controllers"));
/* Create loop functions */
if(NodeExists(m_tConfigurationRoot, "loop_functions")) {
/* User specified a loop_functions section in the XML */
InitLoopFunctions(GetNode(m_tConfigurationRoot, "loop_functions"));
}
else {
/* No loop_functions in the XML */
m_pcLoopFunctions = new CLoopFunctions;
}
/* Physics engines */
InitPhysics(GetNode(m_tConfigurationRoot, "physics_engines"));
/* Media */
InitMedia(GetNode(m_tConfigurationRoot, "media"));
/* Space */
InitSpace(GetNode(m_tConfigurationRoot, "arena"));
/* Call user init function */
if(NodeExists(m_tConfigurationRoot, "loop_functions")) {
m_pcLoopFunctions->Init(GetNode(m_tConfigurationRoot, "loop_functions"));
}
/* Physics engines */
InitPhysics2();
/* Media */
InitMedia2();
/* Initialise visualization */
TConfigurationNodeIterator itVisualization;
if(NodeExists(m_tConfigurationRoot, "visualization") &&
((itVisualization = itVisualization.begin(&GetNode(m_tConfigurationRoot, "visualization"))) != itVisualization.end())) {
InitVisualization(GetNode(m_tConfigurationRoot, "visualization"));
}
else {
LOG << "[INFO] No visualization selected." << std::endl;
m_pcVisualization = new CDefaultVisualization();
}
/* Start profiling, if needed */
if(IsProfiling()) {
m_pcProfiler->Start();
}
}
void CNeighboursTableModel::UpdateNeighbourData(CG2Node* pNode)
{
//qDebug() << "UpdateNeighboursData";
if( !Network.m_pSection.tryLock() )
return; // forget it
if( NodeExists(pNode) )
UpdateNode(pNode);
Network.m_pSection.unlock();
}
void CBoxEntity::Init(TConfigurationNode& t_tree) {
try {
/* Init parent */
CComposableEntity::Init(t_tree);
/* Parse XML to get the size */
GetNodeAttribute(t_tree, "size", m_cSize);
/* Parse XML to get the movable attribute */
bool bMovable;
GetNodeAttribute(t_tree, "movable", bMovable);
if(bMovable) {
/* Parse XML to get the mass */
GetNodeAttribute(t_tree, "mass", m_fMass);
}
else {
m_fMass = 0.0f;
}
/* Create embodied entity using parsed data */
m_pcEmbodiedEntity = new CEmbodiedEntity(this);
AddComponent(*m_pcEmbodiedEntity);
m_pcEmbodiedEntity->Init(GetNode(t_tree, "body"));
m_pcEmbodiedEntity->SetMovable(bMovable);
/* Init LED equipped entity component */
m_pcLEDEquippedEntity = new CLEDEquippedEntity(this,
m_pcEmbodiedEntity);
AddComponent(*m_pcLEDEquippedEntity);
if(NodeExists(t_tree, "leds")) {
/* Create LED equipped entity
* NOTE: the LEDs are not added to the medium yet
*/
m_pcLEDEquippedEntity->Init(GetNode(t_tree, "leds"));
/* Add the LEDs to the medium */
std::string strMedium;
GetNodeAttribute(GetNode(t_tree, "leds"), "medium", strMedium);
m_pcLEDMedium = &CSimulator::GetInstance().GetMedium<CLEDMedium>(strMedium);
m_pcLEDEquippedEntity->AddToMedium(*m_pcLEDMedium);
}
else {
/* No LEDs added, no need to update this entity */
m_pcLEDEquippedEntity->Disable();
m_pcLEDEquippedEntity->SetCanBeEnabledIfDisabled(false);
}
UpdateComponents();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize box entity \"" << GetId() << "\".", ex);
}
}
void CCylinderEntity::Init(TConfigurationNode& t_tree) {
try {
/* Init parent */
CEntity::Init(t_tree);
/* Parse XML to get the radius */
GetNodeAttribute(t_tree, "radius", m_fRadius);
/* Parse XML to get the height */
GetNodeAttribute(t_tree, "height", m_fHeight);
/* Parse XML to get the movable attribute */
GetNodeAttribute(t_tree, "movable", m_bMovable);
if(m_bMovable) {
/* Parse XML to get the mass */
GetNodeAttribute(t_tree, "mass", m_fMass);
}
else {
m_fMass = 0.0f;
}
/* Parse XML to get the visible attribute */
GetNodeAttributeOrDefault(t_tree, "visible", m_bVisible, m_bVisible);
/* Init LED equipped entity component */
m_pcLEDEquippedEntity->Init(t_tree);
if(NodeExists(t_tree, "leds")) {
TConfigurationNode& tLEDs = GetNode(t_tree, "leds");
/* Go through the led entries */
CVector3 cPosition;
CColor cColor;
TConfigurationNodeIterator itLED("led");
for(itLED = itLED.begin(&tLEDs);
itLED != itLED.end();
++itLED) {
GetNodeAttribute(*itLED, "position", cPosition);
GetNodeAttribute(*itLED, "color", cColor);
m_vecBaseLEDPositions.push_back(cPosition);
m_pcLEDEquippedEntity->AddLed(cPosition, cColor);
}
}
/* Create embodied entity using parsed data */
m_pcEmbodiedEntity = new CCylinderEmbodiedEntity(this, m_fRadius, m_fHeight);
m_pcEmbodiedEntity->Init(t_tree);
UpdateComponents();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize the cylinder entity.", ex);
}
}
QString CQTOpenGLMainWindow::GetPOVRaySceneXMLData() {
/* Get the current simulation step */
UInt32 unStep = CSimulator::GetInstance().GetSpace().GetSimulationClock();
/* Get a reference to the camera */
const CQTOpenGLCamera& cCamera = m_pcOpenGLWidget->GetCamera();
/* Get its current position and target */
const CVector3& cPos = cCamera.GetPosition();
const CVector3& cLookAt = cCamera.GetTarget();
/* Get the environment node and its contents from the 'povray_render', if defined */
TConfigurationNode& tExperiment = CSimulator::GetInstance().GetConfigurationRoot();
TConfigurationNode& tVisualization = GetNode(tExperiment, "visualization");
QString strPOVRayEnvironment;
if(NodeExists(tVisualization,"povray_render")) {
TConfigurationNode& tPOVRayVisualization = GetNode(tVisualization, "povray_render");
TConfigurationNode& tPOVRayEnvironment = GetNode(tPOVRayVisualization, "environment");
std::string strPOVRayEnvironmentNodeContent = tPOVRayEnvironment.ToString(tPOVRayEnvironment);
strPOVRayEnvironment = strPOVRayEnvironmentNodeContent.c_str();
}
/* Return the XML portion */
return QString(
"%1\n"
"<scene step=\"%2\">\n"
" <camera type=\"normal\"\n"
" position=\"%3,%4,%5\"\n"
" look_at=\"%6,%7,%8\"\n"
" focal_length=\"%9\" />\n"
"</scene>\n"
)
.arg(strPOVRayEnvironment)
.arg(unStep)
.arg(cPos.GetX())
.arg(cPos.GetY())
.arg(cPos.GetZ())
.arg(cLookAt.GetX())
.arg(cLookAt.GetY())
.arg(cLookAt.GetZ())
.arg(cCamera.GetLensFocalLength() * 1000.0f);
}
void CQTOpenGLCamera::Init(TConfigurationNode& t_tree) {
if(NodeExists(t_tree, "camera")) {
try {
TConfigurationNode tCameraNode;
tCameraNode = GetNode(t_tree, "camera");
TConfigurationNodeIterator itSettingss;
SInt32 nIdx;
for(itSettingss = itSettingss.begin(&tCameraNode);
itSettingss != itSettingss.end();
++itSettingss) {
GetNodeAttribute(*itSettingss, "idx", nIdx);
if(nIdx >=0 && nIdx <= 11) {
m_sSettings[nIdx].Init(*itSettingss);
}
else {
THROW_ARGOSEXCEPTION("Error initializing QTOpenGL camera settings: value given for 'idx' is out of bounds. Value = \"" << nIdx << "\", allowed [0-9].");
}
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing QTOpenGL camera settings", ex);
}
}
}
void CLocalNav::AddPathNode(int nindexSource, int offsetX, int offsetY, BOOL fNoMonsters)
{
BOOL bDepth;
Vector vecSource, vecDest;
int offsetXAbs, offsetYAbs;
int xRevDir, yRevDir;
if (nindexSource == -1)
{
offsetXAbs = offsetX;
offsetYAbs = offsetY;
bDepth = 1;
vecSource = m_vecStartingLoc;
vecDest.x = vecSource.x + (offsetX * HOSTAGE_STEPSIZE);
vecDest.y = vecSource.y + (offsetY * HOSTAGE_STEPSIZE);
vecDest.z = vecSource.z;
}
else
{
node_index_t *nodeSource;
node_index_t *nodeCurrent;
int nindexCurrent;
nodeCurrent = GetNode(nindexSource);
offsetXAbs = offsetX + nodeCurrent->offsetX;
offsetYAbs = offsetY + nodeCurrent->offsetY;
nodeSource = GetNode(m_nindexAvailableNode);
if (NodeExists(offsetXAbs, offsetYAbs) != -1)
return;
vecSource = nodeCurrent->vecLoc;
vecDest.x = vecSource.x + (offsetX * HOSTAGE_STEPSIZE);
vecDest.y = vecSource.y + (offsetY * HOSTAGE_STEPSIZE);
vecDest.z = vecSource.z;
if (m_nindexAvailableNode)
{
nindexCurrent = m_nindexAvailableNode;
do
{
nodeSource--;
nindexCurrent--;
xRevDir = nodeSource->offsetX - offsetXAbs;
if (xRevDir >= 0)
{
if (xRevDir > 1)
continue;
}
else
{
if (-xRevDir > 1)
continue;
}
yRevDir = nodeSource->offsetY - offsetYAbs;
if (yRevDir >= 0)
{
if (yRevDir > 1)
continue;
}
else
{
if (-yRevDir > 1)
continue;
}
if (PathTraversable(nodeSource->vecLoc, vecDest, fNoMonsters) != TRAVERSABLE_NO)
{
nodeCurrent = nodeSource;
nindexSource = nindexCurrent;
}
}
while (nindexCurrent);
}
vecSource = nodeCurrent->vecLoc;
bDepth = nodeCurrent->bDepth + 1;
}
if (PathTraversable(vecSource, vecDest, fNoMonsters) != TRAVERSABLE_NO)
AddNode(nindexSource, vecDest, offsetXAbs, offsetYAbs, bDepth);
}
void CLocalNav::AddPathNode(node_index_t nindexSource, int offsetX, int offsetY, int fNoMonsters)
{
int bDepth;
Vector vecSource, vecDest;
int offsetXAbs, offsetYAbs;
if (nindexSource == NODE_INVALID_EMPTY)
{
bDepth = 1;
offsetXAbs = offsetX;
offsetYAbs = offsetY;
vecSource = m_vecStartingLoc;
vecDest = vecSource + Vector(float_precision(offsetX) * HOSTAGE_STEPSIZE, float_precision(offsetY) * HOSTAGE_STEPSIZE, 0);
}
else
{
localnode_t *nodeSource;
localnode_t *nodeCurrent;
node_index_t nindexCurrent;
nodeCurrent = GetNode(nindexSource);
offsetXAbs = offsetX + nodeCurrent->offsetX;
offsetYAbs = offsetY + nodeCurrent->offsetY;
nodeSource = GetNode(m_nindexAvailableNode);
// if there exists a node, then to ignore adding a the new node
if (NodeExists(offsetXAbs, offsetYAbs) != NODE_INVALID_EMPTY)
{
return;
}
vecSource = nodeCurrent->vecLoc;
vecDest = vecSource + Vector((float_precision(offsetX) * HOSTAGE_STEPSIZE), (float_precision(offsetY) * HOSTAGE_STEPSIZE), 0);
if (m_nindexAvailableNode)
{
nindexCurrent = m_nindexAvailableNode;
do
{
nodeSource--;
nindexCurrent--;
offsetX = (nodeSource->offsetX - offsetXAbs);
if (offsetX >= 0)
{
if (offsetX > 1)
{
continue;
}
}
else
{
if (-offsetX > 1)
{
continue;
}
}
offsetY = (nodeSource->offsetY - offsetYAbs);
if (offsetY >= 0)
{
if (offsetY > 1)
{
continue;
}
}
else
{
if (-offsetY > 1)
{
continue;
}
}
if (PathTraversable(nodeSource->vecLoc, vecDest, fNoMonsters) != PATH_TRAVERSABLE_EMPTY)
{
nodeCurrent = nodeSource;
nindexSource = nindexCurrent;
}
}
while (nindexCurrent);
}
vecSource = nodeCurrent->vecLoc;
bDepth = int(nodeCurrent->bDepth) + 1;
}
if (PathTraversable(vecSource, vecDest, fNoMonsters) != PATH_TRAVERSABLE_EMPTY)
{
AddNode(nindexSource, vecDest, offsetXAbs, offsetYAbs, bDepth);
}
}
void CDynamics2DEngine::Init(TConfigurationNode& t_tree) {
try {
/* Init parent */
CPhysicsEngine::Init(t_tree);
/* Parse XML */
GetNodeAttributeOrDefault(t_tree, "static_cell_size", m_fStaticHashCellSize, m_fStaticHashCellSize);
GetNodeAttributeOrDefault(t_tree, "active_cell_size", m_fActiveHashCellSize, m_fActiveHashCellSize);
GetNodeAttributeOrDefault(t_tree, "static_cells", m_nStaticHashCells, m_nStaticHashCells);
GetNodeAttributeOrDefault(t_tree, "active_cells", m_nActiveHashCells, m_nActiveHashCells);
GetNodeAttributeOrDefault(t_tree, "elevation", m_fElevation, m_fElevation);
if(NodeExists(t_tree, "boundaries")) {
/* Parse the boundary definition */
TConfigurationNode& tBoundaries = GetNode(t_tree, "boundaries");
SBoundarySegment sBoundSegment;
CVector2 cLastPoint, cCurPoint;
std::string strConnectWith;
TConfigurationNodeIterator tVertexIt("vertex");
/* Get the first vertex */
tVertexIt = tVertexIt.begin(&tBoundaries);
if(tVertexIt == tVertexIt.end()) {
THROW_ARGOSEXCEPTION("Physics engine of type \"dynamics2d\", id \"" << GetId() << "\": you didn't specify any <vertex>!");
}
GetNodeAttribute(*tVertexIt, "point", cLastPoint);
m_vecVertices.push_back(cLastPoint);
/* Go through the other vertices */
++tVertexIt;
while(tVertexIt != tVertexIt.end()) {
/* Read vertex data and fill in segment struct */
GetNodeAttribute(*tVertexIt, "point", cCurPoint);
m_vecVertices.push_back(cCurPoint);
sBoundSegment.Segment.SetStart(cLastPoint);
sBoundSegment.Segment.SetEnd(cCurPoint);
GetNodeAttribute(*tVertexIt, "connect_with", strConnectWith);
if(strConnectWith == "gate") {
/* Connect to previous vertex with a gate */
sBoundSegment.Type = SBoundarySegment::SEGMENT_TYPE_GATE;
GetNodeAttribute(*tVertexIt, "to_engine", sBoundSegment.EngineId);
}
else if(strConnectWith == "wall") {
/* Connect to previous vertex with a wall */
sBoundSegment.Type = SBoundarySegment::SEGMENT_TYPE_WALL;
sBoundSegment.EngineId = "";
}
else {
/* Parse error */
THROW_ARGOSEXCEPTION("Physics engine of type \"dynamics2d\", id \"" << GetId() << "\": unknown vertex connection method \"" << strConnectWith << "\". Allowed methods are \"wall\" and \"gate\".");
}
m_vecSegments.push_back(sBoundSegment);
/* Next vertex */
cLastPoint = cCurPoint;
++tVertexIt;
}
/* Check that the boundary is a closed path */
if(m_vecVertices.front() != m_vecVertices.back()) {
THROW_ARGOSEXCEPTION("Physics engine of type \"dynamics2d\", id \"" << GetId() << "\": the specified path is not closed. The first and last points of the boundaries MUST be the same.");
}
}
/* Initialize physics */
cpInitChipmunk();
cpResetShapeIdCounter();
/* Used to attach static geometries so that they won't move and to simulate friction */
m_ptGroundBody = cpBodyNew(INFINITY, INFINITY);
/* Create the space to contain the movable objects */
m_ptSpace = cpSpaceNew();
/* Subiterations to solve constraints.
The more, the better for precision but the worse for speed
*/
m_ptSpace->iterations = GetIterations();
/* Resize the space hash.
This has dramatic effects on performance.
TODO: - find optimal parameters automatically (average entity size)
cpSpaceReindexStaticHash(m_ptSpace, m_fStaticHashCellSize, m_nStaticHashCells);
cpSpaceResizeActiveHash(m_ptSpace, m_fActiveHashCellSize, m_nActiveHashCells);
*/
/* Gripper-Gripped callback functions */
cpSpaceAddCollisionHandler(
m_ptSpace,
SHAPE_GRIPPER,
SHAPE_GRIPPABLE,
BeginCollisionBetweenGripperAndGrippable,
ManageCollisionBetweenGripperAndGrippable,
NULL,
NULL,
NULL);
/* Add boundaries, if specified */
if(! m_vecSegments.empty()) {
cpShape* ptSegment;
for(size_t i = 0; i < m_vecSegments.size(); ++i) {
if(m_vecSegments[i].Type == SBoundarySegment::SEGMENT_TYPE_WALL) {
ptSegment =
cpSpaceAddShape(
m_ptSpace,
cpSegmentShapeNew(
m_ptGroundBody,
cpv(m_vecSegments[i].Segment.GetStart().GetX(),
m_vecSegments[i].Segment.GetStart().GetY()),
cpv(m_vecSegments[i].Segment.GetEnd().GetX(),
m_vecSegments[i].Segment.GetEnd().GetY()),
0.0f));
ptSegment->e = 0.0f; // no elasticity
ptSegment->u = 1.0f; // max friction
}
else {
/* There is at least a gate, transfer is activated */
m_bEntityTransferActive = true;
}
}
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing the dynamics 2D engine \"" << GetId() << "\"", ex);
}
}
void CSimulator::InitFramework(TConfigurationNode& t_tree) {
try {
/* Parse the 'system' node */
if(NodeExists(t_tree, "system")) {
TConfigurationNode tSystem;
tSystem = GetNode(t_tree, "system");
GetNodeAttributeOrDefault(tSystem, "threads", m_unThreads, m_unThreads);
if(m_unThreads == 0) {
LOG << "[INFO] Not using threads" << std::endl;
m_pcSpace = new CSpaceNoThreads();
}
else {
LOG << "[INFO] Using " << m_unThreads << " parallel threads" << std::endl;
std::string strThreadingMethod = "balance_quantity";
GetNodeAttributeOrDefault(tSystem, "method", strThreadingMethod, strThreadingMethod);
if(strThreadingMethod == "balance_quantity") {
LOG << "[INFO] Chosen method \"balance_quantity\": threads will be assigned the same"
<< std::endl
<< "[INFO] number of tasks, independently of the task length."
<< std::endl;
m_pcSpace = new CSpaceMultiThreadBalanceQuantity();
}
else if(strThreadingMethod == "balance_length") {
LOG << "[INFO] Chosen method \"balance_length\": threads will be assigned different"
<< std::endl
<< "[INFO] numbers of tasks, depending on the task length."
<< std::endl;
m_pcSpace = new CSpaceMultiThreadBalanceLength();
}
else {
THROW_ARGOSEXCEPTION("Error parsing the <system> tag. Unknown threading method \"" << strThreadingMethod << "\". Available methods: \"balance_quantity\" and \"balance_length\".");
}
}
}
else {
LOG << "[INFO] Not using threads" << std::endl;
m_pcSpace = new CSpaceNoThreads();
}
/* Get 'experiment' node */
TConfigurationNode tExperiment;
tExperiment = GetNode(t_tree, "experiment");
/* Parse random seed */
/* Buffer to hold the random seed */
if(!m_bWasRandomSeedSet)
GetNodeAttributeOrDefault(tExperiment,
"random_seed",
m_unRandomSeed,
static_cast<UInt32>(0));
/* if random seed is 0 or is not specified, init with the current timeval */
if(m_unRandomSeed != 0) {
CRandom::CreateCategory("argos", m_unRandomSeed);
LOG << "[INFO] Using random seed = " << m_unRandomSeed << std::endl;
m_bWasRandomSeedSet = true;
}
else {
/* Prepare the default value based on the current clock time */
m_bWasRandomSeedSet = false;
struct timeval sTimeValue;
::gettimeofday(&sTimeValue, NULL);
UInt32 unSeed = static_cast<UInt32>(sTimeValue.tv_usec);
m_unRandomSeed = unSeed;
CRandom::CreateCategory("argos", unSeed);
LOG << "[INFO] Using random seed = " << unSeed << std::endl;
}
m_pcRNG = CRandom::CreateRNG("argos");
/* Set the simulation clock tick length */
UInt32 unTicksPerSec;
GetNodeAttribute(tExperiment,
"ticks_per_second",
unTicksPerSec);
CPhysicsEngine::SetSimulationClockTick(1.0 / static_cast<Real>(unTicksPerSec));
/* Set the maximum simulation duration (in seconds) */
Real fExpLength;
GetNodeAttributeOrDefault<Real>(tExperiment,
"length",
fExpLength,
0.0f);
m_unMaxSimulationClock = fExpLength * unTicksPerSec;
LOG << "[INFO] Total experiment length in clock ticks = "
<< (m_unMaxSimulationClock ? ToString(m_unMaxSimulationClock) : "unlimited")
<< std::endl;
/* Check for the 'real_time' attribute */
GetNodeAttributeOrDefault(tExperiment, "real_time", m_bRealTimeClock, m_bRealTimeClock);
if(m_bRealTimeClock) {
LOG << "[INFO] Using the real-time clock." << std::endl;
}
/* Get the profiling tag, if present */
if(NodeExists(t_tree, "profiling")) {
TConfigurationNode& tProfiling = GetNode(t_tree, "profiling");
std::string strFile;
GetNodeAttribute(tProfiling, "file", strFile);
std::string strFormat;
GetNodeAttribute(tProfiling, "format", strFormat);
if(strFormat == "human_readable") {
m_bHumanReadableProfile = true;
}
else if(strFormat == "table") {
m_bHumanReadableProfile = false;
}
else {
THROW_ARGOSEXCEPTION("Unrecognized profile format \"" << strFormat << "\". Accepted values are \"human_readable\" and \"table\".");
}
bool bTrunc = true;
GetNodeAttributeOrDefault(tProfiling, "truncate_file", bTrunc, bTrunc);
m_pcProfiler = new CProfiler(strFile, bTrunc);
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize the simulator. Parse error inside the <framework> tag.", ex);
}
}
void CFootBotUN::Init(TConfigurationNode& t_node) {
/*
* Get sensor/actuator handles
*
* The passed string (ex. "footbot_wheels") corresponds to the XML tag of the device
* whose handle we want to have. For a list of allowed values, type at the command
* prompt:
*
* $ launch_argos -q actuators
*
* to have a list of all the possible actuators, or
*
* $ launch_argos -q sensors
*
* to have a list of all the possible sensors.
*
* NOTE: ARGoS creates and initializes actuators and sensors internally, on the basis of
* the lists provided the XML file at the <controllers><footbot_diffusion><actuators>
* and <controllers><footbot_diffusion><sensors> sections. If you forgot to
* list a device in the XML and then you request it here, an error occurs.
*/
m_pcWheels = dynamic_cast<CCI_FootBotWheelsActuator*>(GetRobot().GetActuator("footbot_wheels"));
m_pcLEDs = dynamic_cast<CCI_FootBotLedsActuator*>(GetRobot().GetActuator("footbot_leds"));
m_pcProximity = dynamic_cast<CCI_FootBotProximitySensor*>(GetRobot().GetSensor("footbot_proximity"));
//Min distance to the current position to the target point to determine it this is reached (meters)
//GetNodeAttributeOrDefault(t_node, "targetMinPointDistance", m_targetMinPointDistance, m_targetMinPointDistance);
std::string text;
if (NodeExists(t_node, "targetMinPointDistance")) {
GetNodeText(GetNode(t_node, "targetMinPointDistance"), text);
sscanf(text.c_str(), "%f", &m_targetMinPointDistance);
//m_targetMinPointDistance = fmin(MAX_RESOLUTION, m_targetMinPointDistance);
}
printf("Min distance to target point %f in robot %s\n", m_targetMinPointDistance, GetRobot().GetRobotId().c_str());
//////////////////////////////////////////////////////////////
// Initialize things required by the communications
//////////////////////////////////////////////////////////////
m_pcWifiSensor = dynamic_cast<CCI_WiFiSensor*>(GetRobot().GetSensor("wifi"));
m_pcWifiActuator = dynamic_cast<CCI_WiFiActuator*>(GetRobot().GetActuator("wifi"));
str_Me = GetRobot().GetRobotId();
m_iSendOrNotSend = 0;
// How many robots are there ?
CSpace::TAnyEntityMap& tEntityMap = m_cSpace.GetEntitiesByType("wifi_equipped_entity");
numberOfRobots = tEntityMap.size();
printf("%s: There are %d robots [RMAGAN]\n", str_Me.c_str(), numberOfRobots);
// Am I a generator?
amIaGenerator = false;
skipFirstSend = true;
int numGenerators = m_numberOfGenerators * numberOfRobots;
int myID = atoi(str_Me.c_str() + 3);
if (myID < numGenerators)
amIaGenerator = true;
if (amIaGenerator)
printf("%s: There are %d generators, I am on of them\n", str_Me.c_str(), numGenerators);
else
printf("%s: There are %d generators, but not me :-(\n", str_Me.c_str(), numGenerators);
if (amIaGenerator) {
UInt32 id = CSimulator::GetInstance().GetRNG()->Uniform(CRange<UInt32>(numGenerators, numberOfRobots));
//UInt32 id = myID + numGenerators;
std::ostringstream str_tmp(ostringstream::out);
str_tmp << "fb_" << id;
str_Dest = str_tmp.str();
printf(" (dest=%s).\n", str_Dest.c_str());
}
//////////////////////////////////////////////////////////////
// end of communications things here
//////////////////////////////////////////////////////////////
/** LCM engine */
// LCM Thread
lcmThreadCommand.setLCMEngine("udpm://239.255.76.67:7667?ttl=1", "TARGET");
lcmThreadCommand.startInternalThread();
lcmThread.setLCMEngine("udpm://239.255.76.67:7667?ttl=1", "TRACK");
lcmThread.startInternalThread();
/** NAVIGATION AND AVOIDING COLLISION */
/* Additional sensors */
encoderSensor = dynamic_cast<CCI_FootBotEncoderSensor*>(GetRobot().GetSensor("footbot_encoder"));
/* Init navigation methods */
initOdometry();
initLocalNavigation(t_node);
}
void PokemonInfoReader::Visit (XmlReader& visitable)
{
auto reader = visitable.ChangeRoot (xmlRootNodeName_);
pokemonInfo_.SetID (
reader->ReadID (
XmlHelper::GetAttrNodeName (DEFAULT_XML_ID_NODE_NAME)));
pokemonInfo_.SetName (reader->ReadString (DEFAULT_XML_NAME_NODE_NAME));
pokemonInfo_.SetDescription (
reader->ReadString (DEFAULT_XML_DESCRIPTION_NODE_NAME));
pokemonInfo_.SetSpecies (
reader->ReadString (DEFAULT_XML_SPECIES_NODE_NAME));
pokemonInfo_.SetHeight (
reader->ReadFloat (DEFAULT_XML_HEIGHT_NODE_NAME));
pokemonInfo_.SetWeight (
reader->ReadFloat (DEFAULT_XML_WEIGHT_NODE_NAME));
pokemonInfo_.SetRarity (
reader->ReadInt (DEFAULT_XML_RARITY_NODE_NAME));
pokemonInfo_.SetGenderProbability (
reader->ReadFloat (DEFAULT_XML_GENDER_NODE_NAME));
pokemonInfo_.SetExperience (
reader->ReadInt (DEFAULT_XML_EXPERIENCE_NODE_NAME));
pokemonInfo_.SetExperienceType (StringHelper::Parse<ExperienceType>
(reader->ReadString (DEFAULT_XML_EXPERIENCE_TYPE_NODE_NAME)));
auto statsReader =
reader->ChangeRoot (DEFAULT_XML_BASE_STATS_NODE_NAME);
pokemonInfo_.SetHitPoint (
statsReader->ReadInt (DEFAULT_XML_HP_NODE_NAME));
pokemonInfo_.SetAttack (
statsReader->ReadInt (DEFAULT_XML_ATTACK_NODE_NAME));
pokemonInfo_.SetDefense (
statsReader->ReadInt (DEFAULT_XML_DEFENSE_NODE_NAME));
pokemonInfo_.SetSpecialAttack (
statsReader->ReadInt (DEFAULT_XML_SPECIAL_ATTACK_NODE_NAME));
pokemonInfo_.SetSpecialDefense (
statsReader->ReadInt (DEFAULT_XML_SPECIAL_DEFENSE_NODE_NAME));
pokemonInfo_.SetSpeed (
statsReader->ReadInt (DEFAULT_XML_SPEED_NODE_NAME));
auto EVReader =
reader->ChangeRoot (DEFAULT_XML_EFFORT_VALUES_NODE_NAME);
pokemonInfo_.SetHitPointEV (
EVReader->ReadInt (DEFAULT_XML_HP_NODE_NAME));
pokemonInfo_.SetAttackEV (
EVReader->ReadInt (DEFAULT_XML_ATTACK_NODE_NAME));
pokemonInfo_.SetDefenseEV (
EVReader->ReadInt (DEFAULT_XML_DEFENSE_NODE_NAME));
pokemonInfo_.SetSpecialAttackEV (
EVReader->ReadInt (DEFAULT_XML_SPECIAL_ATTACK_NODE_NAME));
pokemonInfo_.SetSpecialDefenseEV (
EVReader->ReadInt (DEFAULT_XML_SPECIAL_DEFENSE_NODE_NAME));
pokemonInfo_.SetSpeedEV (
EVReader->ReadInt (DEFAULT_XML_SPEED_NODE_NAME));
if (reader->NodeExists (DEFAULT_XML_EVOLUTION_NODE_NAME))
{
auto evolutionReader = reader->ChangeRoot (
DEFAULT_XML_EVOLUTION_NODE_NAME);
pokemonInfo_.SetEvolutionLevel (
evolutionReader->ReadInt (
XmlHelper::GetAttrNodeName (DEFAULT_XML_LEVEL_ATTR_NAME)));
pokemonInfo_.SetPokemonEvolutionID (evolutionReader->ReadID ());
}
auto baseSkillsReader =
reader->ChangeRoot (DEFAULT_XML_BASE_SKILLS_NODE_NAME);
yap::XmlReaderCollection skillReaders;
baseSkillsReader->ReadNodes
(DEFAULT_XML_SKILL_NODE_NAME, skillReaders);
for (auto& skillReader : skillReaders)
{
UInt16 level = skillReader->ReadInt (
XmlHelper::GetAttrNodeName (DEFAULT_XML_LEVEL_ATTR_NAME));
ID skillID = skillReader->ReadID ();
pokemonInfo_.AddBaseSkill (level, skillID);
}
auto typeReader = reader->ChangeRoot (DEFAULT_XML_TYPES_NODE_NAME);
pokemonInfo_.SetType1
(typeReader->ReadInt (DEFAULT_XML_TYPE1_NODE_NAME));
pokemonInfo_.SetType2
(typeReader->ReadInt (DEFAULT_XML_TYPE2_NODE_NAME));
auto graphicReader = reader->ChangeRoot (DEFAULT_XML_GRAPHICS_NODE_NAME);
pokemonInfo_.SetIconPath (graphicReader->ReadString (DEFAULT_XML_ICON_NODE_NAME));
// <male>
auto maleReader = graphicReader->ChangeRoot (DEFAULT_XML_MALE_NODE_NAME);
pokemonInfo_.SetMaleFrontPath
(maleReader->ReadString (DEFAULT_XML_FRONT_NODE_NAME));
pokemonInfo_.SetMaleBackPath
(maleReader->ReadString (DEFAULT_XML_BACK_NODE_NAME));
auto shinyMaleReader = maleReader->ChangeRoot (DEFAULT_XML_SHINY_NODE_NAME);
pokemonInfo_.SetShinyMaleFrontPath
(shinyMaleReader->ReadString (DEFAULT_XML_FRONT_NODE_NAME));
pokemonInfo_.SetShinyMaleBackPath
(shinyMaleReader->ReadString (DEFAULT_XML_BACK_NODE_NAME));
// </male>
// <female>
auto femaleReader = graphicReader->ChangeRoot (DEFAULT_XML_FEMALE_NODE_NAME);
pokemonInfo_.SetFemaleFrontPath
(femaleReader->ReadString (DEFAULT_XML_FRONT_NODE_NAME));
pokemonInfo_.SetFemaleBackPath
(femaleReader->ReadString (DEFAULT_XML_BACK_NODE_NAME));
auto shinyFemaleReader = femaleReader->ChangeRoot (DEFAULT_XML_SHINY_NODE_NAME);
pokemonInfo_.SetShinyFemaleFrontPath
(shinyFemaleReader->ReadString (DEFAULT_XML_FRONT_NODE_NAME));
pokemonInfo_.SetShinyFemaleBackPath
(shinyFemaleReader->ReadString (DEFAULT_XML_BACK_NODE_NAME));
// </female>
}
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);
}
