forked from ilpincy/argos3
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
More work on real robot integration code.
- Loading branch information
Showing
4 changed files
with
220 additions
and
1 deletion.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,122 @@ | ||
| #include "real_robot.h" | ||
| #include <argos3/core/utility/rate.h> | ||
| #include <argos3/core/utility/logging/argos_log.h> | ||
| #include <argos3/core/control_interface/ci_controller.h> | ||
| #include <unistd.h> | ||
|
|
||
| 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); | ||
| } | ||
|
|
||
| /****************************************/ | ||
| /****************************************/ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,84 @@ | ||
| #ifndef REAL_ROBOT_H | ||
| #define REAL_ROBOT_H | ||
|
|
||
| #include <argos3/core/utility/configuration/argos_configuration.h> | ||
| #include <argos3/core/control_interface/ci_controller.h> | ||
|
|
||
| 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 |