23 #include "led_thread.h" 24 #include "dcm_utils.h" 26 #include <utils/system/pathparser.h> 28 #include <alproxies/allauncherproxy.h> 29 #include <alproxies/dcmproxy.h> 30 #include <alproxies/almemoryproxy.h> 31 #include <alcore/alerror.h> 32 #include <almemoryfastaccess/almemoryfastaccess.h> 34 #include <interfaces/LedInterface.h> 35 #include <interfaces/NaoJointPositionInterface.h> 42 LED_CHESTBOARD_RED, LED_CHESTBOARD_GREEN, LED_CHESTBOARD_BLUE,
43 LED_EARS_LEFT_0DEG, LED_EARS_LEFT_36DEG, LED_EARS_LEFT_72DEG,
44 LED_EARS_LEFT_108DEG, LED_EARS_LEFT_144DEG, LED_EARS_LEFT_180DEG,
45 LED_EARS_LEFT_216DEG, LED_EARS_LEFT_252DEG, LED_EARS_LEFT_288DEG,
47 LED_EARS_RIGHT_0DEG, LED_EARS_RIGHT_36DEG, LED_EARS_RIGHT_72DEG,
48 LED_EARS_RIGHT_108DEG, LED_EARS_RIGHT_144DEG, LED_EARS_RIGHT_180DEG,
49 LED_EARS_RIGHT_216DEG, LED_EARS_RIGHT_252DEG, LED_EARS_RIGHT_288DEG,
50 LED_EARS_RIGHT_324DEG,
51 LED_FACE_LEFT_RED_0DEG, LED_FACE_LEFT_RED_45DEG,
52 LED_FACE_LEFT_RED_90DEG, LED_FACE_LEFT_RED_135DEG, LED_FACE_LEFT_RED_180DEG,
53 LED_FACE_LEFT_RED_225DEG, LED_FACE_LEFT_RED_270DEG, LED_FACE_LEFT_RED_315DEG,
54 LED_FACE_LEFT_GREEN_0DEG, LED_FACE_LEFT_GREEN_45DEG, LED_FACE_LEFT_GREEN_90DEG,
55 LED_FACE_LEFT_GREEN_135DEG, LED_FACE_LEFT_GREEN_180DEG,
56 LED_FACE_LEFT_GREEN_225DEG, LED_FACE_LEFT_GREEN_270DEG,
57 LED_FACE_LEFT_GREEN_315DEG,
58 LED_FACE_LEFT_BLUE_0DEG, LED_FACE_LEFT_BLUE_45DEG,
59 LED_FACE_LEFT_BLUE_90DEG, LED_FACE_LEFT_BLUE_135DEG,
60 LED_FACE_LEFT_BLUE_180DEG, LED_FACE_LEFT_BLUE_225DEG,
61 LED_FACE_LEFT_BLUE_270DEG, LED_FACE_LEFT_BLUE_315DEG,
62 LED_FACE_RIGHT_RED_0DEG, LED_FACE_RIGHT_RED_45DEG, LED_FACE_RIGHT_RED_90DEG,
63 LED_FACE_RIGHT_RED_135DEG, LED_FACE_RIGHT_RED_180DEG, LED_FACE_RIGHT_RED_225DEG,
64 LED_FACE_RIGHT_RED_270DEG, LED_FACE_RIGHT_RED_315DEG,
65 LED_FACE_RIGHT_GREEN_0DEG, LED_FACE_RIGHT_GREEN_45DEG, LED_FACE_RIGHT_GREEN_90DEG,
66 LED_FACE_RIGHT_GREEN_135DEG, LED_FACE_RIGHT_GREEN_180DEG,
67 LED_FACE_RIGHT_GREEN_225DEG,
68 LED_FACE_RIGHT_GREEN_270DEG, LED_FACE_RIGHT_GREEN_315DEG,
69 LED_FACE_RIGHT_BLUE_0DEG, LED_FACE_RIGHT_BLUE_45DEG,
70 LED_FACE_RIGHT_BLUE_90DEG, LED_FACE_RIGHT_BLUE_135DEG,
71 LED_FACE_RIGHT_BLUE_180DEG, LED_FACE_RIGHT_BLUE_225DEG,
72 LED_FACE_RIGHT_BLUE_270DEG, LED_FACE_RIGHT_BLUE_315DEG,
73 LED_LFOOT_RED, LED_LFOOT_GREEN, LED_LFOOT_BLUE,
74 LED_RFOOT_RED, LED_RFOOT_GREEN, LED_RFOOT_BLUE,
89 :
Thread(
"NaoQiLedThread",
Thread::OPMODE_WAITFORWAKEUP),
104 __cfg_verbose_face =
false;
106 __cfg_verbose_face =
config->
get_bool(
"/hardware/nao/leds/verbose_face");
113 __subd_prefix = (std::string)__dcm->getPrefix()[0];
114 }
catch (AL::ALError &e) {
115 throw Exception(
"Failed to get DCM prefix: %s", e.toString().c_str());
119 std::vector<std::string> leddevs;
121 leddevs = dcm::get_devices(__dcm, __almem,
"Led");
122 }
catch (AL::ALError &e) {
123 throw Exception(
"Failed to get LED devices: %s", e.toString().c_str());
127 std::string prefix = __subd_prefix;
128 std::vector<std::string> keys;
129 keys.resize(LedTypeN);
130 __values.resize(LedTypeN);
132 keys[LED_CHESTBOARD_RED] = prefix +
"ChestBoard/Led/Red/Actuator/Value";
133 keys[LED_CHESTBOARD_GREEN] = prefix +
"ChestBoard/Led/Green/Actuator/Value";
134 keys[LED_CHESTBOARD_BLUE] = prefix +
"ChestBoard/Led/Blue/Actuator/Value";
136 prefix = __subd_prefix +
"Ears/Led/";
137 keys[LED_EARS_LEFT_0DEG] = prefix +
"Left/0Deg/Actuator/Value";
138 keys[LED_EARS_LEFT_36DEG] = prefix +
"Left/36Deg/Actuator/Value";
139 keys[LED_EARS_LEFT_72DEG] = prefix +
"Left/72Deg/Actuator/Value";
140 keys[LED_EARS_LEFT_108DEG] = prefix +
"Left/108Deg/Actuator/Value";
141 keys[LED_EARS_LEFT_144DEG] = prefix +
"Left/144Deg/Actuator/Value";
142 keys[LED_EARS_LEFT_180DEG] = prefix +
"Left/180Deg/Actuator/Value";
143 keys[LED_EARS_LEFT_216DEG] = prefix +
"Left/216Deg/Actuator/Value";
144 keys[LED_EARS_LEFT_252DEG] = prefix +
"Left/252Deg/Actuator/Value";
145 keys[LED_EARS_LEFT_288DEG] = prefix +
"Left/288Deg/Actuator/Value";
146 keys[LED_EARS_LEFT_324DEG] = prefix +
"Left/324Deg/Actuator/Value";
148 keys[LED_EARS_RIGHT_0DEG] = prefix +
"Right/0Deg/Actuator/Value";
149 keys[LED_EARS_RIGHT_36DEG] = prefix +
"Right/36Deg/Actuator/Value";
150 keys[LED_EARS_RIGHT_72DEG] = prefix +
"Right/72Deg/Actuator/Value";
151 keys[LED_EARS_RIGHT_108DEG] = prefix +
"Right/108Deg/Actuator/Value";
152 keys[LED_EARS_RIGHT_144DEG] = prefix +
"Right/144Deg/Actuator/Value";
153 keys[LED_EARS_RIGHT_180DEG] = prefix +
"Right/180Deg/Actuator/Value";
154 keys[LED_EARS_RIGHT_216DEG] = prefix +
"Right/216Deg/Actuator/Value";
155 keys[LED_EARS_RIGHT_252DEG] = prefix +
"Right/252Deg/Actuator/Value";
156 keys[LED_EARS_RIGHT_288DEG] = prefix +
"Right/288Deg/Actuator/Value";
157 keys[LED_EARS_RIGHT_324DEG] = prefix +
"Right/324Deg/Actuator/Value";
159 prefix = __subd_prefix +
"Face/Led/";
160 keys[LED_FACE_LEFT_RED_0DEG] = prefix +
"Red/Left/0Deg/Actuator/Value";
161 keys[LED_FACE_LEFT_RED_45DEG] = prefix +
"Red/Left/45Deg/Actuator/Value";
162 keys[LED_FACE_LEFT_RED_90DEG] = prefix +
"Red/Left/90Deg/Actuator/Value";
163 keys[LED_FACE_LEFT_RED_135DEG] = prefix +
"Red/Left/135Deg/Actuator/Value";
164 keys[LED_FACE_LEFT_RED_180DEG] = prefix +
"Red/Left/180Deg/Actuator/Value";
165 keys[LED_FACE_LEFT_RED_225DEG] = prefix +
"Red/Left/225Deg/Actuator/Value";
166 keys[LED_FACE_LEFT_RED_270DEG] = prefix +
"Red/Left/270Deg/Actuator/Value";
167 keys[LED_FACE_LEFT_RED_315DEG] = prefix +
"Red/Left/315Deg/Actuator/Value";
169 keys[LED_FACE_LEFT_GREEN_0DEG] = prefix +
"Green/Left/0Deg/Actuator/Value";
170 keys[LED_FACE_LEFT_GREEN_45DEG] = prefix +
"Green/Left/45Deg/Actuator/Value";
171 keys[LED_FACE_LEFT_GREEN_90DEG] = prefix +
"Green/Left/90Deg/Actuator/Value";
172 keys[LED_FACE_LEFT_GREEN_135DEG] = prefix +
"Green/Left/135Deg/Actuator/Value";
173 keys[LED_FACE_LEFT_GREEN_180DEG] = prefix +
"Green/Left/180Deg/Actuator/Value";
174 keys[LED_FACE_LEFT_GREEN_225DEG] = prefix +
"Green/Left/225Deg/Actuator/Value";
175 keys[LED_FACE_LEFT_GREEN_270DEG] = prefix +
"Green/Left/270Deg/Actuator/Value";
176 keys[LED_FACE_LEFT_GREEN_315DEG] = prefix +
"Green/Left/315Deg/Actuator/Value";
178 keys[LED_FACE_LEFT_BLUE_0DEG] = prefix +
"Blue/Left/0Deg/Actuator/Value";
179 keys[LED_FACE_LEFT_BLUE_45DEG] = prefix +
"Blue/Left/45Deg/Actuator/Value";
180 keys[LED_FACE_LEFT_BLUE_90DEG] = prefix +
"Blue/Left/90Deg/Actuator/Value";
181 keys[LED_FACE_LEFT_BLUE_135DEG] = prefix +
"Blue/Left/135Deg/Actuator/Value";
182 keys[LED_FACE_LEFT_BLUE_180DEG] = prefix +
"Blue/Left/180Deg/Actuator/Value";
183 keys[LED_FACE_LEFT_BLUE_225DEG] = prefix +
"Blue/Left/225Deg/Actuator/Value";
184 keys[LED_FACE_LEFT_BLUE_270DEG] = prefix +
"Blue/Left/270Deg/Actuator/Value";
185 keys[LED_FACE_LEFT_BLUE_315DEG] = prefix +
"Blue/Left/315Deg/Actuator/Value";
188 keys[LED_FACE_RIGHT_RED_0DEG] = prefix +
"Red/Right/0Deg/Actuator/Value";
189 keys[LED_FACE_RIGHT_RED_45DEG] = prefix +
"Red/Right/45Deg/Actuator/Value";
190 keys[LED_FACE_RIGHT_RED_90DEG] = prefix +
"Red/Right/90Deg/Actuator/Value";
191 keys[LED_FACE_RIGHT_RED_135DEG] = prefix +
"Red/Right/135Deg/Actuator/Value";
192 keys[LED_FACE_RIGHT_RED_180DEG] = prefix +
"Red/Right/180Deg/Actuator/Value";
193 keys[LED_FACE_RIGHT_RED_225DEG] = prefix +
"Red/Right/225Deg/Actuator/Value";
194 keys[LED_FACE_RIGHT_RED_270DEG] = prefix +
"Red/Right/270Deg/Actuator/Value";
195 keys[LED_FACE_RIGHT_RED_315DEG] = prefix +
"Red/Right/315Deg/Actuator/Value";
197 keys[LED_FACE_RIGHT_GREEN_0DEG] = prefix +
"Green/Right/0Deg/Actuator/Value";
198 keys[LED_FACE_RIGHT_GREEN_45DEG] = prefix +
"Green/Right/45Deg/Actuator/Value";
199 keys[LED_FACE_RIGHT_GREEN_90DEG] = prefix +
"Green/Right/90Deg/Actuator/Value";
200 keys[LED_FACE_RIGHT_GREEN_135DEG] = prefix +
"Green/Right/135Deg/Actuator/Value";
201 keys[LED_FACE_RIGHT_GREEN_180DEG] = prefix +
"Green/Right/180Deg/Actuator/Value";
202 keys[LED_FACE_RIGHT_GREEN_225DEG] = prefix +
"Green/Right/225Deg/Actuator/Value";
203 keys[LED_FACE_RIGHT_GREEN_270DEG] = prefix +
"Green/Right/270Deg/Actuator/Value";
204 keys[LED_FACE_RIGHT_GREEN_315DEG] = prefix +
"Green/Right/315Deg/Actuator/Value";
206 keys[LED_FACE_RIGHT_BLUE_0DEG] = prefix +
"Blue/Right/0Deg/Actuator/Value";
207 keys[LED_FACE_RIGHT_BLUE_45DEG] = prefix +
"Blue/Right/45Deg/Actuator/Value";
208 keys[LED_FACE_RIGHT_BLUE_90DEG] = prefix +
"Blue/Right/90Deg/Actuator/Value";
209 keys[LED_FACE_RIGHT_BLUE_135DEG] = prefix +
"Blue/Right/135Deg/Actuator/Value";
210 keys[LED_FACE_RIGHT_BLUE_180DEG] = prefix +
"Blue/Right/180Deg/Actuator/Value";
211 keys[LED_FACE_RIGHT_BLUE_225DEG] = prefix +
"Blue/Right/225Deg/Actuator/Value";
212 keys[LED_FACE_RIGHT_BLUE_270DEG] = prefix +
"Blue/Right/270Deg/Actuator/Value";
213 keys[LED_FACE_RIGHT_BLUE_315DEG] = prefix +
"Blue/Right/315Deg/Actuator/Value";
216 prefix = __subd_prefix;
217 keys[LED_LFOOT_RED] = prefix +
"LFoot/Led/Red/Actuator/Value";
218 keys[LED_LFOOT_GREEN] = prefix +
"LFoot/Led/Green/Actuator/Value";
219 keys[LED_LFOOT_BLUE] = prefix +
"LFoot/Led/Blue/Actuator/Value";
221 keys[LED_RFOOT_RED] = prefix +
"RFoot/Led/Red/Actuator/Value";
222 keys[LED_RFOOT_GREEN] = prefix +
"RFoot/Led/Green/Actuator/Value";
223 keys[LED_RFOOT_BLUE] = prefix +
"RFoot/Led/Blue/Actuator/Value";
225 __memfa.reset(
new AL::ALMemoryFastAccess());
228 }
catch (AL::ALError &e) {
229 throw Exception(
"Failed to setup fast memory access: %s",
230 e.toString().c_str());
235 if (! joint_pos_if->has_writer()) {
237 throw Exception(
"Joint Position interface has no writer");
239 joint_pos_if->read();
240 bool skip_head_leds =
241 (joint_pos_if->robot_type() != NaoJointPositionInterface::ROBOTYPE_ACADEMIC);
244 std::vector<std::string>::iterator l;
245 for (l = leddevs.begin(); l != leddevs.end(); ++l) {
247 std::string loc = pp[subdpp.size()];
249 if (! __cfg_verbose_face) {
251 if (locpp[0] ==
"Face")
continue;
253 if (skip_head_leds) {
255 if (locpp[0] ==
"Head")
continue;
258 std::string
id =
"Nao LED " + loc;
259 PathParser::size_type i;
260 for (i = subdpp.size() + 2; (i < pp.size()) && (pp[i] !=
"Actuator"); ++i) {
268 __leds.insert(make_pair(iface, *l +
"/Value"));
271 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) {
272 if (i->first != last) {
283 std::string left_right[2] = {
"Left",
"Right" };
284 std::string rgb[3] = {
"Red",
"Green",
"Blue" };
285 std::string angles[8] = {
"0",
"45",
"90",
"135",
"180",
"225",
"270",
"315"};
287 for (
unsigned int lr = 0; lr < 2; ++lr) {
288 for (
unsigned int cl = 0; cl < 3; ++cl) {
289 std::string
id =
"Nao LED Face/" + rgb[cl] +
"/" + left_right[lr];
293 for (
unsigned int a = 0; a < 8; ++a) {
294 std::string entry =
"Face/Led/" + rgb[cl] +
"/" + left_right[lr];
296 __subd_prefix + entry +
"/" + angles[a] +
"Deg/Actuator/Value";
298 __leds.insert(make_pair(iface, memid));
305 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) {
306 if (i->first != last) {
317 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) {
318 if (i->first == last)
continue;
321 std::pair<LedMap::iterator, LedMap::iterator> ret =
322 __leds.equal_range(i->first);
324 for (LedMap::iterator j = ret.first; j != ret.second; ++j) {
327 for (
unsigned int k = 0; k < keys.size(); ++k) {
328 if (keys[k] == j->second) {
329 __memids.insert(std::make_pair(i->first, k));
339 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) {
340 if (i->first != last) {
354 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) {
355 if (i->first != last) {
369 __memfa->GetValues(__values);
372 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) {
373 if (i->first == last)
continue;
377 std::pair<LedMemMap::iterator, LedMemMap::iterator> ret =
378 __memids.equal_range(i->first);
379 for (LedMemMap::iterator j = ret.first; j != ret.second; ++j) {
380 if (__values[j->second] > maxval) maxval = __values[j->second];
383 if (maxval != i->first->intensity()) {
384 i->first->set_intensity(maxval);
398 std::string kind =
"Merge";
399 int dcm_time = __dcm->getTime(0);
405 if (led_if == NULL)
return false;
407 std::pair<LedMap::iterator, LedMap::iterator> ret =
408 __leds.equal_range(led_if);
411 for (LedMap::iterator i = ret.first; i != ret.second; ++i) {
412 printf(
"Set %s to %f\n", i->second.c_str(), sim->
intensity());
413 dcm::set_value(__dcm, i->second, kind, sim->
intensity(),
414 (int)roundf(dcm_time + sim->
time_sec() * 1000.));
416 }
else if (dynamic_cast<LedInterface::TurnOnMessage *>(message) != NULL) {
417 for (LedMap::iterator i = ret.first; i != ret.second; ++i) {
418 dcm::set_value(__dcm, i->second, kind, 1., dcm_time);
420 }
else if (dynamic_cast<LedInterface::TurnOffMessage *>(message) != NULL) {
421 for (LedMap::iterator i = ret.first; i != ret.second; ++i) {
422 dcm::set_value(__dcm, i->second, kind, 0., dcm_time);
virtual void finalize()
Finalize the thread.
NaoJointPositionInterface Fawkes BlackBoard Interface.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
float intensity() const
Get intensity value.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
AL::ALPtr< AL::ALBroker > naoqi_broker
NaoQi broker.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Thread class encapsulation of pthreads.
Base class for all Fawkes BlackBoard interfaces.
virtual void init()
Initialize the thread.
Thread aspect to use blocked timing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
virtual void loop()
Code to execute in the thread.
Base class for exceptions in Fawkes.
float time_sec() const
Get time_sec value.
SetIntensityMessage Fawkes BlackBoard Interface Message.
bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
LedInterface Fawkes BlackBoard Interface.
NaoQiLedThread()
Constructor.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual ~NaoQiLedThread()
Destructor.
Configuration * config
This is the Configuration member used to access the configuration.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
BlackBoard interface listener.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.