Skip to content

Commit 0022d5f

Browse files
KenN7KenN7
committed
new exp beacon stop, unfiniched work
1 parent 6f8ab32 commit 0022d5f

File tree

2 files changed

+234
-0
lines changed

2 files changed

+234
-0
lines changed

‎loop-functions/gianduja/GiandujaBeaconStopLoopFunc.cpp

Lines changed: 188 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,188 @@
1+
#include "GiandujaBeaconStopLoopFunc.h"
2+
3+
/****************************************/
4+
/****************************************/
5+
6+
GiandujaBeaconStopLoopFunction::GiandujaBeaconStopLoopFunction() {
7+
m_fRadius = 0.10;
8+
//m_cCoordSpot1 = CVector2(0.6,0.8);
9+
m_CCoordRect1Pos = CVector2(0.8,-0.5);
10+
m_CCoordRect1 = CVector2(1.25,-0.5);
11+
12+
m_CCoordRect2Pos = CVector2(-0.8,-1);
13+
//m_CCoordRect2 = CVector2(-1.25,-1.25);
14+
15+
m_unCostI = 0;
16+
m_unCostO = 0;
17+
m_unState = 0;
18+
m_unTbar = 0;
19+
m_fObjectiveFunction = 0;
20+
m_unTime = 0;
21+
}
22+
23+
/****************************************/
24+
/****************************************/
25+
26+
GiandujaBeaconStopLoopFunction::GiandujaBeaconStopLoopFunction(const GiandujaBeaconStopLoopFunction& orig) {}
27+
28+
/****************************************/
29+
/****************************************/
30+
31+
GiandujaBeaconStopLoopFunction::~GiandujaBeaconStopLoopFunction() {}
32+
33+
/****************************************/
34+
/****************************************/
35+
36+
void GiandujaBeaconStopLoopFunction::Destroy() {}
37+
38+
/****************************************/
39+
/****************************************/
40+
41+
void GiandujaBeaconStopLoopFunction::Reset() {
42+
CoreLoopFunctions::Reset();
43+
m_unCostI = 0;
44+
m_unCostO = 0;
45+
m_unState = 0;
46+
m_unTbar = 0;
47+
m_unTime = 0;
48+
m_fObjectiveFunction = 0;
49+
ExtractTime();
50+
PlaceBeacon();
51+
//Real a =m_pcRng->Uniform(CRange<Real>(0.0f, 1.0f));
52+
//Real b =m_pcRng->Uniform(CRange<Real>(0.0f, 1.0f));
53+
//m_cCoordSpot1 = CVector2(-0.7+a*1.4,0.6+b*0.4);
54+
}
55+
56+
/****************************************/
57+
/****************************************/
58+
59+
void GiandujaBeaconStopLoopFunction::Init(TConfigurationNode& t_tree) {
60+
CoreLoopFunctions::Init(t_tree);
61+
ExtractTime();
62+
PlaceBeacon();
63+
//Real a =m_pcRng->Uniform(CRange<Real>(0.0f, 1.0f));
64+
//Real b =m_pcRng->Uniform(CRange<Real>(0.0f, 1.0f));
65+
//m_cCoordSpot1 = CVector2(-0.7+a*1.4,0.6+b*0.4);
66+
//LOG << "Spot is at :" << m_cCoordSpot1 << std::endl;
67+
}
68+
69+
argos::CColor GiandujaBeaconStopLoopFunction::GetFloorColor(const argos::CVector2& c_position_on_plane) {
70+
CVector2 vCurrentPoint(c_position_on_plane.GetX(), c_position_on_plane.GetY());
71+
72+
// Real d = (m_cCoordSpot1 - vCurrentPoint).Length();
73+
//
74+
// if (d <= m_fRadius) {
75+
// return CColor::WHITE;
76+
// }
77+
78+
if ( (vCurrentPoint.GetX()<=m_CCoordRect1Pos.GetX()) && (vCurrentPoint.GetX()>=m_CCoordRect2Pos.GetX()) && (vCurrentPoint.GetY()>=m_CCoordRect2Pos.GetY()) && (vCurrentPoint.GetY()<=m_CCoordRect1Pos.GetY()) ) {
79+
return CColor::GREEN;
80+
}
81+
82+
if ( (vCurrentPoint.GetX()<=m_CCoordRect1.GetX()) && (vCurrentPoint.GetX()>=m_CCoordRect2.GetX()) && (vCurrentPoint.GetY()>=m_CCoordRect2.GetY()) && (vCurrentPoint.GetY()<=m_CCoordRect1.GetY()) ) {
83+
return CColor::WHITE;
84+
}
85+
86+
return CColor::GRAY50;
87+
}
88+
89+
90+
void GiandujaBeaconAggregationLoopFunction::ExtractTime() {
91+
try {
92+
CEPuckEntity& cEntity = dynamic_cast<CEPuckEntity&>(GetSpace().GetEntity("beacon0"));
93+
CEPuckBeacon& cController = dynamic_cast<CEPuckBeacon&>(cEntity.GetControllableEntity().GetController());
94+
m_Tbar = cController.getTBar();
95+
//LOG << "Time=" << m_Tbar << std::endl;
96+
} catch (std::exception& ex) {
97+
LOGERR << "Error while casting ExtractTime: " << ex.what() << std::endl;
98+
}
99+
}
100+
101+
102+
void GiandujaBeaconStopLoopFunction::PlaceBeacon() {
103+
try {
104+
CEPuckEntity& cEpuck = dynamic_cast<CEPuckEntity&>(GetSpace().GetEntity("beacon0"));
105+
MoveEntity(cEpuck.GetEmbodiedEntity(),
106+
CVector3(0, -0.5, 0),
107+
CQuaternion().FromEulerAngles(CRadians::ZERO,
108+
CRadians::ZERO,CRadians::ZERO),false);
109+
} catch (std::exception& ex) {
110+
LOGERR << "Error while casting PlaceBeacon: " << ex.what() << std::endl;
111+
}
112+
113+
}
114+
115+
116+
CVector3 GiandujaBeaconStopLoopFunction::GetRandomPosition() {
117+
Real a;
118+
Real b;
119+
120+
a = m_pcRng->Uniform(CRange<Real>(0.0f, 1.0f));
121+
b = m_pcRng->Uniform(CRange<Real>(0.0f, 1.0f));
122+
123+
Real fPosX = m_CCoordRect2Pos.GetX() + a*fabs(m_CCoordRect2Pos.GetX() - m_CCoordRect1Pos.GetX());
124+
Real fPosY = m_CCoordRect2Pos.GetY() + b*fabs(m_CCoordRect2Pos.GetY() - m_CCoordRect1Pos.GetY());
125+
126+
return CVector3(fPosX, fPosY, 0);
127+
}
128+
129+
130+
/****************************************/
131+
/****************************************/
132+
133+
void GiandujaBeaconStopLoopFunction::PostStep() {
134+
CSpace::TMapPerType& tEpuckMap = GetSpace().GetEntitiesByType("epuck");
135+
CVector2 cEpuckPosition(0,0);
136+
UInt8 un_trigger = 0;
137+
for (CSpace::TMapPerType::iterator it = tEpuckMap.begin(); it != tEpuckMap.end(); ++it) {
138+
CEPuckEntity* pcEpuck = any_cast<CEPuckEntity*>(it->second);
139+
cEpuckPosition.Set(pcEpuck->GetEmbodiedEntity().GetOriginAnchor().Position.GetX(),
140+
pcEpuck->GetEmbodiedEntity().GetOriginAnchor().Position.GetY());
141+
142+
Real fDistanceSpot1 = (m_cCoordSpot1 - cEpuckPosition).Length();
143+
if (m_unTime > m_unTbar) {
144+
if (m_unState == 0) {
145+
if (fDistanceSpot1 <= m_fRadius) {
146+
un_trigger = 1;
147+
}
148+
else if ( fabs(m_tOldPosPoints[pcEpuck].GetX() - cEpuckPosition.GetX()) < 0.005 && fabs(m_tOldPosPoints[pcEpuck].GetY() - cEpuckPosition.GetY()) < 0.005) {
149+
m_unCostI+=1;
150+
}
151+
m_unTbar +=1;
152+
}
153+
else if (m_unState == 1) {
154+
if ( fabs(m_tOldPosPoints[pcEpuck].GetX() - cEpuckPosition.GetX()) > 0.005 && fabs(m_tOldPosPoints[pcEpuck].GetY() - cEpuckPosition.GetY()) > 0.005) {
155+
m_unCostO+=1;
156+
}
157+
}
158+
m_tOldPosPoints[pcEpuck] = cEpuckPosition;
159+
}
160+
else {
161+
if ( (cEpuckPosition.GetX()<=m_CCoordRect1.GetX()) && (cEpuckPosition.GetX()>=m_CCoordRect2.GetX()) && (cEpuckPosition.GetY()>=m_CCoordRect2.GetY()) && (cEpuckPosition.GetY()<=m_CCoordRect1.GetY()) ) {
162+
m_unCostI+=1
163+
}
164+
}
165+
}
166+
167+
if (m_unState == 0 && un_trigger == 1) {
168+
m_unState = 1;
169+
LOG << "Found spot triggered." << std::endl;
170+
}
171+
m_unTime+=1;
172+
}
173+
174+
/****************************************/
175+
/****************************************/
176+
177+
void GiandujaBeaconStopLoopFunction::PostExperiment() {
178+
LOG<< "CostI :" << m_unCostI << " / CostO :" << m_unCostO << " / Tbar:" << m_unTbar << std::endl;
179+
LOG<< 48000 - (m_unCostI + m_unCostO + m_unTbar) << std::endl;
180+
m_fObjectiveFunction = (Real) 48000 - (m_unCostI + m_unCostO + m_unTbar);
181+
182+
}
183+
184+
Real GiandujaBeaconStopLoopFunction::GetObjectiveFunction() {
185+
return (m_fObjectiveFunction);
186+
}
187+
188+
REGISTER_LOOP_FUNCTIONS(GiandujaBeaconStopLoopFunction, "gianduja_stop_loop_functions");

