diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 9887edc2..018a2125 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -73,7 +73,9 @@ endif(NOT EXISTS ${CMAKE_BINARY_DIR}/argos3) # add_subdirectory(core) add_subdirectory(plugins) -add_subdirectory(testing) +if(ARGOS_BUILD_FOR_SIMULATOR) + add_subdirectory(testing) +endif(ARGOS_BUILD_FOR_SIMULATOR) # # Create documentation diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index 3bc34c20..225e080e 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -112,6 +112,9 @@ set(ARGOS3_HEADERS_SIMULATOR_SPACE set(ARGOS3_HEADERS_WRAPPERS_LUA wrappers/lua/lua_controller.h wrappers/lua/lua_utility.h) +# argos3/core/real_robot +set(ARGOS3_HEADERS_REALROBOT + real_robot/real_robot.h) # # Source files @@ -188,6 +191,12 @@ if(ARGOS_BUILD_FOR_SIMULATOR) simulator/space/space_multi_thread_balance_length.cpp simulator/space/space_multi_thread_balance_quantity.cpp simulator/space/space_no_threads.cpp) +else(ARGOS_BUILD_FOR_SIMULATOR) + # Real-robot only code + set(ARGOS3_SOURCES_CORE + ${ARGOS3_SOURCES_CORE} + ${ARGOS3_HEADERS_REALROBOT} + real_robot/real_robot.cpp) endif(ARGOS_BUILD_FOR_SIMULATOR) # Compile Lua wrapper only if Lua was found if(ARGOS_WITH_LUA) @@ -242,6 +251,8 @@ if(ARGOS_BUILD_FOR_SIMULATOR) install(FILES ${ARGOS3_HEADERS_SIMULATOR_VISUALIZATION} DESTINATION include/argos3/core/simulator/visualization) install(FILES ${ARGOS3_HEADERS_SIMULATOR_SPACE_POSITIONAL_INDICES} DESTINATION include/argos3/core/simulator/space/positional_indices) install(FILES ${ARGOS3_HEADERS_SIMULATOR_SPACE} DESTINATION include/argos3/core/simulator/space) +else(ARGOS_BUILD_FOR_SIMULATOR) + install(FILES ${ARGOS3_HEADERS_REALROBOT} DESTINATION include/argos3/core/real_robot) endif(ARGOS_BUILD_FOR_SIMULATOR) if(ARGOS_WITH_LUA) install(FILES ${ARGOS3_HEADERS_WRAPPERS_LUA} DESTINATION include/argos3/core/wrappers/lua) diff --git a/src/core/real_robot/real_robot.cpp b/src/core/real_robot/real_robot.cpp new file mode 100644 index 00000000..722a4a8e --- /dev/null +++ b/src/core/real_robot/real_robot.cpp @@ -0,0 +1,122 @@ +#include "real_robot.h" +#include +#include +#include +#include + +using namespace argos; + +/****************************************/ +/****************************************/ + +CRealRobot::CRealRobot(const std::string& str_conf_fname, + const std::string& str_controller_id) : + m_pcController(NULL) { + /* Parse the .argos file */ + ticpp::Document tConfiguration; + tConfiguration.LoadFile(str_conf_fname); + m_tConfRoot = *tConfiguration.FirstChildElement(); + try { + /* + * Get the control rate + */ + TConfigurationNode& tFramework = GetNode(m_tConfRoot, "framework"); + TConfigurationNode& tExperiment = GetNode(tFramework, "experiment"); + GetNodeAttribute(tExperiment, "ticks_per_second", m_fRate); + /* + * Create the controller + */ + std::string strControllerId, strControllerTag; + TConfigurationNode& tControllers = GetNode(m_tConfRoot, "controllers"); + TConfigurationNodeIterator itControllers; + /* Search for the controller tag with the given id */ + for(itControllers = itControllers.begin(&tControllers); + itControllers != itControllers.end() && strControllerTag == ""; + ++itControllers) { + GetNodeAttribute(*itControllers, "id", strControllerId); + if(strControllerId == str_controller_id) { + strControllerTag = itControllers->Value(); + m_ptControllerConfRoot = &(*itControllers); + } + } + /* Make sure we found the tag */ + if(strControllerTag == "") { + THROW_ARGOSEXCEPTION("Can't find controller with id \"" << str_controller_id << "\""); + } + /* Create the controller */ + m_pcController = ControllerMaker(strControllerTag); + } + catch(CARGoSException& ex) { + LOGERR << ex.what() << std::endl; + } +} + +/****************************************/ +/****************************************/ + +CRealRobot::~CRealRobot() { + if(m_pcController) + delete m_pcController; +} + +/****************************************/ +/****************************************/ + +void CRealRobot::Execute() { + /* Initialize the controller */ + InitController(); + /* Enforce the control rate */ + CRate cRate(m_fRate); + /* Main loop */ + while(1) { + /* Do useful work */ + Sense(); + Control(); + Act(); + /* Sleep to enforce control rate */ + cRate.Sleep(); + } +} + +/****************************************/ +/****************************************/ + +void CRealRobot::InitController() { + /* Set the controller id using the machine hostname */ + char pchHostname[256]; + ::gethostname(pchHostname, 256); + m_pcController->SetId(pchHostname); + /* Go through actuators */ + TConfigurationNode& tActuators = GetNode(*m_ptControllerConfRoot, "actuators"); + TConfigurationNodeIterator itAct; + for(itAct = itAct.begin(&tActuators); + itAct != itAct.end(); + ++itAct) { + /* itAct->Value() is the name of the current actuator */ + CCI_Actuator* pcCIAct = MakeActuator(itAct->Value()); + if(pcCIAct == NULL) { + THROW_ARGOSEXCEPTION("Unknown actuator \"" << itAct->Value() << "\""); + } + pcCIAct->Init(*itAct); + m_pcController->AddActuator(itAct->Value(), pcCIAct); + } + /* Go through sensors */ + TConfigurationNode& tSensors = GetNode(*m_ptControllerConfRoot, "sensors"); + TConfigurationNodeIterator itSens; + for(itSens = itSens.begin(&tSensors); + itSens != itSens.end(); + ++itSens) { + /* itSens->Value() is the name of the current sensor */ + CCI_Sensor* pcCISens = MakeSensor(itSens->Value()); + if(pcCISens == NULL) { + THROW_ARGOSEXCEPTION("Unknown sensor \"" << itSens->Value() << "\""); + } + pcCISens->Init(*itSens); + m_pcController->AddSensor(itSens->Value(), pcCISens); + } + /* Configure the controller */ + m_pcController->Init(*m_ptControllerConfRoot); +} + +/****************************************/ +/****************************************/ diff --git a/src/core/real_robot/real_robot.h b/src/core/real_robot/real_robot.h new file mode 100644 index 00000000..3db53735 --- /dev/null +++ b/src/core/real_robot/real_robot.h @@ -0,0 +1,84 @@ +#ifndef REAL_ROBOT_H +#define REAL_ROBOT_H + +#include +#include + +namespace argos { + + class CRealRobot { + + public: + + /** + * Class constructor. + */ + CRealRobot(const std::string& str_conf_fname, + const std::string& str_controller_id); + + /** + * Class destructor. + */ + virtual ~CRealRobot(); + + /** + * Put your robot initialization code here. + */ + virtual void Init() = 0; + + /** + * Put your robot cleanup code here. + */ + virtual void Destroy() = 0; + + /** + * Creates an actuator given its name. + * Returns NULL if no actuator corresponds to that name. + */ + virtual CCI_Actuator* MakeActuator(const std::string& str_name) = 0; + + /** + * Creates a sensor given its name. + * Returns NULL if no sensor corresponds to that name. + */ + virtual CCI_Sensor* MakeSensor(const std::string& str_name) = 0; + + /** + * Collect data from the sensors. + */ + virtual void Sense() = 0; + + /** + * Execute the robot controller. + */ + virtual void Control() = 0; + + /** + * Send data to the actuators. + */ + virtual void Act() = 0; + + /** + * Perform the main loop. + */ + virtual void Execute(); + + protected: + + /** + * Initializes the controller + */ + virtual void InitController(); + + protected: + + CCI_Controller* m_pcController; + TConfigurationNode m_tConfRoot; + TConfigurationNode* m_ptControllerConfRoot; + Real m_fRate; + + }; + +} + +#endif