battery_equipped_entity.cpp
Go to the documentation of this file.
1 
7 #include <argos3/core/simulator/space/space.h>
8 #include <argos3/core/simulator/entity/composable_entity.h>
9 
10 namespace argos {
11 
12  /****************************************/
13  /****************************************/
14 
16  CEntity(pc_parent),
17  m_fFullCharge(1.0),
18  m_fAvailableCharge(m_fFullCharge),
19  m_pcDischargeModel(NULL) {
21  Disable();
22  }
23 
24  /****************************************/
25  /****************************************/
26 
28  const std::string &str_id,
29  CBatteryDischargeModel* pc_discharge_model,
30  Real f_start_charge,
31  Real f_full_charge) :
32  CEntity(pc_parent, str_id),
33  m_fFullCharge(f_full_charge),
34  m_fAvailableCharge(f_start_charge),
35  m_pcDischargeModel(NULL) {
36  SetDischargeModel(pc_discharge_model);
37  Disable();
38  }
39 
40  /****************************************/
41  /****************************************/
42 
44  const std::string& str_id,
45  const std::string& str_discharge_model,
46  Real f_start_charge,
47  Real f_full_charge) :
48  CEntity(pc_parent, str_id),
49  m_fFullCharge(f_full_charge),
50  m_fAvailableCharge(f_start_charge),
51  m_pcDischargeModel(NULL) {
52  SetDischargeModel(str_discharge_model);
53  Disable();
54  }
55 
56  /****************************************/
57  /****************************************/
58 
60  /* Get rid of battery discharge model */
61  delete m_pcDischargeModel;
62  }
63 
64  /****************************************/
65  /****************************************/
66 
68  try {
69  CEntity::Init(t_tree);
70  /* Get initial battery level */
71  std::string strDischargeModel = "time";
72  GetNodeAttributeOrDefault(t_tree, "discharge_model", strDischargeModel, strDischargeModel);
73  SetDischargeModel(strDischargeModel);
74  m_pcDischargeModel->Init(t_tree);
75  /* Get initial battery charge */
77  /* Get full battery charge */
78  GetNodeAttributeOrDefault(t_tree, "full_charge", m_fFullCharge, m_fFullCharge);
79  }
80  catch(CARGoSException& ex) {
81  THROW_ARGOSEXCEPTION_NESTED("Error initializing the battery sensor equipped entity \"" << GetId() << "\"", ex);
82  }
83  }
84 
85  /****************************************/
86  /****************************************/
87 
90  /* Call the discharge model */
91  (*m_pcDischargeModel)();
92  }
93 
94  /****************************************/
95  /****************************************/
96 
99  m_pcDischargeModel = pc_model;
102  }
103 
104  /****************************************/
105  /****************************************/
106 
107  void CBatteryEquippedEntity::SetDischargeModel(const std::string& str_model) {
109  if(str_model != "") {
112  }
113  }
114 
115  /****************************************/
116  /****************************************/
117 
119  m_pcBattery(NULL) {
120  }
121 
122  /****************************************/
123  /****************************************/
124 
126  }
127 
128  /****************************************/
129  /****************************************/
130 
132  m_pcBattery = pc_battery;
133  }
134 
135  /****************************************/
136  /****************************************/
137 
139  GetNodeAttributeOrDefault(t_tree, "factor", m_fDelta, m_fDelta);
140  }
141 
142  /****************************************/
143  /****************************************/
144 
146  if(m_pcBattery->GetAvailableCharge() > 0.0) {
148  Max<Real>(0.0,
150  }
151  }
152 
153  /****************************************/
154  /****************************************/
155 
157  GetNodeAttributeOrDefault(t_tree, "pos_factor", m_fPosFactor, m_fPosFactor);
158  GetNodeAttributeOrDefault(t_tree, "orient_factor", m_fOrientFactor, m_fOrientFactor);
159  }
160 
161  /****************************************/
162  /****************************************/
163 
165  try {
166  /* Execute default logic */
168  /* Get a hold of the body and anchor of the entity that contains the battery */
169  CEntity* pcRoot = &pc_battery->GetRootEntity();
170  CComposableEntity* cComp = dynamic_cast<CComposableEntity*>(pcRoot);
171  if(cComp != NULL) {
172  CEmbodiedEntity& cBody = cComp->GetComponent<CEmbodiedEntity>("body");
173  m_psAnchor = &cBody.GetOriginAnchor();
174  m_cOldPosition = m_psAnchor->Position;
175  }
176  else {
177  THROW_ARGOSEXCEPTION("Root entity is not composable");
178  }
179  }
180  catch(CARGoSException& ex) {
181  THROW_ARGOSEXCEPTION_NESTED("While setting body for battery model \"motion\"", ex);
182  }
183  }
184 
185  /****************************************/
186  /****************************************/
187 
189  if(m_pcBattery->GetAvailableCharge() > 0.0) {
190  /* Calculate delta position */
191  Real fDeltaPos = Distance(m_psAnchor->Position,
193  /* Calculate delta orientation */
194  CQuaternion cDeltaOrient =
197  CRadians cDeltaAngle;
198  CVector3 cDeltaAxis;
199  cDeltaOrient.ToAngleAxis(cDeltaAngle, cDeltaAxis);
200  /* Calculate new level */
202  Max<Real>(
203  0.0,
205  m_fPosFactor * fDeltaPos -
206  m_fOrientFactor * cDeltaAngle.GetValue()
207  ));
208  /* Save position for next step */
211  }
212  }
213 
214  /****************************************/
215  /****************************************/
216 
218  GetNodeAttributeOrDefault(t_tree, "time_factor", m_fDelta, m_fDelta);
219  GetNodeAttributeOrDefault(t_tree, "pos_factor", m_fPosFactor, m_fPosFactor);
220  GetNodeAttributeOrDefault(t_tree, "orient_factor", m_fOrientFactor, m_fOrientFactor);
221  }
222 
223  /****************************************/
224  /****************************************/
225 
227  try {
228  /* Execute default logic */
230  /* Get a hold of the body and anchor of the entity that contains the battery */
231  CEntity* pcRoot = &pc_battery->GetRootEntity();
232  CComposableEntity* cComp = dynamic_cast<CComposableEntity*>(pcRoot);
233  if(cComp != NULL) {
234  CEmbodiedEntity& cBody = cComp->GetComponent<CEmbodiedEntity>("body");
235  m_psAnchor = &cBody.GetOriginAnchor();
236  m_cOldPosition = m_psAnchor->Position;
237  }
238  else {
239  THROW_ARGOSEXCEPTION("Root entity is not composable");
240  }
241  }
242  catch(CARGoSException& ex) {
243  THROW_ARGOSEXCEPTION_NESTED("While setting body for battery model \"time_motion\"", ex);
244  }
245  }
246 
247  /****************************************/
248  /****************************************/
249 
251  if(m_pcBattery->GetAvailableCharge() > 0.0) {
252  /* Calculate delta position */
253  Real fDeltaPos = Distance(m_psAnchor->Position,
255  /* Calculate delta orientation */
256  CQuaternion cDeltaOrient =
259  CRadians cDeltaAngle;
260  CVector3 cDeltaAxis;
261  cDeltaOrient.ToAngleAxis(cDeltaAngle, cDeltaAxis);
262  /* Calculate new level */
264  Max<Real>(
265  0.0,
267  m_fDelta -
268  m_fPosFactor * fDeltaPos -
269  m_fOrientFactor * cDeltaAngle.GetValue()
270  ));
271  /* Save position for next step */
274  }
275  }
276 
277  /****************************************/
278  /****************************************/
279 
284 
285  /****************************************/
286  /****************************************/
287 
288 }
Real Distance(const CVector2 &c_v1, const CVector2 &c_v2)
Computes the distance between the passed vectors.
Definition: vector2.h:418
void ToAngleAxis(CRadians &c_angle, CVector3 &c_vector) const
Definition: quaternion.h:143
A 3D vector class.
Definition: vector3.h:29
void GetNodeAttributeOrDefault(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer, const T &t_default)
Returns the value of a node's attribute, or the passed default value.
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
REGISTER_BATTERY_DISCHARGE_MODEL(CBatteryDischargeModelTime,"time")
The basic entity type.
Definition: entity.h:89
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
virtual void SetBattery(CBatteryEquippedEntity *pc_battery)
CQuaternion Orientation
The orientation of the anchor wrt the global coordinate system.
Definition: physics_model.h:53
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
This entity is a link to a body in the physics engine.
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception...
virtual void SetBattery(CBatteryEquippedEntity *pc_battery)
CBatteryEquippedEntity * m_pcBattery
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
CEntity & GetRootEntity()
Returns the root entity containing this entity.
Definition: entity.cpp:115
void SetAvailableCharge(Real f_available_charge)
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
Basic class for an entity that contains other entities.
static TYPE * New(const std::string &str_label)
Creates a new object of type TYPE
Definition: factory_impl.h:48
A battery discharge model based only on time.
CQuaternion Inverse() const
Definition: quaternion.h:98
virtual void SetBattery(CBatteryEquippedEntity *pc_battery)
The exception that wraps all errors in ARGoS.
CBatteryDischargeModel * m_pcDischargeModel
Discharge model.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: entity.cpp:40
void SetDischargeModel(CBatteryDischargeModel *pc_model)
virtual void Update()
Updates the state of this entity.
void Disable()
Disables the entity.
Definition: entity.h:275
Real GetValue() const
Returns the value in radians.
Definition: angles.h:111
REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CEntity)
CVector3 Position
The position of the anchor wrt the global coordinate system.
Definition: physics_model.h:51
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
The discharge model dictates how the battery discharges over time.
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
A battery discharge model in which the charge decreases with both time and motion.
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
Real m_fAvailableCharge
Available charge.
CBatteryEquippedEntity(CComposableEntity *pc_parent)
A battery discharge model based only on motion.
const SAnchor & GetOriginAnchor() const
Returns a const reference to the origin anchor associated to this entity.
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.