‎loop-functions/gianduja/GiandujaBeaconStopLoopFunc.h

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
#ifndef GIANDUJA_BEACON_STOP_LOOP_FUNC
2+
#define GIANDUJA_BEACON_STOP_LOOP_FUNC
3+
4+
#include "../../src/CoreLoopFunctions.h"
5+
#include <argos3/core/simulator/space/space.h>
6+
#include <argos3/plugins/robots/e-puck/simulator/epuck_entity.h>
7+
8+
using namespace argos;
9+
10+
class GiandujaBeaconStopLoopFunction: public CoreLoopFunctions {
11+
public:
12+
GiandujaStopLoopFunction();
13+
GiandujaStopLoopFunction(const GiandujaStopLoopFunction& orig);
14+
virtual ~GiandujaStopLoopFunction();
15+
16+
virtual void Destroy();
17+
18+
virtual argos::CColor GetFloorColor(const argos::CVector2& c_position_on_plane);
19+
virtual void PostExperiment();
20+
virtual void PostStep();
21+
virtual void Init(TConfigurationNode& t_tree);
22+
virtual void Reset();
23+
virtual CVector3 GetRandomPosition();
24+
25+
Real GetObjectiveFunction();
26+
27+
private:
28+
Real m_fRadius;
29+
CVector2 m_cCoordSpot1;
30+
CVector2 m_CCoordRect1;
31+
//CVector2 m_CCoordRect2;
32+
CVector2 m_CCoordRect2Pos;
33+
CVector2 m_CCoordRect1Pos;
34+
35+
typedef std::map<CEPuckEntity*, CVector2 > TOldPosMap;
36+
TOldPosMap m_tOldPosPoints;
37+
38+
UInt8 m_unState;
39+
UInt32 m_unCostI;
40+
UInt32 m_unCostO;
41+
UInt32 m_unTbar;
42+
Real m_fObjectiveFunction;
43+
UInt32 m_unTime;
44+
};
45+
46+
#endif

0 commit comments

Comments
 (0)