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 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 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);
}
}
void CPositionalEntity::Init(TConfigurationNode& t_tree) {
try {
/* Initialize base entity */
CEntity::Init(t_tree);
/* Get the position of the entity */
GetNodeAttributeOrDefault(t_tree, "position", m_cPosition, CVector3());
/* Get the orientation of the entity */
GetNodeAttributeOrDefault(t_tree, "orientation", m_cOrientation, CQuaternion());
m_cInitPosition = m_cPosition;
m_cInitOrientation = m_cOrientation;
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize positional entity \"" << GetId() << "\".", ex);
}
}
void CEPuckLightSensor::Init(TConfigurationNode& t_tree) {
try {
CCI_EPuckLightSensor::Init(t_tree);
/* Show rays? */
GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
/* Getting noise */
GetNodeAttributeOrDefault(t_tree, "noise_level", m_fNoiseLevel, m_fNoiseLevel);
m_fNoiseLevel *= 1024.0f;
/* Random number generator*/
m_pcRNG = CARGoSRandom::CreateRNG("argos");
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Initialization error in e-puck light sensor.", ex);
}
}
void CRABEquippedEntity::Init(TConfigurationNode& t_tree) {
try {
/*
* Init entity.
* Here we explicitly avoid to call CPositionalEntity::Init() because that
* would also initialize position and orientation, which, instead, must
* be calculated from reference entity and offsets.
*/
CEntity::Init(t_tree);
/* Get offsets */
GetNodeAttributeOrDefault(t_tree, "pos_offset", m_cPosOffset, m_cPosOffset);
std::string strRotOffset;
GetNodeAttributeOrDefault(t_tree, "rot_offset", strRotOffset, strRotOffset);
if(strRotOffset != "") {
CDegrees cRotOffsetEuler[3];
ParseValues(strRotOffset, 3, cRotOffsetEuler, ',');
m_cRotOffset.FromEulerAngles(ToRadians(cRotOffsetEuler[0]),
ToRadians(cRotOffsetEuler[1]),
ToRadians(cRotOffsetEuler[2]));
}
/* Parse and look up the anchor */
std::string strAnchorId;
GetNodeAttribute(t_tree, "anchor", strAnchorId);
/*
* NOTE: here we get a reference to the embodied entity
* This line works under the assumption that:
* 1. the RABEquippedEntity 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.
*/
m_pcEntityBody = &GetParent().GetComponent<CEmbodiedEntity>("body");
m_psAnchor = &m_pcEntityBody->GetAnchor(strAnchorId);
/* Get message size */
size_t unMsgSize;
GetNodeAttribute(t_tree, "msg_size", unMsgSize);
m_cData.Resize(unMsgSize);
/* Get transmission range */
GetNodeAttribute(t_tree, "range", m_fRange);
/* Set init position and orientation */
Update();
SetInitPosition(GetPosition());
SetInitOrientation(GetOrientation());
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing a range and bearing entity \"" << GetId() << "\"", ex);
}
}
void CQTOpenGLMainWindow::CreateOpenGLWidget(TConfigurationNode& t_tree) {
/* Create user functions */
m_pcUserFunctions = CreateUserFunctions(t_tree);
/* Set up OpenGL features */
QGLFormat cGLFormat = QGLFormat::defaultFormat();
cGLFormat.setSampleBuffers(m_pcToggleAntiAliasingAction->isChecked());
cGLFormat.setStencil(false);
QGLFormat::setDefaultFormat(cGLFormat);
/* Create the widget */
QWidget* pcPlaceHolder = new QWidget(this);
m_pcOpenGLWidget = new CQTOpenGLWidget(pcPlaceHolder, *m_pcUserFunctions);
m_pcOpenGLWidget->setCursor(QCursor(Qt::OpenHandCursor));
m_pcOpenGLWidget->GetCamera().Init(t_tree);
m_pcOpenGLWidget->GetFrameGrabData().Init(t_tree);
m_pcToggleAntiAliasingAction->setChecked(cGLFormat.sampleBuffers());
/* Invert mouse controls? */
bool bInvertMouse;
GetNodeAttributeOrDefault(t_tree, "invert_mouse", bInvertMouse, false);
m_pcOpenGLWidget->SetInvertMouse(bInvertMouse);
/* Set the window as the central widget */
CQTOpenGLLayout* pcQTOpenGLLayout = new CQTOpenGLLayout();
pcQTOpenGLLayout->addWidget(m_pcOpenGLWidget);
pcPlaceHolder->setLayout(pcQTOpenGLLayout);
setCentralWidget(pcPlaceHolder);
}
void CKilobotDiffusion::Init(TConfigurationNode& t_node) {
/*
* Get sensor/actuator handles
*
* The passed string (ex. "differential_steering") 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:
*
* $ argos3 -q actuators
*
* to have a list of all the possible actuators, or
*
* $ argos3 -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 configuration
* file at the <controllers><kilobot_diffusion><actuators> and
* <controllers><kilobot_diffusion><sensors> sections. If you forgot to
* list a device in the XML and then you request it here, an error
* occurs.
*/
// Get sensor/actuator handles
m_pcMotors = GetActuator<CCI_DifferentialSteeringActuator>("differential_steering");
// Parse the configuration file
GetNodeAttributeOrDefault(t_node, "max_motion_steps", m_unMaxMotionSteps, m_unMaxMotionSteps );
if( m_unMaxMotionSteps == 0 ) {
LOGERR << "[FATAL] Invalid value for num_moving_steps (" << m_unMaxMotionSteps << "). Should be a positive integer." << std::endl;
}
Reset();
}
void CDynamics3DEngine::Init(TConfigurationNode& t_tree) {
/* Init parent */
CPhysicsEngine::Init(t_tree);
/* Parse the XML */
GetNodeAttributeOrDefault(t_tree, "gravity", m_cGravity, CVector3(0.0f, 0.0f, -9.81f));
GetNodeAttributeOrDefault<Real>(t_tree, "erp", m_fERP, 0.8);
GetNodeAttributeOrDefault<Real>(t_tree, "cfm", m_fCFM, 0.01);
GetNodeAttributeOrDefault<UInt32>(t_tree, "iterations", m_unIterations, 20);
GetNodeAttributeOrDefault<size_t>(t_tree, "max_contacts", m_unMaxContacts, 32);
/* Init ODE stuff */
m_tWorldID = dWorldCreate();
m_tSpaceID = dHashSpaceCreate(0);
dSpaceSetSublevel(m_tSpaceID, 0);
m_tContactJointGroupID = dJointGroupCreate(0);
dWorldSetGravity(m_tWorldID, 0.0f, 0.0f, -9.81f);
dWorldSetERP(m_tWorldID, m_fERP);
dWorldSetCFM(m_tWorldID, m_fCFM);
dWorldSetQuickStepNumIterations(m_tWorldID, m_unIterations);
dInitODE();
/* Initialize contact information */
m_ptContacts = new dContact[m_unMaxContacts];
for(UInt32 i = 0; i < m_unMaxContacts; ++i) {
::memset(&(m_ptContacts[i].surface), 0, sizeof(dSurfaceParameters));
m_ptContacts[i].surface.mode = dContactMu2;
m_ptContacts[i].surface.mu = dInfinity;
m_ptContacts[i].surface.mu2 = dInfinity;
}
/* Add a planar floor */
m_tFloor = dCreatePlane(m_tSpaceID, 0, 0, 1.0f, 0.0f);
/* Set the random seed from a random number taken from ARGoS RNG */
m_pcRNG = CARGoSRandom::CreateRNG("argos");
dRandSetSeed(m_pcRNG->Uniform(CRange<UInt32>(1,65535)));
}
void CQTOpenGLCamera::SSettings::Init(TConfigurationNode& t_tree) {
/* Get positional attributes */
GetNodeAttribute(t_tree, "position", Position);
GetNodeAttribute(t_tree, "look_at", Target);
/* Calculate the Forward vector */
Forward = (Target - Position).Normalize();
/* Calculate the Left vector
It is located on a plane parallel to XY
It is perpendicular to the projection of Forward on that plane
*/
if(Forward.GetX() != 0 && Forward.GetY() != 0) {
CVector2 cLeftXY(Forward.GetX(), Forward.GetY());
cLeftXY.Perpendicularize();
Left.Set(cLeftXY.GetX(), cLeftXY.GetY(), 0.0f);
Left.Normalize();
}
else {
Left.Set(0.0f, 1.0f, 0.0f);
}
/* Calculate the Up vector with a cross product */
Up = Forward;
Up.CrossProduct(Left).Normalize();
/* Get optional optics parameters */
Real fValue;
GetNodeAttributeOrDefault(t_tree, "lens_focal_length", fValue, 20.0f);
LensFocalLength = fValue * 0.001f;
CalculateYFieldOfView();
}
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 CFootBotDistanceScannerRotZOnlySensor::Init(TConfigurationNode& t_tree) {
try {
CCI_FootBotDistanceScannerSensor::Init(t_tree);
/* Show rays? */
GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
/* Noise range */
GetNodeAttributeOrDefault(t_tree, "noise_range", m_cNoiseRange, m_cNoiseRange);
if(m_cNoiseRange.GetSpan() > 0.0f) {
m_bAddNoise = true;
m_pcRNG = CRandom::CreateRNG("argos");
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Initialization error in foot-bot distance scanner rot_z_only sensor.", ex);
}
}
void CFootBotMotorGroundSensor::Init(TConfigurationNode& t_tree) {
try {
/* Parse noise level */
GetNodeAttributeOrDefault(t_tree, "noise_level", m_fNoiseLevel, m_fNoiseLevel);
m_fNoiseLevel *= FOOTBOT_MOTOR_GROUND_SENSOR_READING_RANGE.GetSpan();
/* Parse calibration flag */
GetNodeAttributeOrDefault(t_tree, "calibrate", m_bCalibrated, m_bCalibrated);
if( !m_bCalibrated ) LOGERR << "[WARNING] Using motor ground sensors without calibration" << std::endl;
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Initialization error in foot-bot motor ground sensor", ex);
}
/* Random number generator*/
m_pcRNG = CARGoSRandom::CreateRNG("argos");
}
void CRABMedium::Init(TConfigurationNode& t_tree) {
try {
CMedium::Init(t_tree);
/* Check occlusions? */
GetNodeAttributeOrDefault(t_tree, "check_occlusions", m_bCheckOcclusions, m_bCheckOcclusions);
/* Get the positional index method */
std::string strPosIndexMethod("grid");
GetNodeAttributeOrDefault(t_tree, "index", strPosIndexMethod, strPosIndexMethod);
/* Get the arena center and size */
CVector3 cArenaCenter;
CVector3 cArenaSize;
TConfigurationNode& tArena = GetNode(CSimulator::GetInstance().GetConfigurationRoot(), "arena");
GetNodeAttribute(tArena, "size", cArenaSize);
GetNodeAttributeOrDefault(tArena, "center", cArenaCenter, cArenaCenter);
/* Create the positional index for embodied entities */
if(strPosIndexMethod == "grid") {
size_t punGridSize[3];
if(!NodeAttributeExists(t_tree, "grid_size")) {
punGridSize[0] = cArenaSize.GetX();
punGridSize[1] = cArenaSize.GetY();
punGridSize[2] = cArenaSize.GetZ();
}
else {
std::string strPosGridSize;
GetNodeAttribute(t_tree, "grid_size", strPosGridSize);
ParseValues<size_t>(strPosGridSize, 3, punGridSize, ',');
}
CGrid<CRABEquippedEntity>* pcGrid = new CGrid<CRABEquippedEntity>(
cArenaCenter - cArenaSize * 0.5f, cArenaCenter + cArenaSize * 0.5f,
punGridSize[0], punGridSize[1], punGridSize[2]);
m_pcRABEquippedEntityGridUpdateOperation = new CRABEquippedEntityGridEntityUpdater(*pcGrid);
pcGrid->SetUpdateEntityOperation(m_pcRABEquippedEntityGridUpdateOperation);
m_pcRABEquippedEntityIndex = pcGrid;
}
else {
THROW_ARGOSEXCEPTION("Unknown method \"" << strPosIndexMethod << "\" for the positional index.");
}
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error in initialization of the range-and-bearing medium", ex);
}
}
void CFootBotLightRotZOnlySensor::Init(TConfigurationNode& t_tree) {
try {
/* Show rays? */
GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
/* Parse noise level */
Real fNoiseLevel = 0.0f;
GetNodeAttributeOrDefault(t_tree, "noise_level", fNoiseLevel, fNoiseLevel);
if(fNoiseLevel < 0.0f) {
THROW_ARGOSEXCEPTION("Can't specify a negative value for the noise level of the light sensor");
}
else if(fNoiseLevel > 0.0f) {
m_bAddNoise = true;
m_cNoiseRange.Set(-fNoiseLevel, fNoiseLevel);
m_pcRNG = CRandom::CreateRNG("argos");
}
m_tReadings.resize(m_pcLightEntity->GetNumSensors());
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Initialization error in rot_z_only light sensor", ex);
}
}
void CTargetEntity::Init(TConfigurationNode& t_tree) {
try {
CComposableEntity::Init(t_tree);
embodiedEntity = new CEmbodiedEntity(this);
AddComponent(*embodiedEntity);
embodiedEntity->Init(GetNode(t_tree, "body"));
ledEquippedEntity = new CLEDEquippedEntity(this, ledsId);
AddComponent(*ledEquippedEntity);
ledEquippedEntity->AddLEDRing(
CVector3(0.0f, 0.0f, LED_RING_ELEVATION),
LED_RING_RADIUS,
LED_RING_START_ANGLE,
8,
embodiedEntity->GetOriginAnchor());
Real fRange = 0.8f;
GetNodeAttributeOrDefault(t_tree, "rab_range", fRange, fRange);
UInt32 unDataSize = 2;
GetNodeAttributeOrDefault(t_tree, "rab_data_size", unDataSize, unDataSize);
rabEquippedEntity = new CRABEquippedEntity(this,
rabId,
unDataSize,
fRange,
embodiedEntity->GetOriginAnchor(),
*embodiedEntity,
CVector3(0.0f, 0.0f, RAB_ELEVATION));
AddComponent(*rabEquippedEntity);
controllableEntity = new CControllableEntity(this);
AddComponent(*controllableEntity);
controllableEntity->Init(GetNode(t_tree, "controller"));
UpdateComponents();
}
catch (CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", 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);
}
}
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);
}
}
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 CFootBotBaseGroundRotZOnlySensor::Init(TConfigurationNode& t_tree) {
try {
CCI_FootBotBaseGroundSensor::Init(t_tree);
/* Parse noise level */
Real fNoiseLevel = 0.0f;
GetNodeAttributeOrDefault(t_tree, "noise_level", fNoiseLevel, fNoiseLevel);
if(fNoiseLevel < 0.0f) {
THROW_ARGOSEXCEPTION("Can't specify a negative value for the noise level of the foot-bot ground sensor");
}
else if(fNoiseLevel > 0.0f) {
m_bAddNoise = true;
m_cNoiseRange.Set(-fNoiseLevel, fNoiseLevel);
m_pcRNG = CRandom::CreateRNG("argos");
}
m_tReadings.resize(8);
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Initialization error in foot-bot rotzonly ground sensor", ex);
}
}
void CSpiriEntity::Init(TConfigurationNode& t_tree) {
try {
/*
* Init parent
*/
CComposableEntity::Init(t_tree);
/*
* Create and init components
*/
/*
* Embodied entity
* Better to put this first, because many other entities need this one
*/
m_pcEmbodiedEntity = new CEmbodiedEntity(this);
AddComponent(*m_pcEmbodiedEntity);
m_pcEmbodiedEntity->Init(GetNode(t_tree, "body"));
/* Quadrotor entity */
m_pcQuadRotorEntity = new CQuadRotorEntity(this, "quadrotor_0");
AddComponent(*m_pcQuadRotorEntity);
/* RAB equipped entity */
Real fRABRange = 3.0f;
GetNodeAttributeOrDefault(t_tree, "rab_range", fRABRange, fRABRange);
UInt32 unRABDataSize = 10;
GetNodeAttributeOrDefault(t_tree, "rab_data_size", unRABDataSize, unRABDataSize);
SAnchor& cRABAnchor = m_pcEmbodiedEntity->AddAnchor("rab", CVector3(0.0f, 0.0f, RAB_ELEVATION));
m_pcRABEquippedEntity = new CRABEquippedEntity(
this,
"rab_0",
unRABDataSize,
fRABRange,
cRABAnchor,
*m_pcEmbodiedEntity);
AddComponent(*m_pcRABEquippedEntity);
m_pcEmbodiedEntity->EnableAnchor("rab");
/* Perspective camera */
CDegrees cCameraAperture(30.0f);
GetNodeAttributeOrDefault(t_tree, "camera_aperture", cCameraAperture, cCameraAperture);
Real fCameraRange = 10.0f;
GetNodeAttributeOrDefault(t_tree, "camera_range", fCameraRange, fCameraRange);
SAnchor& cCameraAnchor =
m_pcEmbodiedEntity->AddAnchor(
"camera",
CVector3(BODY_RADIUS, 0.0f, 0.0f));
m_pcPerspectiveCameraEquippedEntity = new CPerspectiveCameraEquippedEntity(
this,
"perspective_camera_0",
ToRadians(cCameraAperture),
0.035f,
fCameraRange,
640, 480,
cCameraAnchor);
AddComponent(*m_pcPerspectiveCameraEquippedEntity);
m_pcEmbodiedEntity->EnableAnchor("camera");
/* Controllable entity
It must be the last one, for actuators/sensors to link to composing entities correctly */
m_pcControllableEntity = new CControllableEntity(this);
AddComponent(*m_pcControllableEntity);
m_pcControllableEntity->Init(GetNode(t_tree, "controller"));
/* Update components */
UpdateComponents();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", 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 CDynamics3DGravityPlugin::Init(TConfigurationNode& t_tree) {
GetNodeAttributeOrDefault(t_tree, "g", m_fAcceleration, m_fAcceleration);
}
void CKinematics2DEngine::Init(TConfigurationNode& t_tree) {
/* Init parent */
CPhysicsEngine::Init(t_tree);
GetNodeAttributeOrDefault(t_tree, "check_collisions", m_bDetectCollisions, m_bDetectCollisions);
}
void CEyeBotEntity::Init(TConfigurationNode& t_tree) {
try {
/*
* Init parent
*/
CComposableEntity::Init(t_tree);
/*
* Create and init components
*/
/*
* Embodied entity
* Better to put this first, because many other entities need this one
*/
m_pcEmbodiedEntity = new CEmbodiedEntity(this);
AddComponent(*m_pcEmbodiedEntity);
m_pcEmbodiedEntity->Init(GetNode(t_tree, "body"));
/* Quadrotor entity */
m_pcQuadRotorEntity = new CQuadRotorEntity(this, "quadrotor_0");
AddComponent(*m_pcQuadRotorEntity);
/* LED equipped entity, with LEDs [0-11] and beacon [12] */
m_pcLEDEquippedEntity = new CLEDEquippedEntity(this, "leds_0");
m_pcLEDEquippedEntity->AddLEDRing(
CVector3(0.0f, 0.0f, LED_LOWER_RING_ELEVATION),
LED_RING_RADIUS,
HALF_LED_ANGLE_SLICE,
16,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcLEDEquippedEntity->AddLEDRing(
CVector3(0.0f, 0.0f, LED_UPPER_RING_ELEVATION),
LED_RING_RADIUS,
HALF_LED_ANGLE_SLICE,
16,
m_pcEmbodiedEntity->GetOriginAnchor());
CVector3 cLEDPos(LED_RING_RADIUS * 0.7f,
0.0f,
LED_LOWER_RING_ELEVATION - 0.01f);
cLEDPos.RotateZ(3.0f * CRadians::PI_OVER_FOUR);
m_pcLEDEquippedEntity->AddLED(
cLEDPos,
m_pcEmbodiedEntity->GetOriginAnchor());
AddComponent(*m_pcLEDEquippedEntity);
/* Proximity sensor equipped entity */
m_pcProximitySensorEquippedEntity =
new CProximitySensorEquippedEntity(this,
"proximity_0");
AddComponent(*m_pcProximitySensorEquippedEntity);
m_pcProximitySensorEquippedEntity->AddSensorRing(
CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
PROXIMITY_SENSOR_RING_RADIUS,
PROXIMITY_SENSOR_RING_START_ANGLE,
PROXIMITY_SENSOR_RING_RANGE,
24,
m_pcEmbodiedEntity->GetOriginAnchor());
/* Light sensor equipped entity */
m_pcLightSensorEquippedEntity =
new CLightSensorEquippedEntity(this,
"light_0");
AddComponent(*m_pcLightSensorEquippedEntity);
m_pcLightSensorEquippedEntity->AddSensorRing(
CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
PROXIMITY_SENSOR_RING_RADIUS,
PROXIMITY_SENSOR_RING_START_ANGLE,
PROXIMITY_SENSOR_RING_RANGE,
24,
m_pcEmbodiedEntity->GetOriginAnchor());
/* RAB equipped entity */
Real fRange = 3.0f;
GetNodeAttributeOrDefault(t_tree, "rab_range", fRange, fRange);
m_pcRABEquippedEntity = new CRABEquippedEntity(
this,
"rab_0",
10,
fRange,
m_pcEmbodiedEntity->GetOriginAnchor(),
*m_pcEmbodiedEntity);
AddComponent(*m_pcRABEquippedEntity);
/* Controllable entity
It must be the last one, for actuators/sensors to link to composing entities correctly */
m_pcControllableEntity = new CControllableEntity(this);
AddComponent(*m_pcControllableEntity);
m_pcControllableEntity->Init(GetNode(t_tree, "controller"));
/* Update components */
UpdateComponents();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
}
}
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();
}
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);
}
}