void CBuzzController::SetBytecode(const std::string& str_fname) { /* Reset the BuzzVM */ if(m_tBuzzVM) buzzvm_destroy(&m_tBuzzVM); m_tBuzzVM = buzzvm_new(m_unRobotId); /* Save the bytecode filename */ m_strBytecodeFName = str_fname; /* Load the bytecode */ std::ifstream cBCodeFile(str_fname.c_str(), std::ios::binary | std::ios::ate); if(cBCodeFile.fail()) { THROW_ARGOSEXCEPTION("Can't open file \"" << str_fname << "\": " << strerror(errno)); } std::ifstream::pos_type unFileSize = cBCodeFile.tellg(); m_cBytecode.Clear(); m_cBytecode.Resize(unFileSize); cBCodeFile.seekg(0, std::ios::beg); cBCodeFile.read(reinterpret_cast<char*>(m_cBytecode.ToCArray()), unFileSize); /* Load the script */ buzzvm_set_bcode(m_tBuzzVM, m_cBytecode.ToCArray(), m_cBytecode.Size()); if(buzzvm_set_bcode(m_tBuzzVM, m_cBytecode.ToCArray(), m_cBytecode.Size()) != BUZZVM_STATE_READY) { THROW_ARGOSEXCEPTION("Error loading Buzz script \"" << str_fname << "\": " << buzzvm_strerror(m_tBuzzVM)); } /* Register basic function */ if(RegisterFunctions() != BUZZVM_STATE_READY) { THROW_ARGOSEXCEPTION("Error while registering functions"); } /* Execute the global part of the script */ buzzvm_execute_script(m_tBuzzVM); /* Call the Init() function */ buzzvm_function_call(m_tBuzzVM, "init", 0); }
virtual CColor GetColorAtPoint(Real f_x, Real f_y) { /* Compute coordinates on the image */ UInt32 x = (f_x + m_cHalfArenaSize.GetX()) * m_fArenaToImageCoordinateXFactor; UInt32 y = (f_y + m_cHalfArenaSize.GetY()) * m_fArenaToImageCoordinateYFactor; /* Check the bit depth */ if(m_cImage.getBitsPerPixel() <= 8) { RGBQUAD* ptColorPalette; BYTE tPixelIndex; /* 1, 4 or 8 bits per pixel */ if(! m_cImage.getPixelIndex(x, y, &tPixelIndex)) { THROW_ARGOSEXCEPTION("Unable to access image pixel at (" << x << "," << y << "). Image size (" << m_cImage.getWidth() << "," << m_cImage.getHeight() << ")"); } ptColorPalette = m_cImage.getPalette(); return CColor(ptColorPalette[tPixelIndex].rgbRed, ptColorPalette[tPixelIndex].rgbGreen, ptColorPalette[tPixelIndex].rgbBlue); } else { /* 16, 24 or 32 bits per pixel */ RGBQUAD tColorPixel; if(! m_cImage.getPixelColor(x, y, &tColorPixel)) { THROW_ARGOSEXCEPTION("Unable to access image pixel at (" << x << "," << y << "). Image size (" << m_cImage.getWidth() << "," << m_cImage.getHeight() << ")"); } return CColor(tColorPixel.rgbRed, tColorPixel.rgbGreen, tColorPixel.rgbBlue); } }
CQTOpenGLUserFunctions* CQTOpenGLMainWindow::CreateUserFunctions(TConfigurationNode& t_tree) { /* Parse XML for user functions */ if(NodeExists(t_tree, "user_functions")) { /* Use the passed user functions */ /* Get data from XML */ TConfigurationNode tNode = GetNode(t_tree, "user_functions"); std::string strLabel, strLibrary; GetNodeAttribute(tNode, "label", strLabel); GetNodeAttribute(tNode, "library", strLibrary); /* Load the library taking care of the $ARGOSINSTALLDIR variable */ void* ptUserFunctions = ::dlopen(ExpandARGoSInstallDir(strLibrary).c_str(), RTLD_LOCAL | RTLD_LAZY); if(ptUserFunctions == NULL) { THROW_ARGOSEXCEPTION("Failed opening QTOpenGL user function library \"" << strLibrary << "\": " << dlerror() << std::endl); } /* Create the user functions */ if(mapQTOpenGLUserFunctionFactory.find(strLabel) == mapQTOpenGLUserFunctionFactory.end()) { THROW_ARGOSEXCEPTION("Unknown QTOpenGL user function type \"" << strLabel << "\", probably your user functions have been registered with a different name." << std::endl); } return mapQTOpenGLUserFunctionFactory[strLabel](); } else { /* Use standard (empty) user functions */ return new CQTOpenGLUserFunctions; } }
void CSpaceMultiThreadBalanceLength::Init(TConfigurationNode& t_tree) { /* Initialize the space */ CSpace::Init(t_tree); /* Initialize thread related structures */ int nErrors; /* Init mutexes */ if((nErrors = pthread_mutex_init(&m_tStartSenseControlPhaseMutex, NULL)) || (nErrors = pthread_mutex_init(&m_tStartActPhaseMutex, NULL)) || (nErrors = pthread_mutex_init(&m_tStartPhysicsPhaseMutex, NULL)) || (nErrors = pthread_mutex_init(&m_tStartMediaPhaseMutex, NULL)) || (nErrors = pthread_mutex_init(&m_tFetchTaskMutex, NULL))) { THROW_ARGOSEXCEPTION("Error creating thread mutexes " << ::strerror(nErrors)); } /* Init conditionals */ if((nErrors = pthread_cond_init(&m_tStartSenseControlPhaseCond, NULL)) || (nErrors = pthread_cond_init(&m_tStartActPhaseCond, NULL)) || (nErrors = pthread_cond_init(&m_tStartPhysicsPhaseCond, NULL)) || (nErrors = pthread_cond_init(&m_tStartMediaPhaseCond, NULL)) || (nErrors = pthread_cond_init(&m_tFetchTaskCond, NULL))) { THROW_ARGOSEXCEPTION("Error creating thread conditionals " << ::strerror(nErrors)); } /* Reset the idle thread count */ m_unSenseControlPhaseIdleCounter = CSimulator::GetInstance().GetNumThreads(); m_unActPhaseIdleCounter = CSimulator::GetInstance().GetNumThreads(); m_unPhysicsPhaseIdleCounter = CSimulator::GetInstance().GetNumThreads(); m_unMediaPhaseIdleCounter = CSimulator::GetInstance().GetNumThreads(); /* Start threads */ StartThreads(); }
void CARGoSCommandLineArgParser::Parse(SInt32 n_argc, char** ppch_argv) { CCommandLineArgParser::Parse(n_argc, ppch_argv); /* Configure LOG/LOGERR coloring */ if(m_bNonColoredLog) { LOG.DisableColoredOutput(); LOGERR.DisableColoredOutput(); } /* Check whether LOG and LOGERR should go to files */ if(m_strLogFileName != "") { LOG.DisableColoredOutput(); m_cLogFile.open(m_strLogFileName.c_str(), std::ios::trunc | std::ios::out); if(m_cLogFile.fail()) { THROW_ARGOSEXCEPTION("Error opening file \"" << m_strLogFileName << "\""); } m_pcInitLogStream = LOG.GetStream().rdbuf(); LOG.GetStream().rdbuf(m_cLogFile.rdbuf()); } if(m_strLogErrFileName != "") { LOGERR.DisableColoredOutput(); m_cLogErrFile.open(m_strLogErrFileName.c_str(), std::ios::trunc | std::ios::out); if(m_cLogErrFile.fail()) { THROW_ARGOSEXCEPTION("Error opening file \"" << m_strLogErrFileName << "\""); } m_pcInitLogErrStream = LOGERR.GetStream().rdbuf(); LOGERR.GetStream().rdbuf(m_cLogErrFile.rdbuf()); } /* Check that either -h, -c or -q was passed (strictly one of them) */ if(m_strExperimentConfigFile == "" && m_strQuery == "" && ! m_bHelpWanted) { THROW_ARGOSEXCEPTION("No --help, --config-file or --query options specified."); } if((m_strExperimentConfigFile != "" && m_strQuery != "") || (m_strExperimentConfigFile != "" && m_bHelpWanted) || (m_strQuery != "" && m_bHelpWanted)) { THROW_ARGOSEXCEPTION("Options --help, --config-file and --query are mutually exclusive."); } if(m_strExperimentConfigFile != "") { m_eAction = ACTION_RUN_EXPERIMENT; } if(m_strQuery != "") { m_eAction = ACTION_QUERY; } if(m_bHelpWanted) { m_eAction = ACTION_SHOW_HELP; } }
void CQTOpenGLMainWindow::POVRayScenePreview() { try { /* Initialize the POV-Ray working directory */ QDir cDirectory(QDir::tempPath() + "/argos-povray"); /* Erase it if it exists */ if(cDirectory.exists()) { if(::system(QString("rm -rf %1").arg(cDirectory.absolutePath()).toAscii().data()) != 0) { THROW_ARGOSEXCEPTION("Could not remove directory \"" << cDirectory.absolutePath().toAscii().data() << "\"."); } } /* Create the directory */ if(::system(QString("mkdir %1").arg(cDirectory.absolutePath()).toAscii().data()) != 0) { THROW_ARGOSEXCEPTION("Could not create directory \"" << cDirectory.absolutePath().toAscii().data() << "\"."); } /* Now create the XML file that will contain the POV-Ray scene configuration */ QFile cPOVRayXMLConf(cDirectory.absolutePath() + "/argos-povray.xml"); cPOVRayXMLConf.open(QFile::WriteOnly | QFile::Truncate); /* Associate a text stream to perform writing to it */ QTextStream cPOVRayXMLConfStream(&cPOVRayXMLConf); /* Write the configuration */ cPOVRayXMLConfStream << "<povray_render id=\"pov\" output_folder=\"" << cDirectory.absolutePath() << "\">\n"; cPOVRayXMLConfStream << GetPOVRaySceneXMLData(); cPOVRayXMLConfStream << "</povray_render>\n"; cPOVRayXMLConf.close(); /* Now parse this file as an ARGoS TConfigurationTree */ ticpp::Document tPOVRayXMLConfTree(cPOVRayXMLConf.fileName().toAscii().data()); tPOVRayXMLConfTree.LoadFile(); /* It's time to create the POV-Ray visualization */ CPovrayRender cPOVRayRender; cPOVRayRender.Init(*tPOVRayXMLConfTree.FirstChildElement()); /* Write the .pov frame file */ cPOVRayRender.WriteOneFrame(cDirectory.absolutePath().append("/pov/frame.pov").toAscii().data()); /* Eventually, call POV-Ray to render the file */ if(::system(QString("cd %1 && ") .arg(cDirectory.absolutePath()) .append("./render_single_frame_on_pc.sh pov/frame.pov") .toAscii().data()) !=0) { THROW_ARGOSEXCEPTION("Could not create POV-Ray preview"); } } catch(CARGoSException& ex) { QString strError = QString("Error creating POV-Ray preview\n%1").arg(QString(ex.what())); QMessageBox::critical(this, tr("ARGoS v2.0"), strError, QMessageBox::Ok); } }
void CEntity::Init(TConfigurationNode& t_tree) { try { /* * Set the id of the entity from XML or type description */ /* Was an id specified explicitly? */ if(NodeAttributeExists(t_tree, "id")) { /* Yes, use that */ GetNodeAttribute(t_tree, "id", m_strId); } else { /* No, derive it from the parent */ if(m_pcParent != NULL) { UInt32 unIdCount = 0; while(GetParent().HasComponent(GetTypeDescription() + "[" + GetTypeDescription() + "_" + ToString(unIdCount) + "]")) { ++unIdCount; } m_strId = GetTypeDescription() + "_" + ToString(unIdCount); } else { THROW_ARGOSEXCEPTION("Root entities must provide the identifier tag"); } } } catch(CARGoSException& ex) { THROW_ARGOSEXCEPTION_NESTED("Failed to initialize an entity.", ex); } }
CEntity& CFootBotEntity::GetComponent(const std::string& str_component) { if(str_component == "embodied_entity") { return *m_pcEmbodiedEntity; } else if(str_component == "controllable_entity") { return *m_pcControllableEntity; } else if(str_component == "wheeled_entity<2>") { return *m_pcWheeledEntity; } else if(str_component == "led_equipped_entity") { return *m_pcLEDEquippedEntity; } else if(str_component == "gripper_equipped_entity") { return *m_pcGripperEquippedEntity; } else if(str_component == "distance_scanner_equipped_entity") { return *m_pcDistanceScannerEquippedEntity; } else if(str_component == "rab_equipped_entity<10>") { return *m_pcRABEquippedEntity; } else if(str_component == "wifi_equipped_entity") { return *m_pcWiFiEquippedEntity; } else { THROW_ARGOSEXCEPTION("A foot-bot does not have a component of type \"" << str_component << "\""); } }
void CSimulator::InitMedia(TConfigurationNode& t_tree) { try { /* Cycle through the media */ TConfigurationNodeIterator itMedia; for(itMedia = itMedia.begin(&t_tree); itMedia != itMedia.end(); ++itMedia) { /* Create the medium */ CMedium* pcMedium = CFactory<CMedium>::New(itMedia->Value()); try { /* Initialize the medium */ pcMedium->Init(*itMedia); /* Check that an medium with that ID does not exist yet */ if(m_mapMedia.find(pcMedium->GetId()) == m_mapMedia.end()) { /* Add it to the lists */ m_mapMedia[pcMedium->GetId()] = pcMedium; m_vecMedia.push_back(pcMedium); } else { /* Duplicate id -> error */ THROW_ARGOSEXCEPTION("A medium with id \"" << pcMedium->GetId() << "\" exists already. The ids must be unique!"); } } catch(CARGoSException& ex) { /* Error while executing medium init, destroy what done to prevent memory leaks */ pcMedium->Destroy(); delete pcMedium; THROW_ARGOSEXCEPTION_NESTED("Error initializing medium type \"" << itMedia->Value() << "\"", ex); } } } catch(CARGoSException& ex) { THROW_ARGOSEXCEPTION_NESTED("Failed to initialize the media. Parse error in the <media> subtree.", ex); } }
void CFloorEntity::Init(TConfigurationNode& t_tree) { /* Init parent */ CEntity::Init(t_tree); /* Get arena size */ m_cFloorSize = CSimulator::GetInstance().GetSpace().GetArenaSize(); /* Parse XML */ GetNodeAttribute(t_tree, "source", m_strColorSource); if(m_strColorSource == "image") { std::string strPath; GetNodeAttribute(t_tree, "path", strPath); strPath = ExpandARGoSInstallDir(strPath); m_pcColorSource = new CFloorColorFromImageFile(strPath, m_cFloorSize.GetX(), m_cFloorSize.GetY()); } else if(m_strColorSource == "loop_functions") { GetNodeAttribute(t_tree, "pixels_per_meter", m_unPixelsPerMeter); m_pcColorSource = new CFloorColorFromLoopFunctions(m_unPixelsPerMeter, m_cFloorSize.GetX(), m_cFloorSize.GetY()); } else { THROW_ARGOSEXCEPTION("Unknown image source \"" << m_strColorSource << "\" for the floor entity \"" << GetId() << "\""); } }
void CLandmarks::Destroy() { /* Close the output file */ m_cOutFile.close(); if(m_cOutFile.fail()) { THROW_ARGOSEXCEPTION("Error closing file \"" << m_strOutFile << "\""); } }
void CSimulator::InitPhysics(TConfigurationNode& t_tree) { try { /* Cycle through the physics engines */ TConfigurationNodeIterator itEngines; for(itEngines = itEngines.begin(&t_tree); itEngines != itEngines.end(); ++itEngines) { /* Create the physics engine */ CPhysicsEngine* pcEngine = CFactory<CPhysicsEngine>::New(itEngines->Value()); try { /* Initialize the engine */ pcEngine->Init(*itEngines); /* Check that an engine with that ID does not exist yet */ if(m_mapPhysicsEngines.find(pcEngine->GetId()) == m_mapPhysicsEngines.end()) { /* Add it to the lists */ m_mapPhysicsEngines[pcEngine->GetId()] = pcEngine; m_vecPhysicsEngines.push_back(pcEngine); } else { /* Duplicate id -> error */ THROW_ARGOSEXCEPTION("A physics engine with id \"" << pcEngine->GetId() << "\" exists already. The ids must be unique!"); } } catch(CARGoSException& ex) { /* Error while executing engine init, destroy what done to prevent memory leaks */ pcEngine->Destroy(); delete pcEngine; THROW_ARGOSEXCEPTION_NESTED("Error initializing physics engine type \"" << itEngines->Value() << "\"", ex); } } } catch(CARGoSException& ex) { THROW_ARGOSEXCEPTION_NESTED("Failed to initialize the physics engines. Parse error in the <physics_engines> subtree.", ex); } }
void CLandmarks::Reset() { /* Set to false the target flags */ m_vecAtTarget = std::vector<bool>(TARGETS, false); /* Close the output file */ m_cOutFile.close(); if(m_cOutFile.fail()) { THROW_ARGOSEXCEPTION("Error closing file \"" << m_strOutFile << "\""); } /* Open the file for text writing */ m_cOutFile.open(m_strOutFile.c_str(), std::ofstream::out | std::ofstream::trunc); if(m_cOutFile.fail()) { THROW_ARGOSEXCEPTION("Error opening file \"" << m_strOutFile << "\""); } /* Write a header line */ m_cOutFile << "# Clock\tIn Chain\tOn Target" << std::endl; }
void CBTFootbotRecruiterRootBehavior::Step(CCI_FootBotState& c_robot_state) { // Update Data UpdateFoodData(); UpdateStateData(); // Decide which behaviour to execute switch(m_sStateData.State) { case SStateData::STATE_EXPLORING: { Explore(); break; } case SStateData::STATE_SIGNAL_AND_PICK_UP: { SignalPickUp(); break; } case SStateData::STATE_DROP: { Drop(); break; } case SStateData::STATE_RETURN_TO_NEST: { ReturnToNest(); break; } case SStateData::STATE_GO_TO_FOOD: { GoToFood(); break; } default: { THROW_ARGOSEXCEPTION("Invalid State"); } } }
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); } }
CPhysicsModel& CEmbodiedEntity::GetPhysicsModel(const std::string& str_engine_id) { CPhysicsModel::TMap::iterator it = m_tPhysicsModelMap.find(str_engine_id); if(it == m_tPhysicsModelMap.end()) { THROW_ARGOSEXCEPTION("Entity \"" << GetContext() << GetId() << "\" has no associated entity in physics engine \"" << str_engine_id << "\""); } return *(it->second); }
const SBoundingBox& CEmbodiedEntity::GetBoundingBox() const { if(GetPhysicsModelsNum() == 0) { /* No engine associated to this entity */ THROW_ARGOSEXCEPTION("CEmbodiedEntity::GetBoundingBox() : entity \"" << GetContext() << GetId() << "\" is not associated to any engine"); } return *m_sBoundingBox; }
CDirectionalLEDMedium& CDirectionalLEDEntity::GetMedium() const { if(m_pcMedium == nullptr) { THROW_ARGOSEXCEPTION("directional LED entity \"" << GetContext() << GetId() << "\" has no associated medium."); } return *m_pcMedium; }
TConfigurationNode& CSimulator::GetConfigForController(const std::string& str_id) { TControllerConfigurationMap::iterator it = m_mapControllerConfig.find(str_id); if(it == m_mapControllerConfig.end()) { THROW_ARGOSEXCEPTION("Can't find XML configuration for controller id \"" << str_id << "\""); } return *(it->second); }
void CRABEquippedEntity::SetData(const CByteArray& c_data) { if(m_cData.Size() == c_data.Size()) { m_cData = c_data; } else { THROW_ARGOSEXCEPTION("CRABEquippedEntity::SetData() : data size does not match, expected " << m_cData.Size() << ", got " << c_data.Size()); } }
CCI_Actuator* CActuatorsFactory::NewActuator(const std::string& str_actuator_type, const std::string& str_actuator_implementation) { std::string strKey = str_actuator_type + " (" + str_actuator_implementation + ")"; if(GetActuatorPlugin()->FactoryMap.find(strKey) == GetActuatorPlugin()->FactoryMap.end()) { THROW_ARGOSEXCEPTION("Actuator type \"" << str_actuator_type << "\", implementation \"" << str_actuator_implementation << "\" not found"); } return GetActuatorPlugin()->FactoryMap[strKey](); }
const CComposableEntity& CEntity::GetParent() const { if(m_pcParent != NULL) { return *m_pcParent; } else { THROW_ARGOSEXCEPTION("Entity \"" << GetId() << "\" has no parent"); } }
Real CRotorEquippedEntity::GetRotorVelocity(size_t un_index) const { if(un_index < m_unNumRotors) { return m_pfRotorVelocities[un_index]; } else { THROW_ARGOSEXCEPTION("CRotorEquippedEntity::GetRotorVelocity() : index " << un_index << " out of bounds (allowed [0:" << m_unNumRotors << "])"); } }
void CCI_RangeAndBearingActuator::SetData(const CByteArray& c_data) { if(m_cData.Size() == c_data.Size()) { m_cData = c_data; } else { THROW_ARGOSEXCEPTION("CCI_RangeAndBearingActuator::SetData() : data size does not match, expected " << m_cData.Size() << ", got " << c_data.Size()); } }
const CVector3& CRotorEquippedEntity::GetRotorPosition(size_t un_index) const { if(un_index < m_unNumRotors) { return m_pcRotorPositions[un_index]; } else { THROW_ARGOSEXCEPTION("CRotorEquippedEntity::GetRotorPosition() : index " << un_index << " out of bounds (allowed [0:" << m_unNumRotors << "])"); } }
CByteArray& CByteArray::FetchBuffer(UInt8* pun_buffer, size_t un_size) { if(Size() < un_size) THROW_ARGOSEXCEPTION("Attempting to extract too many bytes from byte array (" << un_size << " requested, " << Size() << " available)"); for(size_t i = 0; i < un_size; ++i) { *(pun_buffer+i) = m_vecBuffer[i]; } m_vecBuffer.erase(m_vecBuffer.begin(), m_vecBuffer.begin() + un_size); return *this; }
void CEmbodiedEntity::AddPhysicsModel(const std::string& str_engine_id, CPhysicsModel& c_physics_model) { if(m_bMovable && GetPhysicsModelsNum() > 0) { THROW_ARGOSEXCEPTION(GetContext() << GetId() << " is movable embodied entity and can't have more than 1 physics engine entity associated"); } m_tPhysicsModelMap[str_engine_id] = &c_physics_model; m_tPhysicsModelVector.push_back(&c_physics_model); CalculateBoundingBox(); }
void CRotorEquippedEntity::SetRotor(UInt32 un_index, const CVector3& c_position) { if(un_index < m_unNumRotors) { m_pcRotorPositions[un_index] = c_position; } else { THROW_ARGOSEXCEPTION("CRotorEquippedEntity::SetRotor() : index " << un_index << " out of bounds (allowed [0:" << m_unNumRotors << "])"); } }
void CPointMass3DEngine::RemoveControllableEntity(const std::string& str_id) { TControllableEntityMap::iterator it = m_tControllableEntities.find(str_id); if(it != m_tControllableEntities.end()) { m_tControllableEntities.erase(it); } else { THROW_ARGOSEXCEPTION("Controllable entity id \"" << str_id << "\" not found in dynamics 2D engine \"" << GetId() << "\""); } }
const CSet<CRABEquippedEntity*,SEntityComparator>& CRABMedium::GetRABsCommunicatingWith(CRABEquippedEntity& c_entity) const { TRoutingTable::const_iterator it = m_tRoutingTable.find(c_entity.GetIndex()); if(it != m_tRoutingTable.end()) { return it->second; } else { THROW_ARGOSEXCEPTION("RAB entity \"" << c_entity.GetContext() << c_entity.GetId() << "\" is not managed by the RAB medium \"" << GetId() << "\""); } }