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);
}
}
/*****
* Initialize the controller via the XML configuration file. ARGoS typically
* wants objects & variables initialized here instead of in the constructor(s).
*****/
void iAnt_controller::Init(TConfigurationNode& node) {
/* Shorter names, please. #This_Is_Not_Java */
typedef CCI_PositioningSensor CCI_PS;
typedef CCI_DifferentialSteeringActuator CCI_DSA;
typedef CCI_FootBotProximitySensor CCI_FBPS;
/* Initialize the robot's actuator and sensor objects. */
motorActuator = GetActuator<CCI_DSA>("differential_steering");
compass = GetSensor<CCI_PS> ("positioning");
proximitySensor = GetSensor<CCI_FBPS> ("footbot_proximity");
TConfigurationNode iAnt_params = GetNode(node, "iAnt_params");
GetNodeAttribute(iAnt_params, "RobotForwardSpeed", RobotForwardSpeed);
GetNodeAttribute(iAnt_params, "RobotTurningSpeed", RobotTurningSpeed);
GetNodeAttribute(iAnt_params, "AngleToleranceInDegrees", angleInDegrees);
AngleToleranceInRadians.Set(-ToRadians(angleInDegrees),ToRadians(angleInDegrees));
stepSize = 0.1; /* Assigns the robot's stepSize */
startPosition = CVector3(0.0, 0.0, 0.0);
}
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 CTextRender::Init(TConfigurationNode& t_tree)
{
try {
/* Call parent init */
CRender::Init(t_tree);
/* Get precision */
GetNodeAttribute(t_tree, "precision", m_unPrecision);
++m_unPrecision;
/* Get the file name, if any */
std::string strFileName, strDefault;
GetNodeAttributeOrDefault(t_tree, "file", strFileName, strDefault);
/* Decide what to do with output: stdout or file? */
if(strFileName != "") {
/* Output to file */
m_cOutputFileStream.open(strFileName.c_str(), std::ios::out);
if ( m_cOutputFileStream.bad( ) ) {
THROW_ARGOSEXCEPTION("Error opening file \"" <<
strFileName << "\" for text rendering.");
}
m_cOutputStream.rdbuf(m_cOutputFileStream.rdbuf());
}
else {
/* Output to stdout */
m_cOutputStream.rdbuf(std::cout.rdbuf( ));
}
/* Display a column header */
m_cOutputStream << std::setw(8) << setiosflags(std::ios::left) << "# clock" << " "
<< std::setw(16) << setiosflags(std::ios::left) << "Robot type" << " "
<< std::setw(16) << setiosflags(std::ios::left) << "Robot id" << " "
<< std::setw(8) << setiosflags(std::ios::left) << "X" << " "
<< std::setw(8) << setiosflags(std::ios::left) << "Y" << " "
<< std::setw(8) << setiosflags(std::ios::left) << "Z" << " "
<< std::setw(8) << setiosflags(std::ios::left) << "rotZ" << " "
<< std::setw(8) << setiosflags(std::ios::left) << "rotY" << " "
<< std::setw(8) << setiosflags(std::ios::left) << "rotX"
<< std::endl << std::flush;
/* Create the visitor */
m_pcVisitor = new CTextRenderVisitorDraw(m_cOutputStream, m_unPrecision);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize the text renderer", ex);
}
}
bool CXmlReader::GetNodeAttribute(LPCTSTR szKey, bool& bValue)
{
int nValue = 0;
if (GetNodeAttribute(szKey, nValue))
{
if (nValue)
bValue = true;
else
bValue = false;
return true;
}
else
return false;
}
HRESULT COFSNcDlg2::GetNodeAttributeAsLong(IXMLDOMNode *pNode, BSTR bsAttrName, long *pAttrValue, int nBase)
{
HRESULT hr;
IXMLDOMElement *pEle = NULL;
WCHAR *szNULL = L"\0x00";
CComBSTR bs;
ASSERT(pAttrValue != NULL);
if(pNode == NULL)
return E_INVALIDARG;
hr = GetNodeAttribute(pNode, bsAttrName, bs);
if(bs.m_str != NULL)
*pAttrValue = wcstol(bs.m_str, &szNULL, nBase);
return hr;
}
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");
/* Get the script name */
std::string strFName;
GetNodeAttribute(t_node, "bytecode_file", strFName);
/* Initialize the rest */
m_tBuzzVM = NULL;
m_unRobotId = FromString<UInt16>(GetId().substr(2));
SetBytecode(strFName);
UpdateSensors();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing the Buzz controller", ex);
}
}
/**
* Load the sphere configuration from its XML tag
*/
void CSphereEntity::Init(TConfigurationNode &t_tree)
{
try
{
// Init parent
CComposableEntity::Init(t_tree);
// Parse XML to get the radius (required)
GetNodeAttribute(t_tree, "radius", m_fRadius);
// Parse XML to get the movable attribute (optional: defaults to true)
bool bMovable;
GetNodeAttributeOrDefault(t_tree, "movable", bMovable, true);
// Get the mass from XML if the sphere is movable
if (bMovable)
{
// Parse XML to get the mass (optional, defaults to 1)
GetNodeAttributeOrDefault(t_tree, "mass", m_fMass, 1.0f);
}
else
{
m_fMass = 0.0f;
}
// Create embodied entity using parsed data
m_pcEmbodiedEntity = new CEmbodiedEntity(this);
m_pcEmbodiedEntity->Init(GetNode(t_tree, "body"));
m_pcEmbodiedEntity->SetMovable(bMovable);
AddComponent(*m_pcEmbodiedEntity);
UpdateComponents();
}
catch (CARGoSException &ex)
{
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize the ball entity.", ex);
}
}
template <class T> void AssignController(T& c_entity) {
/* Get a reference to the controllable entity */
CControllableEntity& cControllableEntity = c_entity.GetControllableEntity();
/* Look for the controller with the right id in the XML */
TConfigurationNode tControllersTree;
tControllersTree = GetNode(CSimulator::GetInstance().GetConfigurationRoot(), "controllers");
bool found = false;
TConfigurationNodeIterator itControllers;
std::string strControllerId;
itControllers = itControllers.begin(&tControllersTree);
while(!found && itControllers != itControllers.end()) {
GetNodeAttribute(*itControllers, "id", strControllerId);
if(strControllerId == cControllableEntity.GetControllerId()) {
found = true;
}
else {
++itControllers;
}
}
/* Did we find the controller? */
ARGOS_ASSERT(found,
"[FATAL] The entity \"" <<
c_entity.GetId() << "\" has been associated with a controller with id \"" <<
cControllableEntity.GetControllerId() <<
"\", but a controller with this id wasn't found in the <controllers> section of the XML file.");
/* Now itControllers points to the right controller subtree */
/* Get the parameters subtree */
TConfigurationNode tControllerParameters;
tControllerParameters = GetNode(*itControllers, "parameters");
/* Create the controller */
CCI_Controller* pcController =
CSimulator::GetInstance().
GetDynamicLinkingManager().
NewController(c_entity,
cControllableEntity.GetControllerId(),
tControllerParameters);
/* Set the controller to the entity */
cControllableEntity.SetController(*pcController);
}
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);
}
}
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 CCombinedLoopFunctions::Init(TConfigurationNode& t_node) {
try {
arenaSize = CVector2(GetSpace().GetArenaSize().GetX()-1, GetSpace().GetArenaSize().GetY()-1);
/* 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, "nestSize", nestSize);
GetNodeAttribute(tForaging, "nbFoodPatches", nbFoodPatches);
GetNodeAttribute(tForaging, "renewalRate", renewalRate);
foodClock = renewalRate;
GetNodeAttribute(tForaging, "output", m_strOutput);
GetNodeAttribute(tForaging, "type", type);
GetNodeAttribute(tForaging, "foodPatchSize", foodPatchSize);
GetNodeAttribute(tForaging, "nbFoodItems", NbFoodItems);
GetNodeAttribute(tForaging, "radius", m_fFoodSquareRadius);
m_fFoodSquareRadius *= m_fFoodSquareRadius;
int nbSolitary, nbRecruiter, nbRecruitee;
GetNodeAttribute(tForaging, "nbSolitary", nbSolitary);
GetNodeAttribute(tForaging, "nbRecruiter", nbRecruiter);
GetNodeAttribute(tForaging, "nbRecruitee", nbRecruitee);
if(type == "uniform")
generateUniformFoodPatch();
else if (type == "patched")
generateFoodPatches();
else if(type == "mixed"){
generateUniformFoodPatch();
generateFoodPatches();
}
FillFood();
CSpace::TMapPerType& m_cFootbots = GetSpace().GetEntitiesByType("foot-bot");
for(CSpace::TMapPerType::iterator it = m_cFootbots.begin(); it != m_cFootbots.end(); ++it) {
/* Get handle to foot-bot entity and controller */
CFootBotEntity& cFootBot = *any_cast<CFootBotEntity*>(it->second);
CBTFootbotCombinedController& cController = dynamic_cast<CBTFootbotCombinedController&>(cFootBot.GetControllableEntity().GetController());
CBTFootbotCombinedRootBehavior* pcRootBehavior = cController.GetRootBehavior();
if(nbSolitary != 0){
pcRootBehavior->SetRobotType(SOLITARY);
nbSolitary--;
LOG << "Solitary added" << "\n";
}else if(nbRecruiter != 0){
pcRootBehavior->SetRobotType(RECRUITER);
nbRecruiter--;
LOG << "Recruiter added" << "\n";
}else if(nbRecruitee != 0){
pcRootBehavior->SetRobotType(RECRUITEE);
nbRecruitee--;
LOG << "Recruitee added" << "\n";
}
}
}
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,solitary,recruiter,recruitee" << std::endl;
}
/*****
* Required by ARGoS. This function initializes global variables using
* values from the XML configuration file supplied when ARGoS is run.
*****/
void DSA_loop_functions::Init(TConfigurationNode& node) {
/* Temporary variables. */
CSimulator *simulator = &GetSimulator();
CPhysicsEngine *physicsEngine = &simulator->GetPhysicsEngine("default");
CVector3 ArenaSize = GetSpace().GetArenaSize();
CVector2 rangeX = CVector2(-ArenaSize.GetX()/2.0, ArenaSize.GetX()/2.0);
CVector2 rangeY = CVector2(-ArenaSize.GetY()/2.0, ArenaSize.GetY()/2.0);
CDegrees USV_InDegrees;
/* Get each tag in the loop functions section of the XML file. */
TConfigurationNode simNode = GetNode(node, "simulation");
TConfigurationNode random = GetNode(node, "_0_FoodDistribution_Random");
TConfigurationNode cluster = GetNode(node, "_1_FoodDistribution_Cluster");
TConfigurationNode powerLaw = GetNode(node, "_2_FoodDistribution_PowerLaw");
GetNodeAttribute(simNode, "MaxSimCounter", MaxSimCounter);
GetNodeAttribute(simNode, "VariableSeed", VariableSeed);
GetNodeAttribute(simNode, "OutputData", OutputData);
GetNodeAttribute(simNode, "NestPosition", NestPosition);
GetNodeAttribute(simNode, "NestRadius", NestRadius);
GetNodeAttribute(simNode, "FoodRadius", FoodRadius);
GetNodeAttribute(simNode, "FoodDistribution", FoodDistribution);
GetNodeAttribute(random, "FoodItemCount", FoodItemCount);
GetNodeAttribute(cluster, "NumberOfClusters", NumberOfClusters);
GetNodeAttribute(cluster, "ClusterWidthX", ClusterWidthX);
GetNodeAttribute(cluster, "ClusterLengthY", ClusterLengthY);
GetNodeAttribute(powerLaw, "PowerRank", PowerRank);
/* Convert and calculate additional values. */
NestRadiusSquared = (NestRadius) * (NestRadius);
FoodRadiusSquared = (FoodRadius + 0.04) * (FoodRadius + 0.04);
ForageRangeX.Set(rangeX.GetX() + (2.0 * FoodRadius),
rangeX.GetY() - (2.0 * FoodRadius));
ForageRangeY.Set(rangeY.GetX() + (2.0 * FoodRadius),
rangeY.GetY() - (2.0 * FoodRadius));
RNG = CRandom::CreateRNG("argos");
/* Store the iAnts in a more friendly, human-readable structure. */
CSpace::TMapPerType& footbots = GetSpace().GetEntitiesByType("foot-bot");
CSpace::TMapPerType::iterator it;
ReadFile();
size_t STOP = 0;
size_t robots = 0;
for(it = footbots.begin(); it != footbots.end(); it++)
{
CFootBotEntity& footBot = *any_cast<CFootBotEntity*>(it->second);
DSA_controller& c = dynamic_cast<DSA_controller&>(footBot.GetControllableEntity().GetController());
DSAnts.push_back(&c);
c.SetData(&data);
c.SetStop(STOP);
STOP += 10; /* adds 10 seconds extra pause for each robot */
if(robots <= N_robots)
{
c.GetPattern(robotPattern[robots]);
robots++;
//attempt to add mutiple colors for each robot
// if(robots==2)
// {
// DrawTargetRays = 2;
// }
// else if(robots ==3)
// {
// DrawTargetRays = 3;
// }
}
}
/* Set up the food distribution based on the XML file. */
SetFoodDistribution();
}
BOOL COFSNcDlg2::LoadLabel(IXMLDOMNode *pXmlRoot, LPCTSTR szName, CLabel *pLbl, BOOL bVisible)
{
BOOL bResult = FALSE;
LoadSkins skin;
long nErrorCode = 0;
HRESULT hr = S_OK;
CString strErrorMessage;
IStreamPtr pStream = NULL;
CComPtr<IXMLDOMNode> pLabel = NULL, pFont = NULL;
CComBSTR bs;
_bstr_t bsImagePath;
long x=0, y=0, cx=0, cy=0;
WCHAR *szNULL = L"\0x00";
pLbl->ShowWindow(SW_HIDE);
bs.Empty();
bs = L"Label[@Name='";
bs += szName;
bs += L"']";
pXmlRoot->selectSingleNode(bs, &pLabel);
if(pLabel)
{
bResult = TRUE;
// Get button coordinates
bs.Empty();
SelectChildNode(pLabel, CComBSTR(L"XPos"), NULL, &bs);
if(bs.m_str != NULL)
x = wcstol(bs.m_str, &szNULL, 10);
bs.Empty();
SelectChildNode(pLabel, CComBSTR(L"YPos"), NULL, &bs);
if(bs.m_str != NULL)
y = wcstol(bs.m_str, &szNULL, 10);
bs.Empty();
SelectChildNode(pLabel, CComBSTR(L"XLen"), NULL, &bs);
if(bs.m_str != NULL)
cx = wcstol(bs.m_str, &szNULL, 10);
bs.Empty();
SelectChildNode(pLabel, CComBSTR(L"YLen"), NULL, &bs);
if(bs.m_str != NULL)
cy = wcstol(bs.m_str, &szNULL, 10);
COLORREF crText, crBG;
LoadColor(pLabel, _T("Text"), crText);
if(crText != CLR_NONE)
pLbl->SetTextColor(crText);
LoadColor(pLabel, _T("BG"), crBG);
if(crBG != CLR_NONE)
{
pLbl->SetTransparent(FALSE);
pLbl->SetBkColor(crBG);
}
else
pLbl->SetTransparent(TRUE);
// Load font
pLabel->selectSingleNode(CComBSTR(L"Font"), &pFont);
if(pFont)
{
bs.Empty();
GetNodeAttribute(pFont, CComBSTR(L"Face"), bs);
if(bs.m_str != NULL && bs.Length())
pLbl->SetFontName(CString(bs));
long n = 0;
GetNodeAttributeAsLong(pFont, CComBSTR(L"Size"), &n, 10);
if(n != 0)
pLbl->SetFontSize(n);
n = 0;
GetNodeAttributeAsLong(pFont, CComBSTR(L"Bold"), &n, 10);
pLbl->SetFontBold(n != 0);
n = 0;
GetNodeAttributeAsLong(pFont, CComBSTR(L"Italic"), &n, 10);
pLbl->SetFontItalic(n != 0);
n = 0;
GetNodeAttributeAsLong(pFont, CComBSTR(L"Underline"), &n, 10);
pLbl->SetFontUnderline(n != 0);
//pFont->Release();
}
UINT nFlags = SWP_NOZORDER;
if(bVisible)
nFlags |= SWP_SHOWWINDOW;
pLbl->SetWindowPos(NULL, x, y, cx, cy, nFlags);
// Load settings for resize
IXMLDOMNode *pResize = NULL;
pLabel->selectSingleNode(CComBSTR(L"Resize"), &pResize);
if(pResize)
{
long tlcx, tlcy, brcx, brcy;
GetNodeAttributeAsLong(pResize, _bstr_t("TLCX"), &tlcx, 10);
GetNodeAttributeAsLong(pResize, _bstr_t("TLCY"), &tlcy, 10);
GetNodeAttributeAsLong(pResize, _bstr_t("BRCX"), &brcx, 10);
GetNodeAttributeAsLong(pResize, _bstr_t("BRCY"), &brcy, 10);
if(tlcx || tlcy || brcx || brcy)
AddAnchor(pLbl->GetSafeHwnd(), CSize(tlcx, tlcy), CSize(brcx, brcy));
//pResize->Release();
}
//pLabel->Release();
}
return bResult;
}
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 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 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);
}
void CCameraDefaultSensor::Init(TConfigurationNode& t_tree) {
try {
/* Parent class init */
CCI_CameraSensor::Init(t_tree);
/* Show the frustums */
GetNodeAttributeOrDefault(t_tree, "show_frustum", m_bShowFrustum, m_bShowFrustum);
/* For each camera */
TConfigurationNodeIterator itCamera("camera");
for(itCamera = itCamera.begin(&t_tree);
itCamera != itCamera.end();
++itCamera) {
/* Get camera indentifier */
std::string strId;
GetNodeAttribute(*itCamera, "id", strId);
/* Parse and look up the anchor */
std::string strAnchorId;
GetNodeAttribute(*itCamera, "anchor", strAnchorId);
SAnchor& sAnchor = m_pcEmbodiedEntity->GetAnchor(strAnchorId);
/* parse the offset */
CVector3 cOffsetPosition;
CQuaternion cOffsetOrientation;
GetNodeAttribute(*itCamera, "position", cOffsetPosition);
GetNodeAttribute(*itCamera, "orientation", cOffsetOrientation);
CTransformationMatrix3 cOffset(cOffsetOrientation, cOffsetPosition);
/* parse the range */
CRange<Real> cRange;
GetNodeAttribute(*itCamera, "range", cRange);
/* create the projection matrix */
CSquareMatrix<3> cProjectionMatrix;
cProjectionMatrix.SetIdentityMatrix();
/* set the focal length */
CVector2 cFocalLength;
GetNodeAttribute(*itCamera, "focal_length", cFocalLength);
cProjectionMatrix(0,0) = cFocalLength.GetX(); // Fx
cProjectionMatrix(1,1) = cFocalLength.GetY(); // Fy
/* set the principal point */
CVector2 cPrincipalPoint;
GetNodeAttribute(*itCamera, "principal_point", cPrincipalPoint);
cProjectionMatrix(0,2) = cPrincipalPoint.GetX(); // Px
cProjectionMatrix(1,2) = cPrincipalPoint.GetY(); // Py
/* set the distortion parameters */
/*
CMatrix<1,5> cDistortionParameters;
std::string strDistortionParameters;
Real pfDistortionParameters[3];
GetNodeAttribute(*itCamera, "distortion_parameters", strDistortionParameters);
ParseValues<Real>(strDistortionParameters, 3, pfDistortionParameters, ',');
cDistortionParameters(0,0) = pfDistortionParameters[0]; // K1
cDistortionParameters(0,1) = pfDistortionParameters[1]; // K2
cDistortionParameters(0,4) = pfDistortionParameters[2]; // K3
*/
/* parse the resolution */
CVector2 cResolution;
GetNodeAttribute(*itCamera, "resolution", cResolution);
/* create and initialise the algorithms */
std::vector<CCameraSensorSimulatedAlgorithm*> vecSimulatedAlgorithms;
std::vector<CCI_CameraSensorAlgorithm*> vecAlgorithms;
TConfigurationNodeIterator itAlgorithm;
for(itAlgorithm = itAlgorithm.begin(&(*itCamera));
itAlgorithm != itAlgorithm.end();
++itAlgorithm) {
/* create the algorithm */
CCameraSensorSimulatedAlgorithm* pcAlgorithm =
CFactory<CCameraSensorSimulatedAlgorithm>::New(itAlgorithm->Value());
/* check that algorithm inherits from a control interface */
CCI_CameraSensorAlgorithm* pcCIAlgorithm =
dynamic_cast<CCI_CameraSensorAlgorithm*>(pcAlgorithm);
if(pcCIAlgorithm == nullptr) {
THROW_ARGOSEXCEPTION("Algorithm \"" << itAlgorithm->Value() <<
"\" does not inherit from CCI_CameraSensorAlgorithm");
}
/* initialize the algorithm's control interface */
pcCIAlgorithm->Init(*itAlgorithm);
/* store pointers to the algorithms */
vecSimulatedAlgorithms.push_back(pcAlgorithm);
vecAlgorithms.push_back(pcCIAlgorithm);
}
/* create the simulated sensor */
m_vecSensors.emplace_back(sAnchor, cOffset, cRange, cProjectionMatrix,
cResolution, vecSimulatedAlgorithms);
/* create the sensor's control interface */
m_vecInterfaces.emplace_back(strId, vecAlgorithms);
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing camera sensor", ex);
}
Update();
}
