real_robot.cpp
Go to the documentation of this file.
1 #include "real_robot.h"
2 #include <argos3/core/utility/rate.h>
3 #include <argos3/core/utility/logging/argos_log.h>
4 #include <argos3/core/control_interface/ci_controller.h>
5 #include <signal.h>
6 #include <unistd.h>
7 
8 using namespace argos;
9 
10 /****************************************/
11 /****************************************/
12 
14 
15 /****************************************/
16 /****************************************/
17 
19  m_pcController(NULL) {
20  /* Set instance */
21  m_pcInstance = this;
22 }
23 
24 /****************************************/
25 /****************************************/
26 
27 void CRealRobot::Init(const std::string& str_conf_fname,
28  const std::string& str_controller_id) {
29  /* Parse the .argos file */
30  m_tConfiguration.LoadFile(str_conf_fname);
31  m_tConfRoot = *m_tConfiguration.FirstChildElement();
32  /*
33  * Install signal handlers
34  */
35  ::signal(SIGINT, Cleanup);
36  ::signal(SIGQUIT, Cleanup);
37  ::signal(SIGABRT, Cleanup);
38  ::signal(SIGTERM, Cleanup);
39  /*
40  * Get the control rate
41  */
42  TConfigurationNode& tFramework = GetNode(m_tConfRoot, "framework");
43  TConfigurationNode& tExperiment = GetNode(tFramework, "experiment");
44  GetNodeAttribute(tExperiment, "ticks_per_second", m_fRate);
45  /*
46  * Parse XML to identify the controller to run
47  */
48  std::string strControllerId, strControllerTag;
49  TConfigurationNode& tControllers = GetNode(m_tConfRoot, "controllers");
50  TConfigurationNodeIterator itControllers;
51  /* Search for the controller tag with the given id */
52  for(itControllers = itControllers.begin(&tControllers);
53  itControllers != itControllers.end() && strControllerTag == "";
54  ++itControllers) {
55  GetNodeAttribute(*itControllers, "id", strControllerId);
56  if(strControllerId == str_controller_id) {
57  strControllerTag = itControllers->Value();
58  m_ptControllerConfRoot = &(*itControllers);
59  }
60  }
61  /* Make sure we found the tag */
62  if(strControllerTag == "") {
63  THROW_ARGOSEXCEPTION("Can't find controller with id \"" << str_controller_id << "\"");
64  }
65  /*
66  * Initialize the robot
67  */
68  LOG << "[INFO] Robot initialization start" << std::endl;
69  InitRobot();
70  LOG << "[INFO] Robot initialization done" << std::endl;
71  /*
72  * Initialize the controller
73  */
74  LOG << "[INFO] Controller type '" << strControllerTag << "', id '" << str_controller_id << "' initialization start" << std::endl;
75  m_pcController = ControllerMaker(strControllerTag);
76  /* Set the controller id using the machine hostname */
77  char pchHostname[256];
78  pchHostname[255] = '\0';
79  ::gethostname(pchHostname, 255);
80  m_pcController->SetId(pchHostname);
81  LOG << "[INFO] Controller id set to '" << pchHostname << "'" << std::endl;
82  /* Go through actuators */
83  TConfigurationNode& tActuators = GetNode(*m_ptControllerConfRoot, "actuators");
85  for(itAct = itAct.begin(&tActuators);
86  itAct != itAct.end();
87  ++itAct) {
88  /* itAct->Value() is the name of the current actuator */
89  CCI_Actuator* pcCIAct = MakeActuator(itAct->Value());
90  if(pcCIAct == NULL) {
91  THROW_ARGOSEXCEPTION("Unknown actuator \"" << itAct->Value() << "\"");
92  }
93  pcCIAct->Init(*itAct);
94  m_pcController->AddActuator(itAct->Value(), pcCIAct);
95  }
96  /* Go through sensors */
97  TConfigurationNode& tSensors = GetNode(*m_ptControllerConfRoot, "sensors");
99  for(itSens = itSens.begin(&tSensors);
100  itSens != itSens.end();
101  ++itSens) {
102  /* itSens->Value() is the name of the current sensor */
103  CCI_Sensor* pcCISens = MakeSensor(itSens->Value());
104  if(pcCISens == NULL) {
105  THROW_ARGOSEXCEPTION("Unknown sensor \"" << itSens->Value() << "\"");
106  }
107  pcCISens->Init(*itSens);
108  m_pcController->AddSensor(itSens->Value(), pcCISens);
109  }
110  /* Configure the controller */
112  LOG << "[INFO] Controller type '" << strControllerTag << "', id '" << str_controller_id << "' initialization done" << std::endl;
113 }
114 
115 /****************************************/
116 /****************************************/
117 
119  if(m_pcController)
120  delete m_pcController;
121 }
122 
123 /****************************************/
124 /****************************************/
125 
128 }
129 
130 /****************************************/
131 /****************************************/
132 
134  /* Enforce the control rate */
135  CRate cRate(m_fRate);
136  /* Main loop */
137  LOG << "[INFO] Control loop running" << std::endl;
138  while(1) {
139  /* Do useful work */
140  Sense();
141  Control();
142  Act();
143  /* Sleep to enforce control rate */
144  cRate.Sleep();
145  }
146 }
147 
148 /****************************************/
149 /****************************************/
150 
152  LOG << "[INFO] Stopping controller" << std::endl;
153  if(m_pcInstance != NULL) {
155  delete m_pcInstance;
156  }
157  LOG << "[INFO] All done" << std::endl;
158  exit(0);
159 }
160 
161 /****************************************/
162 /****************************************/
CCI_Controller * m_pcController
Definition: real_robot.h:79
static CRealRobot * m_pcInstance
Definition: real_robot.h:84
virtual void Init(const std::string &str_conf_fname, const std::string &str_controller_id)
Initializes the robot and the controller.
Definition: real_robot.cpp:27
void SetId(const std::string &str_id)
Sets the id of the robot associated to this controller.
Definition: ci_controller.h:88
argos::CCI_Controller * ControllerMaker(const std::string &str_label)
Registers a new controller inside ARGoS.
virtual void InitRobot()=0
Put your robot initialization code here.
The basic interface for all actuators.
Definition: ci_actuator.h:34
CARGoSLog LOG(std::cout, SLogColor(ARGOS_LOG_ATTRIBUTE_BRIGHT, ARGOS_LOG_COLOR_GREEN))
Definition: argos_log.h:179
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
TConfigurationNode & GetNode(TConfigurationNode &t_node, const std::string &str_tag)
Given a tree root node, returns the first of its child nodes with the wanted name.
virtual void Control()
Execute the robot controller.
Definition: real_robot.cpp:126
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
virtual void Act()=0
Send data to the actuators.
void AddSensor(const std::string &str_sensor_type, CCI_Sensor *pc_sensor)
Adds an sensor to this controller.
TConfigurationNode * m_ptControllerConfRoot
Definition: real_robot.h:82
virtual void Init(TConfigurationNode &t_node)
Initializes the actuator from the XML configuration tree.
Definition: ci_actuator.h:54
static void Cleanup(int)
Cleanup function called when the controller is stopped.
Definition: real_robot.cpp:151
CRealRobot()
Class constructor.
Definition: real_robot.cpp:18
virtual void ControlStep()
Executes a control step.
Definition: ci_controller.h:55
The basic interface for all sensors.
Definition: ci_sensor.h:34
virtual void Init(TConfigurationNode &t_node)
Initializes the sensor from the XML configuration tree.
Definition: ci_sensor.h:54
ticpp::Iterator< ticpp::Element > TConfigurationNodeIterator
The iterator for the ARGoS configuration XML node.
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
virtual void Sense()=0
Collect data from the sensors.
virtual void Init(TConfigurationNode &t_node)
Initializes the controller.
Definition: ci_controller.h:48
virtual CCI_Actuator * MakeActuator(const std::string &str_name)=0
Creates an actuator given its name.
TConfigurationNode m_tConfRoot
Definition: real_robot.h:81
virtual ~CRealRobot()
Class destructor.
Definition: real_robot.cpp:118
virtual void Destroy()=0
Put your robot cleanup code here.
ticpp::Document m_tConfiguration
Definition: real_robot.h:80
virtual void Execute()
Performs the main loop.
Definition: real_robot.cpp:133
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
void Sleep()
Definition: rate.cpp:17
void AddActuator(const std::string &str_actuator_type, CCI_Actuator *pc_actuator)
Adds an actuator to this controller.
virtual CCI_Sensor * MakeSensor(const std::string &str_name)=0
Creates a sensor given its name.