Modifier un module existant#
Dans ce tutoriel, nous allons voir comment modifier un module existant en prenant toujours pour exemple la détection d’un objet rouge via la caméra.
Le code#
Code pour LEDHandler.h
1/**
2* @file LEDHandler.h
3* This file implements a module that generates the LEDRequest from certain representations.
4* @author jeff
5*/
6
7#pragma once
8
9#include "Representations/BehaviorControl/TeamBehaviorStatus.h"
10#include "Representations/Communication/GameInfo.h"
11#include "Representations/Communication/RobotInfo.h"
12#include "Representations/Communication/TeamData.h"
13#include "Representations/Communication/TeamInfo.h"
14#include "Representations/Infrastructure/FrameInfo.h"
15#include "Representations/Infrastructure/LEDRequest.h"
16#include "Representations/Infrastructure/SensorData/SystemSensorData.h"
17#include "Representations/Modeling/BallModel.h"
18#include "Representations/Perception/FieldFeatures/FieldFeatureOverview.h"
19#include "Representations/Sensing/GroundContactState.h"
20#include "Tools/Module/Module.h"
21#include "Representations/Infrastructure/RobotHealth.h"
22#include "Representations/Perception/ColorDetects/ColorDetect.h"
23
24MODULE(LEDHandler,
25{,
26 REQUIRES(BallModel),
27 REQUIRES(FieldFeatureOverview),
28 REQUIRES(FrameInfo),
29 REQUIRES(GameInfo),
30 REQUIRES(GroundContactState),
31 REQUIRES(JointSensorData),
32 REQUIRES(OwnTeamInfo),
33 REQUIRES(RobotInfo),
34 USES(RobotHealth),
35 REQUIRES(SystemSensorData),
36 REQUIRES(TeamBehaviorStatus),
37 REQUIRES(TeamData),
38 REQUIRES(ColorDetect),
39 PROVIDES(LEDRequest),
40 DEFINES_PARAMETERS(
41 {,
42 (int)(5) chargingLightSlowness,
43 (int)(2000) gameControllerTimeOut,
44 (int)(50) tempForHalfLEDActive,
45 (int)(65) tempForLEDBlinking,
46 (int)(75) tempForLEDFastBlinking,
47 }),
48});
49
50class LEDHandler : public LEDHandlerBase
51{
52public:
53 ENUM(EyeColor,
54 {,
55 red,
56 green,
57 blue,
58 white,
59 magenta,
60 yellow,
61 cyan,
62 });
63
64private:
65 void update(LEDRequest& ledRequest) override;
66
67 void setEyeColor(LEDRequest& ledRequest,
68 bool left,
69 EyeColor col,
70 LEDRequest::LEDState s);
71
72 void setBatteryLevelInEar(LEDRequest& ledRequest, LEDRequest::LED baseLED);
73
74 void setRightEar(LEDRequest& ledRequest);
75 void setLeftEar(LEDRequest& ledRequest);
76 void setLeftEye(LEDRequest& ledRequest);
77 void setRightEye(LEDRequest& ledRequest);
78 void setHead(LEDRequest& ledRequest);
79 void setChestButton(LEDRequest& ledRequest);
80 void setLeftFoot(LEDRequest& ledRequest);
81 void setRightFoot(LEDRequest& ledRequest);
82
83 size_t chargingLED = 0;
84 const LEDRequest::LED headLEDCircle[LEDRequest::numOfHeadLEDs] =
85 {
86 LEDRequest::headRearLeft2,
87 LEDRequest::headRearLeft1,
88 LEDRequest::headRearLeft0,
89 LEDRequest::headMiddleLeft0,
90 LEDRequest::headFrontLeft0,
91 LEDRequest::headFrontLeft1,
92 LEDRequest::headFrontRight1,
93 LEDRequest::headFrontRight0,
94 LEDRequest::headMiddleRight0,
95 LEDRequest::headRearRight0,
96 LEDRequest::headRearRight1,
97 LEDRequest::headRearRight2
98 };
99};
Code pour LEDHandler.cpp
1/**
2* @file LEDHandler.cpp
3* This file implements a module that generates the LEDRequest from certain representations.
4* @author jeff
5* @author <a href="mailto:jesse@tzi.de">Jesse Richter-Klug</a>
6*/
7
8#include "LEDHandler.h"
9
10#include <algorithm>
11
12void LEDHandler::update(LEDRequest& ledRequest)
13{
14 //reset
15 FOREACH_ENUM(LEDRequest::LED, led)
16 ledRequest.ledStates[led] = LEDRequest::off;
17
18 setRightEye(ledRequest);
19 setLeftEye(ledRequest);
20 setChestButton(ledRequest);
21 setLeftFoot(ledRequest);
22 setRightFoot(ledRequest);
23
24 //update
25 setRightEar(ledRequest);
26 setLeftEar(ledRequest);
27 setHead(ledRequest);
28}
29
30void LEDHandler::setRightEar(LEDRequest& ledRequest)
31{
32 //right ear -> battery
33 setBatteryLevelInEar(ledRequest, LEDRequest::earsRight0Deg);
34}
35
36void LEDHandler::setLeftEar(LEDRequest& ledRequest)
37{
38 //left ear -> connected players
39 // + GameController connection lost -> freaky blinking
40 if(theFrameInfo.getTimeSince(theGameInfo.timeLastPacketReceived) > gameControllerTimeOut)
41 {
42 ledRequest.ledStates[LEDRequest::earsLeft324Deg] = LEDRequest::blinking;
43 ledRequest.ledStates[LEDRequest::earsLeft144Deg] = LEDRequest::blinking;
44 }
45
46 int numberOfConnectedTeammates = static_cast<int>(theTeamData.teammates.size());
47 if(numberOfConnectedTeammates > 0)
48 {
49 ledRequest.ledStates[LEDRequest::earsLeft0Deg] = LEDRequest::on;
50 ledRequest.ledStates[LEDRequest::earsLeft36Deg] = LEDRequest::on;
51 }
52 if(numberOfConnectedTeammates > 1)
53 {
54 ledRequest.ledStates[LEDRequest::earsLeft72Deg] = LEDRequest::on;
55 ledRequest.ledStates[LEDRequest::earsLeft108Deg] = LEDRequest::on;
56 }
57 if(numberOfConnectedTeammates > 2)
58 {
59 ledRequest.ledStates[LEDRequest::earsLeft180Deg] = LEDRequest::on;
60 ledRequest.ledStates[LEDRequest::earsLeft216Deg] = LEDRequest::on;
61 }
62 if(numberOfConnectedTeammates > 3)
63 {
64 ledRequest.ledStates[LEDRequest::earsLeft252Deg] = LEDRequest::on;
65 ledRequest.ledStates[LEDRequest::earsLeft288Deg] = LEDRequest::on;
66 }
67}
68
69void LEDHandler::setEyeColor(LEDRequest& ledRequest,
70 bool left,
71 EyeColor col,
72 LEDRequest::LEDState s)
73{
74 LEDRequest::LED first = left ? LEDRequest::faceLeftRed0Deg : LEDRequest::faceRightRed0Deg;
75
76 static const int redOffset = 0,
77 greenOffset = LEDRequest::faceLeftGreen0Deg - LEDRequest::faceLeftRed0Deg,
78 blueOffset = LEDRequest::faceLeftBlue0Deg - LEDRequest::faceLeftRed0Deg,
79 numOfLEDsPerColor = LEDRequest::faceLeftRed315Deg - LEDRequest::faceLeftRed0Deg;
80
81 LEDRequest::LEDState halfState = s == LEDRequest::off ? LEDRequest::off : LEDRequest::half;
82
83 switch(col)
84 {
85 case red:
86 for(int i = 0; i <= numOfLEDsPerColor; i++)
87 ledRequest.ledStates[first + redOffset + i] = s;
88 break;
89 case green:
90 for(int i = 0; i <= numOfLEDsPerColor; i++)
91 ledRequest.ledStates[first + greenOffset + i] = s;
92 break;
93 case blue:
94 for(int i = 0; i <= numOfLEDsPerColor; i++)
95 ledRequest.ledStates[first + blueOffset + i] = s;
96 break;
97 case white:
98 for(int i = 0; i <= numOfLEDsPerColor; i++)
99 ledRequest.ledStates[first + redOffset + i] = s;
100 for(int i = 0; i <= numOfLEDsPerColor; i++)
101 ledRequest.ledStates[first + greenOffset + i] = s;
102 for(int i = 0; i <= numOfLEDsPerColor; i++)
103 ledRequest.ledStates[first + blueOffset + i] = s;
104 break;
105 case magenta:
106 for(int i = 0; i <= numOfLEDsPerColor; i++)
107 ledRequest.ledStates[first + redOffset + i] = halfState;
108 for(int i = 0; i <= numOfLEDsPerColor; i++)
109 ledRequest.ledStates[first + blueOffset + i] = s;
110 break;
111 case yellow:
112 for(int i = 0; i <= numOfLEDsPerColor; i++)
113 ledRequest.ledStates[first + greenOffset + i] = halfState;
114 for(int i = 0; i <= numOfLEDsPerColor; i++)
115 ledRequest.ledStates[first + redOffset + i] = s;
116 break;
117 case cyan:
118 for(int i = 0; i <= numOfLEDsPerColor; i++)
119 ledRequest.ledStates[first + greenOffset + i] = halfState;
120 for(int i = 0; i <= numOfLEDsPerColor; i++)
121 ledRequest.ledStates[first + blueOffset + i] = s;
122 break;
123 default:
124 FAIL("Unknown color.");
125 break;
126 }
127}
128
129void LEDHandler::setLeftEye(LEDRequest& ledRequest)
130{
131 //no groundContact
132 if(!theGroundContactState.contact/* && (theFrameInfo.time & 512)*/)
133 setEyeColor(ledRequest, true, yellow, LEDRequest::on);
134 else
135 {
136 bool ballSeen = theFrameInfo.getTimeSince(theBallModel.timeWhenLastSeen) < 250;
137 bool featureSeen = theFrameInfo.getTimeSince(theFieldFeatureOverview.combinedStatus.lastSeen) < 250;
138 bool redPresent = theRefereePosePercept.isThereRedColor;
139
140 if(ballSeen && featureSeen)
141 setEyeColor(ledRequest, true, red, LEDRequest::on);
142 else if(ballSeen)
143 setEyeColor(ledRequest, true, white, LEDRequest::on);
144 else if(featureSeen)
145 setEyeColor(ledRequest, true, blue, LEDRequest::on);
146 else if(redPresent)
147 setEyeColor(ledRequest, true, green, LEDRequest::on);
148 }
149}
150
151void LEDHandler::setRightEye(LEDRequest& ledRequest)
152{
153 if(theTeamBehaviorStatus.role.playsTheBall())
154 setEyeColor(ledRequest, false, red, LEDRequest::on);
155 else if(theTeamBehaviorStatus.role.isGoalkeeper())
156 setEyeColor(ledRequest, false, blue, LEDRequest::on);
157 else if(theTeamBehaviorStatus.role.supporterIndex() == 0)
158 setEyeColor(ledRequest, false, white, LEDRequest::on);
159 else if(theTeamBehaviorStatus.role.supporterIndex() == 1)
160 setEyeColor(ledRequest, false, yellow, LEDRequest::on);
161 else if(theTeamBehaviorStatus.role.supporterIndex() == 2)
162 setEyeColor(ledRequest, false, green, LEDRequest::on);
163 else if(theTeamBehaviorStatus.role.supporterIndex() == 3)
164 setEyeColor(ledRequest, false, cyan, LEDRequest::on);
165}
166
167void LEDHandler::setHead(LEDRequest& ledRequest)
168{
169 for(unsigned i = LEDRequest::headRearLeft0; i <= LEDRequest::headMiddleLeft0; i++)
170 ledRequest.ledStates[i] = LEDRequest::off;
171
172 if(theSystemSensorData.batteryCharging)
173 {
174 for(LEDRequest::LED i = LEDRequest::firstHeadLED; i <= LEDRequest::lastHeadLED; i = LEDRequest::LED(unsigned(i) + 1))
175 ledRequest.ledStates[i] = LEDRequest::off;
176
177 ++chargingLED %= (LEDRequest::numOfHeadLEDs * chargingLightSlowness);
178 const LEDRequest::LED currentLED = headLEDCircle[chargingLED / chargingLightSlowness];
179 const LEDRequest::LED nextLED = headLEDCircle[(chargingLED / chargingLightSlowness + 1u) % LEDRequest::numOfHeadLEDs];
180 ledRequest.ledStates[currentLED] = LEDRequest::on;
181 ledRequest.ledStates[nextLED] = LEDRequest::on;
182 }
183}
184
185void LEDHandler::setChestButton(LEDRequest& ledRequest)
186{
187 switch(theRobotInfo.mode)
188 {
189 case RobotInfo::unstiff:
190 ledRequest.ledStates[LEDRequest::chestBlue] = LEDRequest::blinking;
191 break;
192 case RobotInfo::calibration:
193 ledRequest.ledStates[LEDRequest::chestRed] = LEDRequest::on;
194 ledRequest.ledStates[LEDRequest::chestBlue] = LEDRequest::on;
195 break;
196 case RobotInfo::active:
197 default:
198 if(theRobotInfo.penalty != PENALTY_NONE)
199 ledRequest.ledStates[LEDRequest::chestRed] = LEDRequest::on;
200 else
201 switch(theGameInfo.state)
202 {
203 case STATE_READY:
204 ledRequest.ledStates[LEDRequest::chestBlue] = LEDRequest::on;
205 break;
206 case STATE_SET:
207 ledRequest.ledStates[LEDRequest::chestRed] = LEDRequest::on;
208 ledRequest.ledStates[LEDRequest::chestGreen] = LEDRequest::half;
209 break;
210 case STATE_PLAYING:
211 ledRequest.ledStates[LEDRequest::chestGreen] = LEDRequest::on;
212 break;
213 }
214 }
215}
216
217void LEDHandler::setLeftFoot(LEDRequest& ledRequest)
218{
219 switch(theOwnTeamInfo.teamColor)
220 {
221 case TEAM_ORANGE:
222 ledRequest.ledStates[LEDRequest::footLeftGreen] = LEDRequest::half;
223 case TEAM_RED:
224 ledRequest.ledStates[LEDRequest::footLeftRed] = LEDRequest::on;
225 break;
226 case TEAM_WHITE:
227 ledRequest.ledStates[LEDRequest::footLeftBlue] = LEDRequest::on;
228 case TEAM_YELLOW:
229 ledRequest.ledStates[LEDRequest::footLeftRed] = LEDRequest::on;
230 case TEAM_GREEN:
231 ledRequest.ledStates[LEDRequest::footLeftGreen] = LEDRequest::on;
232 break;
233 case TEAM_PURPLE:
234 ledRequest.ledStates[LEDRequest::footLeftRed] = LEDRequest::on;
235 case TEAM_BLUE:
236 ledRequest.ledStates[LEDRequest::footLeftBlue] = LEDRequest::on;
237 break;
238 case TEAM_GRAY:
239 ledRequest.ledStates[LEDRequest::footLeftBlue] = LEDRequest::half;
240 case TEAM_BROWN: // more a darker yellow
241 ledRequest.ledStates[LEDRequest::footLeftRed] = LEDRequest::half;
242 ledRequest.ledStates[LEDRequest::footLeftGreen] = LEDRequest::half;
243 break;
244 }
245}
246
247void LEDHandler::setRightFoot(LEDRequest& ledRequest)
248{
249 if(theGameInfo.state == STATE_INITIAL &&
250 theGameInfo.gamePhase == GAME_PHASE_PENALTYSHOOT &&
251 theGameInfo.kickingTeam == theOwnTeamInfo.teamNumber)
252 ledRequest.ledStates[LEDRequest::footRightGreen] = LEDRequest::on;
253 else if(theGameInfo.state == STATE_INITIAL &&
254 theGameInfo.gamePhase == GAME_PHASE_PENALTYSHOOT &&
255 theGameInfo.kickingTeam != theOwnTeamInfo.teamNumber)
256 {
257 ledRequest.ledStates[LEDRequest::footRightRed] = LEDRequest::on;
258 ledRequest.ledStates[LEDRequest::footRightGreen] = LEDRequest::on;
259 }
260 else if(theFrameInfo.getTimeSince(theGameInfo.timeLastPacketReceived) < gameControllerTimeOut &&
261 theGameInfo.state <= STATE_SET &&
262 theGameInfo.kickingTeam == theOwnTeamInfo.teamNumber)
263 {
264 ledRequest.ledStates[LEDRequest::footRightRed] = LEDRequest::on;
265 ledRequest.ledStates[LEDRequest::footRightGreen] = LEDRequest::on;
266 ledRequest.ledStates[LEDRequest::footRightBlue] = LEDRequest::on;
267 }
268}
269
270void LEDHandler::setBatteryLevelInEar(LEDRequest& ledRequest, LEDRequest::LED baseLED)
271{
272 int onLEDs = std::min(static_cast<int>(theSystemSensorData.batteryLevel / 0.1f), 9);
273
274 for(int i = 0; i <= onLEDs; ++i)
275 ledRequest.ledStates[baseLED + i] = LEDRequest::on;
276}
277
278MAKE_MODULE(LEDHandler, behaviorControl);
Explications#
Dans cet exemple, l’objectif est de modifier la couleur des yeux afin qu’elle reflète la détection de la couleur rouge par la caméra. Pour ce faire, nous devons adapter les modules responsables de l’affichage des LED dans les yeux lorsque la représentation de détection de rouge est activée.
Les étapes à suivre sont les suivantes :
Inclusion de la représentation : Dans le fichier LEDHandler.h, incluez le fichier de la représentation en ajoutant :
#include "Representations/Perception/ColorDetects/ColorDetect.h"
Mise à jour des dépendances : Ajoutez la représentation dans la liste des REQUIRES :
REQUIRES(ColorDetect),
Modification de la méthode concernée : Dans le fichier LEDHandler.cpp, modifiez la méthode setLeftEye pour intégrer la nouvelle logique. Par exemple, récupérez la valeur de la détection de rouge :
bool redPresent = theRefereePosePercept.isThereRedColor;
Ensuite, ajoutez une condition dans la séquence de choix de couleur pour activer la LED verte lorsque du rouge est détecté :
if(ballSeen && featureSeen) setEyeColor(ledRequest, true, red, LEDRequest::on); else if(ballSeen) setEyeColor(ledRequest, true, white, LEDRequest::on); else if(featureSeen) setEyeColor(ledRequest, true, blue, LEDRequest::on); else if(redPresent) setEyeColor(ledRequest, true, green, LEDRequest::on);
Ainsi, vous adaptez le comportement du module LEDHandler pour que la couleur affichée dans l’œil gauche change en fonction de la détection de la couleur rouge par la caméra. Vous pouvez maintenant passer aux tests avec le tutoriel suivant.