示例#1
0
 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);
    }
 }
示例#2
0
 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);
    }
 }
示例#3
0
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);
   }
}
示例#4
0
 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);
    }
 }
示例#5
0
 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);
    }
 }
示例#6
0
 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);
    }
 }
示例#7
0
 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();
}
示例#9
0
 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)));
 }
示例#10
0
 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();
 }
示例#11
0
 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");
   }
示例#14
0
 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);
    }
 }
示例#19
0
 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);
    }
 }
示例#20
0
   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);
      }
   }
示例#21
0
   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);
    }
 }
示例#23
0
 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);
    }
 }
示例#24
0
 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);
    }
 }
示例#25
0
 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);
 } 
示例#27
0
  void CKinematics2DEngine::Init(TConfigurationNode& t_tree) {
    /* Init parent */
    CPhysicsEngine::Init(t_tree);

    GetNodeAttributeOrDefault(t_tree, "check_collisions", m_bDetectCollisions, m_bDetectCollisions);
  }
示例#28
0
 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);
    }
 }
示例#29
0
 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);
    }
 }