23 #include "led_thread.h"
25 #include "dcm_utils.h"
27 #include <alcore/alerror.h>
28 #include <almemoryfastaccess/almemoryfastaccess.h>
29 #include <alproxies/allauncherproxy.h>
30 #include <alproxies/almemoryproxy.h>
31 #include <alproxies/dcmproxy.h>
32 #include <interfaces/LedInterface.h>
33 #include <interfaces/NaoJointPositionInterface.h>
34 #include <utils/system/pathparser.h>
57 LED_EARS_RIGHT_108DEG,
58 LED_EARS_RIGHT_144DEG,
59 LED_EARS_RIGHT_180DEG,
60 LED_EARS_RIGHT_216DEG,
61 LED_EARS_RIGHT_252DEG,
62 LED_EARS_RIGHT_288DEG,
63 LED_EARS_RIGHT_324DEG,
64 LED_FACE_LEFT_RED_0DEG,
65 LED_FACE_LEFT_RED_45DEG,
66 LED_FACE_LEFT_RED_90DEG,
67 LED_FACE_LEFT_RED_135DEG,
68 LED_FACE_LEFT_RED_180DEG,
69 LED_FACE_LEFT_RED_225DEG,
70 LED_FACE_LEFT_RED_270DEG,
71 LED_FACE_LEFT_RED_315DEG,
72 LED_FACE_LEFT_GREEN_0DEG,
73 LED_FACE_LEFT_GREEN_45DEG,
74 LED_FACE_LEFT_GREEN_90DEG,
75 LED_FACE_LEFT_GREEN_135DEG,
76 LED_FACE_LEFT_GREEN_180DEG,
77 LED_FACE_LEFT_GREEN_225DEG,
78 LED_FACE_LEFT_GREEN_270DEG,
79 LED_FACE_LEFT_GREEN_315DEG,
80 LED_FACE_LEFT_BLUE_0DEG,
81 LED_FACE_LEFT_BLUE_45DEG,
82 LED_FACE_LEFT_BLUE_90DEG,
83 LED_FACE_LEFT_BLUE_135DEG,
84 LED_FACE_LEFT_BLUE_180DEG,
85 LED_FACE_LEFT_BLUE_225DEG,
86 LED_FACE_LEFT_BLUE_270DEG,
87 LED_FACE_LEFT_BLUE_315DEG,
88 LED_FACE_RIGHT_RED_0DEG,
89 LED_FACE_RIGHT_RED_45DEG,
90 LED_FACE_RIGHT_RED_90DEG,
91 LED_FACE_RIGHT_RED_135DEG,
92 LED_FACE_RIGHT_RED_180DEG,
93 LED_FACE_RIGHT_RED_225DEG,
94 LED_FACE_RIGHT_RED_270DEG,
95 LED_FACE_RIGHT_RED_315DEG,
96 LED_FACE_RIGHT_GREEN_0DEG,
97 LED_FACE_RIGHT_GREEN_45DEG,
98 LED_FACE_RIGHT_GREEN_90DEG,
99 LED_FACE_RIGHT_GREEN_135DEG,
100 LED_FACE_RIGHT_GREEN_180DEG,
101 LED_FACE_RIGHT_GREEN_225DEG,
102 LED_FACE_RIGHT_GREEN_270DEG,
103 LED_FACE_RIGHT_GREEN_315DEG,
104 LED_FACE_RIGHT_BLUE_0DEG,
105 LED_FACE_RIGHT_BLUE_45DEG,
106 LED_FACE_RIGHT_BLUE_90DEG,
107 LED_FACE_RIGHT_BLUE_135DEG,
108 LED_FACE_RIGHT_BLUE_180DEG,
109 LED_FACE_RIGHT_BLUE_225DEG,
110 LED_FACE_RIGHT_BLUE_270DEG,
111 LED_FACE_RIGHT_BLUE_315DEG,
131 :
Thread(
"NaoQiLedThread",
Thread::OPMODE_WAITFORWAKEUP),
145 cfg_verbose_face_ =
false;
147 cfg_verbose_face_ =
config->
get_bool(
"/hardware/nao/leds/verbose_face");
155 subd_prefix_ = (std::string)dcm_->getPrefix()[0];
156 }
catch (AL::ALError &e) {
157 throw Exception(
"Failed to get DCM prefix: %s", e.toString().c_str());
161 std::vector<std::string> leddevs;
163 leddevs = dcm::get_devices(dcm_, almem_,
"Led");
164 }
catch (AL::ALError &e) {
165 throw Exception(
"Failed to get LED devices: %s", e.toString().c_str());
169 std::string prefix = subd_prefix_;
170 std::vector<std::string> keys;
171 keys.resize(LedTypeN);
172 values_.resize(LedTypeN);
174 keys[LED_CHESTBOARD_RED] = prefix +
"ChestBoard/Led/Red/Actuator/Value";
175 keys[LED_CHESTBOARD_GREEN] = prefix +
"ChestBoard/Led/Green/Actuator/Value";
176 keys[LED_CHESTBOARD_BLUE] = prefix +
"ChestBoard/Led/Blue/Actuator/Value";
178 prefix = subd_prefix_ +
"Ears/Led/";
179 keys[LED_EARS_LEFT_0DEG] = prefix +
"Left/0Deg/Actuator/Value";
180 keys[LED_EARS_LEFT_36DEG] = prefix +
"Left/36Deg/Actuator/Value";
181 keys[LED_EARS_LEFT_72DEG] = prefix +
"Left/72Deg/Actuator/Value";
182 keys[LED_EARS_LEFT_108DEG] = prefix +
"Left/108Deg/Actuator/Value";
183 keys[LED_EARS_LEFT_144DEG] = prefix +
"Left/144Deg/Actuator/Value";
184 keys[LED_EARS_LEFT_180DEG] = prefix +
"Left/180Deg/Actuator/Value";
185 keys[LED_EARS_LEFT_216DEG] = prefix +
"Left/216Deg/Actuator/Value";
186 keys[LED_EARS_LEFT_252DEG] = prefix +
"Left/252Deg/Actuator/Value";
187 keys[LED_EARS_LEFT_288DEG] = prefix +
"Left/288Deg/Actuator/Value";
188 keys[LED_EARS_LEFT_324DEG] = prefix +
"Left/324Deg/Actuator/Value";
190 keys[LED_EARS_RIGHT_0DEG] = prefix +
"Right/0Deg/Actuator/Value";
191 keys[LED_EARS_RIGHT_36DEG] = prefix +
"Right/36Deg/Actuator/Value";
192 keys[LED_EARS_RIGHT_72DEG] = prefix +
"Right/72Deg/Actuator/Value";
193 keys[LED_EARS_RIGHT_108DEG] = prefix +
"Right/108Deg/Actuator/Value";
194 keys[LED_EARS_RIGHT_144DEG] = prefix +
"Right/144Deg/Actuator/Value";
195 keys[LED_EARS_RIGHT_180DEG] = prefix +
"Right/180Deg/Actuator/Value";
196 keys[LED_EARS_RIGHT_216DEG] = prefix +
"Right/216Deg/Actuator/Value";
197 keys[LED_EARS_RIGHT_252DEG] = prefix +
"Right/252Deg/Actuator/Value";
198 keys[LED_EARS_RIGHT_288DEG] = prefix +
"Right/288Deg/Actuator/Value";
199 keys[LED_EARS_RIGHT_324DEG] = prefix +
"Right/324Deg/Actuator/Value";
201 prefix = subd_prefix_ +
"Face/Led/";
202 keys[LED_FACE_LEFT_RED_0DEG] = prefix +
"Red/Left/0Deg/Actuator/Value";
203 keys[LED_FACE_LEFT_RED_45DEG] = prefix +
"Red/Left/45Deg/Actuator/Value";
204 keys[LED_FACE_LEFT_RED_90DEG] = prefix +
"Red/Left/90Deg/Actuator/Value";
205 keys[LED_FACE_LEFT_RED_135DEG] = prefix +
"Red/Left/135Deg/Actuator/Value";
206 keys[LED_FACE_LEFT_RED_180DEG] = prefix +
"Red/Left/180Deg/Actuator/Value";
207 keys[LED_FACE_LEFT_RED_225DEG] = prefix +
"Red/Left/225Deg/Actuator/Value";
208 keys[LED_FACE_LEFT_RED_270DEG] = prefix +
"Red/Left/270Deg/Actuator/Value";
209 keys[LED_FACE_LEFT_RED_315DEG] = prefix +
"Red/Left/315Deg/Actuator/Value";
211 keys[LED_FACE_LEFT_GREEN_0DEG] = prefix +
"Green/Left/0Deg/Actuator/Value";
212 keys[LED_FACE_LEFT_GREEN_45DEG] = prefix +
"Green/Left/45Deg/Actuator/Value";
213 keys[LED_FACE_LEFT_GREEN_90DEG] = prefix +
"Green/Left/90Deg/Actuator/Value";
214 keys[LED_FACE_LEFT_GREEN_135DEG] = prefix +
"Green/Left/135Deg/Actuator/Value";
215 keys[LED_FACE_LEFT_GREEN_180DEG] = prefix +
"Green/Left/180Deg/Actuator/Value";
216 keys[LED_FACE_LEFT_GREEN_225DEG] = prefix +
"Green/Left/225Deg/Actuator/Value";
217 keys[LED_FACE_LEFT_GREEN_270DEG] = prefix +
"Green/Left/270Deg/Actuator/Value";
218 keys[LED_FACE_LEFT_GREEN_315DEG] = prefix +
"Green/Left/315Deg/Actuator/Value";
220 keys[LED_FACE_LEFT_BLUE_0DEG] = prefix +
"Blue/Left/0Deg/Actuator/Value";
221 keys[LED_FACE_LEFT_BLUE_45DEG] = prefix +
"Blue/Left/45Deg/Actuator/Value";
222 keys[LED_FACE_LEFT_BLUE_90DEG] = prefix +
"Blue/Left/90Deg/Actuator/Value";
223 keys[LED_FACE_LEFT_BLUE_135DEG] = prefix +
"Blue/Left/135Deg/Actuator/Value";
224 keys[LED_FACE_LEFT_BLUE_180DEG] = prefix +
"Blue/Left/180Deg/Actuator/Value";
225 keys[LED_FACE_LEFT_BLUE_225DEG] = prefix +
"Blue/Left/225Deg/Actuator/Value";
226 keys[LED_FACE_LEFT_BLUE_270DEG] = prefix +
"Blue/Left/270Deg/Actuator/Value";
227 keys[LED_FACE_LEFT_BLUE_315DEG] = prefix +
"Blue/Left/315Deg/Actuator/Value";
229 keys[LED_FACE_RIGHT_RED_0DEG] = prefix +
"Red/Right/0Deg/Actuator/Value";
230 keys[LED_FACE_RIGHT_RED_45DEG] = prefix +
"Red/Right/45Deg/Actuator/Value";
231 keys[LED_FACE_RIGHT_RED_90DEG] = prefix +
"Red/Right/90Deg/Actuator/Value";
232 keys[LED_FACE_RIGHT_RED_135DEG] = prefix +
"Red/Right/135Deg/Actuator/Value";
233 keys[LED_FACE_RIGHT_RED_180DEG] = prefix +
"Red/Right/180Deg/Actuator/Value";
234 keys[LED_FACE_RIGHT_RED_225DEG] = prefix +
"Red/Right/225Deg/Actuator/Value";
235 keys[LED_FACE_RIGHT_RED_270DEG] = prefix +
"Red/Right/270Deg/Actuator/Value";
236 keys[LED_FACE_RIGHT_RED_315DEG] = prefix +
"Red/Right/315Deg/Actuator/Value";
238 keys[LED_FACE_RIGHT_GREEN_0DEG] = prefix +
"Green/Right/0Deg/Actuator/Value";
239 keys[LED_FACE_RIGHT_GREEN_45DEG] = prefix +
"Green/Right/45Deg/Actuator/Value";
240 keys[LED_FACE_RIGHT_GREEN_90DEG] = prefix +
"Green/Right/90Deg/Actuator/Value";
241 keys[LED_FACE_RIGHT_GREEN_135DEG] = prefix +
"Green/Right/135Deg/Actuator/Value";
242 keys[LED_FACE_RIGHT_GREEN_180DEG] = prefix +
"Green/Right/180Deg/Actuator/Value";
243 keys[LED_FACE_RIGHT_GREEN_225DEG] = prefix +
"Green/Right/225Deg/Actuator/Value";
244 keys[LED_FACE_RIGHT_GREEN_270DEG] = prefix +
"Green/Right/270Deg/Actuator/Value";
245 keys[LED_FACE_RIGHT_GREEN_315DEG] = prefix +
"Green/Right/315Deg/Actuator/Value";
247 keys[LED_FACE_RIGHT_BLUE_0DEG] = prefix +
"Blue/Right/0Deg/Actuator/Value";
248 keys[LED_FACE_RIGHT_BLUE_45DEG] = prefix +
"Blue/Right/45Deg/Actuator/Value";
249 keys[LED_FACE_RIGHT_BLUE_90DEG] = prefix +
"Blue/Right/90Deg/Actuator/Value";
250 keys[LED_FACE_RIGHT_BLUE_135DEG] = prefix +
"Blue/Right/135Deg/Actuator/Value";
251 keys[LED_FACE_RIGHT_BLUE_180DEG] = prefix +
"Blue/Right/180Deg/Actuator/Value";
252 keys[LED_FACE_RIGHT_BLUE_225DEG] = prefix +
"Blue/Right/225Deg/Actuator/Value";
253 keys[LED_FACE_RIGHT_BLUE_270DEG] = prefix +
"Blue/Right/270Deg/Actuator/Value";
254 keys[LED_FACE_RIGHT_BLUE_315DEG] = prefix +
"Blue/Right/315Deg/Actuator/Value";
256 prefix = subd_prefix_;
257 keys[LED_LFOOT_RED] = prefix +
"LFoot/Led/Red/Actuator/Value";
258 keys[LED_LFOOT_GREEN] = prefix +
"LFoot/Led/Green/Actuator/Value";
259 keys[LED_LFOOT_BLUE] = prefix +
"LFoot/Led/Blue/Actuator/Value";
261 keys[LED_RFOOT_RED] = prefix +
"RFoot/Led/Red/Actuator/Value";
262 keys[LED_RFOOT_GREEN] = prefix +
"RFoot/Led/Green/Actuator/Value";
263 keys[LED_RFOOT_BLUE] = prefix +
"RFoot/Led/Blue/Actuator/Value";
265 memfa_.reset(
new AL::ALMemoryFastAccess());
268 }
catch (AL::ALError &e) {
269 throw Exception(
"Failed to setup fast memory access: %s", e.toString().c_str());
276 throw Exception(
"Joint Position interface has no writer");
278 joint_pos_if->
read();
279 bool skip_head_leds =
280 (joint_pos_if->
robot_type() != NaoJointPositionInterface::ROBOTYPE_ACADEMIC);
283 std::vector<std::string>::iterator l;
284 for (l = leddevs.begin(); l != leddevs.end(); ++l) {
286 std::string loc = pp[subdpp.size()];
288 if (!cfg_verbose_face_) {
290 if (locpp[0] ==
"Face")
293 if (skip_head_leds) {
295 if (locpp[0] ==
"Head")
299 std::string
id =
"Nao LED " + loc;
300 PathParser::size_type i;
301 for (i = subdpp.size() + 2; (i < pp.size()) && (pp[i] !=
"Actuator"); ++i) {
308 leds_.insert(make_pair(iface, *l +
"/Value"));
311 for (LedMap::iterator i = leds_.begin(); i != leds_.end(); ++i) {
312 if (i->first != last) {
323 std::string left_right[2] = {
"Left",
"Right"};
324 std::string rgb[3] = {
"Red",
"Green",
"Blue"};
325 std::string angles[8] = {
"0",
"45",
"90",
"135",
"180",
"225",
"270",
"315"};
327 for (
unsigned int lr = 0; lr < 2; ++lr) {
328 for (
unsigned int cl = 0; cl < 3; ++cl) {
329 std::string
id =
"Nao LED Face/" + rgb[cl] +
"/" + left_right[lr];
332 for (
unsigned int a = 0; a < 8; ++a) {
333 std::string entry =
"Face/Led/" + rgb[cl] +
"/" + left_right[lr];
334 std::string memid = subd_prefix_ + entry +
"/" + angles[a] +
"Deg/Actuator/Value";
336 leds_.insert(make_pair(iface, memid));
343 for (LedMap::iterator i = leds_.begin(); i != leds_.end(); ++i) {
344 if (i->first != last) {
355 for (LedMap::iterator i = leds_.begin(); i != leds_.end(); ++i) {
356 if (i->first == last)
360 std::pair<LedMap::iterator, LedMap::iterator> ret = leds_.equal_range(i->first);
362 for (LedMap::iterator j = ret.first; j != ret.second; ++j) {
365 for (
unsigned int k = 0; k < keys.size(); ++k) {
366 if (keys[k] == j->second) {
367 memids_.insert(std::make_pair(i->first, k));
377 for (LedMap::iterator i = leds_.begin(); i != leds_.end(); ++i) {
378 if (i->first != last) {
392 for (LedMap::iterator i = leds_.begin(); i != leds_.end(); ++i) {
393 if (i->first != last) {
407 memfa_->GetValues(values_);
410 for (LedMap::iterator i = leds_.begin(); i != leds_.end(); ++i) {
411 if (i->first == last)
416 std::pair<LedMemMap::iterator, LedMemMap::iterator> ret = memids_.equal_range(i->first);
417 for (LedMemMap::iterator j = ret.first; j != ret.second; ++j) {
418 if (values_[j->second] > maxval)
419 maxval = values_[j->second];
422 if (maxval != i->first->intensity()) {
423 i->first->set_intensity(maxval);
435 std::string kind =
"Merge";
436 int dcm_time = dcm_->getTime(0);
445 std::pair<LedMap::iterator, LedMap::iterator> ret = leds_.equal_range(led_if);
448 for (LedMap::iterator i = ret.first; i != ret.second; ++i) {
449 printf(
"Set %s to %f\n", i->second.c_str(), sim->
intensity());
451 dcm_, i->second, kind, sim->
intensity(), (
int)roundf(dcm_time + sim->
time_sec() * 1000.));
454 for (LedMap::iterator i = ret.first; i != ret.second; ++i) {
455 dcm::set_value(dcm_, i->second, kind, 1., dcm_time);
458 for (LedMap::iterator i = ret.first; i != ret.second; ++i) {
459 dcm::set_value(dcm_, i->second, kind, 0., dcm_time);
virtual void finalize()
Finalize the thread.
virtual ~NaoQiLedThread()
Destructor.
bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message) noexcept
BlackBoard message received notification.
NaoQiLedThread()
Constructor.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
BlackBoard interface listener.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
Base class for exceptions in Fawkes.
Base class for all Fawkes BlackBoard interfaces.
void read()
Read from BlackBoard into local copy.
bool has_writer() const
Check if there is a writer for the interface.
SetIntensityMessage Fawkes BlackBoard Interface Message.
float intensity() const
Get intensity value.
float time_sec() const
Get time_sec value.
TurnOffMessage Fawkes BlackBoard Interface Message.
TurnOnMessage Fawkes BlackBoard Interface Message.
LedInterface Fawkes BlackBoard Interface.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
NaoJointPositionInterface Fawkes BlackBoard Interface.
RobotType robot_type() const
Get robot_type value.
AL::ALPtr< AL::ALBroker > naoqi_broker
NaoQi broker.
Thread class encapsulation of pthreads.
Fawkes library namespace.