Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * led_thread.cpp - Provide NaoQi LEDs to Fawkes 00004 * 00005 * Created: Thu Jun 30 19:52:00 2011 00006 * Copyright 2006-2011 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "led_thread.h" 00024 #include "dcm_utils.h" 00025 00026 #include <utils/system/pathparser.h> 00027 00028 #include <alproxies/allauncherproxy.h> 00029 #include <alproxies/dcmproxy.h> 00030 #include <alproxies/almemoryproxy.h> 00031 #include <alcore/alerror.h> 00032 #include <almemoryfastaccess/almemoryfastaccess.h> 00033 00034 #include <interfaces/LedInterface.h> 00035 #include <interfaces/NaoJointPositionInterface.h> 00036 00037 #include <cmath> 00038 00039 using namespace fawkes; 00040 00041 enum LedType { 00042 LED_CHESTBOARD_RED, LED_CHESTBOARD_GREEN, LED_CHESTBOARD_BLUE, 00043 LED_EARS_LEFT_0DEG, LED_EARS_LEFT_36DEG, LED_EARS_LEFT_72DEG, 00044 LED_EARS_LEFT_108DEG, LED_EARS_LEFT_144DEG, LED_EARS_LEFT_180DEG, 00045 LED_EARS_LEFT_216DEG, LED_EARS_LEFT_252DEG, LED_EARS_LEFT_288DEG, 00046 LED_EARS_LEFT_324DEG, 00047 LED_EARS_RIGHT_0DEG, LED_EARS_RIGHT_36DEG, LED_EARS_RIGHT_72DEG, 00048 LED_EARS_RIGHT_108DEG, LED_EARS_RIGHT_144DEG, LED_EARS_RIGHT_180DEG, 00049 LED_EARS_RIGHT_216DEG, LED_EARS_RIGHT_252DEG, LED_EARS_RIGHT_288DEG, 00050 LED_EARS_RIGHT_324DEG, 00051 LED_FACE_LEFT_RED_0DEG, LED_FACE_LEFT_RED_45DEG, 00052 LED_FACE_LEFT_RED_90DEG, LED_FACE_LEFT_RED_135DEG, LED_FACE_LEFT_RED_180DEG, 00053 LED_FACE_LEFT_RED_225DEG, LED_FACE_LEFT_RED_270DEG, LED_FACE_LEFT_RED_315DEG, 00054 LED_FACE_LEFT_GREEN_0DEG, LED_FACE_LEFT_GREEN_45DEG, LED_FACE_LEFT_GREEN_90DEG, 00055 LED_FACE_LEFT_GREEN_135DEG, LED_FACE_LEFT_GREEN_180DEG, 00056 LED_FACE_LEFT_GREEN_225DEG, LED_FACE_LEFT_GREEN_270DEG, 00057 LED_FACE_LEFT_GREEN_315DEG, 00058 LED_FACE_LEFT_BLUE_0DEG, LED_FACE_LEFT_BLUE_45DEG, 00059 LED_FACE_LEFT_BLUE_90DEG, LED_FACE_LEFT_BLUE_135DEG, 00060 LED_FACE_LEFT_BLUE_180DEG, LED_FACE_LEFT_BLUE_225DEG, 00061 LED_FACE_LEFT_BLUE_270DEG, LED_FACE_LEFT_BLUE_315DEG, 00062 LED_FACE_RIGHT_RED_0DEG, LED_FACE_RIGHT_RED_45DEG, LED_FACE_RIGHT_RED_90DEG, 00063 LED_FACE_RIGHT_RED_135DEG, LED_FACE_RIGHT_RED_180DEG, LED_FACE_RIGHT_RED_225DEG, 00064 LED_FACE_RIGHT_RED_270DEG, LED_FACE_RIGHT_RED_315DEG, 00065 LED_FACE_RIGHT_GREEN_0DEG, LED_FACE_RIGHT_GREEN_45DEG, LED_FACE_RIGHT_GREEN_90DEG, 00066 LED_FACE_RIGHT_GREEN_135DEG, LED_FACE_RIGHT_GREEN_180DEG, 00067 LED_FACE_RIGHT_GREEN_225DEG, 00068 LED_FACE_RIGHT_GREEN_270DEG, LED_FACE_RIGHT_GREEN_315DEG, 00069 LED_FACE_RIGHT_BLUE_0DEG, LED_FACE_RIGHT_BLUE_45DEG, 00070 LED_FACE_RIGHT_BLUE_90DEG, LED_FACE_RIGHT_BLUE_135DEG, 00071 LED_FACE_RIGHT_BLUE_180DEG, LED_FACE_RIGHT_BLUE_225DEG, 00072 LED_FACE_RIGHT_BLUE_270DEG, LED_FACE_RIGHT_BLUE_315DEG, 00073 LED_LFOOT_RED, LED_LFOOT_GREEN, LED_LFOOT_BLUE, 00074 LED_RFOOT_RED, LED_RFOOT_GREEN, LED_RFOOT_BLUE, 00075 LedTypeN 00076 }; 00077 00078 /** @class NaoQiLedThread "led_thread.h" 00079 * Thread to synchronize with LEDs. 00080 * This thread registered for data changed events to a specified LED and 00081 * updates the blackboard interface. It also processes actuation commands. 00082 * if there is a reader for any of the high frequency interfaces. It 00083 * is also responsible for processing incoming commands. 00084 */ 00085 00086 00087 /** Constructor. */ 00088 NaoQiLedThread::NaoQiLedThread() 00089 : Thread("NaoQiLedThread", Thread::OPMODE_WAITFORWAKEUP), 00090 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE), 00091 BlackBoardInterfaceListener("NaoQiLedThread") 00092 { 00093 } 00094 00095 00096 /** Destructor. */ 00097 NaoQiLedThread::~NaoQiLedThread() 00098 { 00099 } 00100 00101 void 00102 NaoQiLedThread::init() 00103 { 00104 __cfg_verbose_face = false; 00105 try { 00106 __cfg_verbose_face = config->get_bool("/hardware/nao/leds/verbose_face"); 00107 } catch (Exception &e) {} // ignored, use default 00108 00109 __dcm = naoqi_broker->getDcmProxy(); 00110 __almem = naoqi_broker->getMemoryProxy(); 00111 00112 try { 00113 __subd_prefix = (std::string)__dcm->getPrefix()[0]; 00114 } catch (AL::ALError &e) { 00115 throw Exception("Failed to get DCM prefix: %s", e.toString().c_str()); 00116 } 00117 PathParser subdpp(__subd_prefix); 00118 00119 std::vector<std::string> leddevs; 00120 try { 00121 leddevs = dcm::get_devices(__dcm, __almem, "Led"); 00122 } catch (AL::ALError &e) { 00123 throw Exception("Failed to get LED devices: %s", e.toString().c_str()); 00124 } 00125 00126 // Initialize fast memory access 00127 std::string prefix = __subd_prefix; 00128 std::vector<std::string> keys; 00129 keys.resize(LedTypeN); 00130 __values.resize(LedTypeN); 00131 00132 keys[LED_CHESTBOARD_RED] = prefix + "ChestBoard/Led/Red/Actuator/Value"; 00133 keys[LED_CHESTBOARD_GREEN] = prefix + "ChestBoard/Led/Green/Actuator/Value"; 00134 keys[LED_CHESTBOARD_BLUE] = prefix + "ChestBoard/Led/Blue/Actuator/Value"; 00135 00136 prefix = __subd_prefix + "Ears/Led/"; 00137 keys[LED_EARS_LEFT_0DEG] = prefix + "Left/0Deg/Actuator/Value"; 00138 keys[LED_EARS_LEFT_36DEG] = prefix + "Left/36Deg/Actuator/Value"; 00139 keys[LED_EARS_LEFT_72DEG] = prefix + "Left/72Deg/Actuator/Value"; 00140 keys[LED_EARS_LEFT_108DEG] = prefix + "Left/108Deg/Actuator/Value"; 00141 keys[LED_EARS_LEFT_144DEG] = prefix + "Left/144Deg/Actuator/Value"; 00142 keys[LED_EARS_LEFT_180DEG] = prefix + "Left/180Deg/Actuator/Value"; 00143 keys[LED_EARS_LEFT_216DEG] = prefix + "Left/216Deg/Actuator/Value"; 00144 keys[LED_EARS_LEFT_252DEG] = prefix + "Left/252Deg/Actuator/Value"; 00145 keys[LED_EARS_LEFT_288DEG] = prefix + "Left/288Deg/Actuator/Value"; 00146 keys[LED_EARS_LEFT_324DEG] = prefix + "Left/324Deg/Actuator/Value"; 00147 00148 keys[LED_EARS_RIGHT_0DEG] = prefix + "Right/0Deg/Actuator/Value"; 00149 keys[LED_EARS_RIGHT_36DEG] = prefix + "Right/36Deg/Actuator/Value"; 00150 keys[LED_EARS_RIGHT_72DEG] = prefix + "Right/72Deg/Actuator/Value"; 00151 keys[LED_EARS_RIGHT_108DEG] = prefix + "Right/108Deg/Actuator/Value"; 00152 keys[LED_EARS_RIGHT_144DEG] = prefix + "Right/144Deg/Actuator/Value"; 00153 keys[LED_EARS_RIGHT_180DEG] = prefix + "Right/180Deg/Actuator/Value"; 00154 keys[LED_EARS_RIGHT_216DEG] = prefix + "Right/216Deg/Actuator/Value"; 00155 keys[LED_EARS_RIGHT_252DEG] = prefix + "Right/252Deg/Actuator/Value"; 00156 keys[LED_EARS_RIGHT_288DEG] = prefix + "Right/288Deg/Actuator/Value"; 00157 keys[LED_EARS_RIGHT_324DEG] = prefix + "Right/324Deg/Actuator/Value"; 00158 00159 prefix = __subd_prefix + "Face/Led/"; 00160 keys[LED_FACE_LEFT_RED_0DEG] = prefix + "Red/Left/0Deg/Actuator/Value"; 00161 keys[LED_FACE_LEFT_RED_45DEG] = prefix + "Red/Left/45Deg/Actuator/Value"; 00162 keys[LED_FACE_LEFT_RED_90DEG] = prefix + "Red/Left/90Deg/Actuator/Value"; 00163 keys[LED_FACE_LEFT_RED_135DEG] = prefix + "Red/Left/135Deg/Actuator/Value"; 00164 keys[LED_FACE_LEFT_RED_180DEG] = prefix + "Red/Left/180Deg/Actuator/Value"; 00165 keys[LED_FACE_LEFT_RED_225DEG] = prefix + "Red/Left/225Deg/Actuator/Value"; 00166 keys[LED_FACE_LEFT_RED_270DEG] = prefix + "Red/Left/270Deg/Actuator/Value"; 00167 keys[LED_FACE_LEFT_RED_315DEG] = prefix + "Red/Left/315Deg/Actuator/Value"; 00168 00169 keys[LED_FACE_LEFT_GREEN_0DEG] = prefix + "Green/Left/0Deg/Actuator/Value"; 00170 keys[LED_FACE_LEFT_GREEN_45DEG] = prefix + "Green/Left/45Deg/Actuator/Value"; 00171 keys[LED_FACE_LEFT_GREEN_90DEG] = prefix + "Green/Left/90Deg/Actuator/Value"; 00172 keys[LED_FACE_LEFT_GREEN_135DEG] = prefix + "Green/Left/135Deg/Actuator/Value"; 00173 keys[LED_FACE_LEFT_GREEN_180DEG] = prefix + "Green/Left/180Deg/Actuator/Value"; 00174 keys[LED_FACE_LEFT_GREEN_225DEG] = prefix + "Green/Left/225Deg/Actuator/Value"; 00175 keys[LED_FACE_LEFT_GREEN_270DEG] = prefix + "Green/Left/270Deg/Actuator/Value"; 00176 keys[LED_FACE_LEFT_GREEN_315DEG] = prefix + "Green/Left/315Deg/Actuator/Value"; 00177 00178 keys[LED_FACE_LEFT_BLUE_0DEG] = prefix + "Blue/Left/0Deg/Actuator/Value"; 00179 keys[LED_FACE_LEFT_BLUE_45DEG] = prefix + "Blue/Left/45Deg/Actuator/Value"; 00180 keys[LED_FACE_LEFT_BLUE_90DEG] = prefix + "Blue/Left/90Deg/Actuator/Value"; 00181 keys[LED_FACE_LEFT_BLUE_135DEG] = prefix + "Blue/Left/135Deg/Actuator/Value"; 00182 keys[LED_FACE_LEFT_BLUE_180DEG] = prefix + "Blue/Left/180Deg/Actuator/Value"; 00183 keys[LED_FACE_LEFT_BLUE_225DEG] = prefix + "Blue/Left/225Deg/Actuator/Value"; 00184 keys[LED_FACE_LEFT_BLUE_270DEG] = prefix + "Blue/Left/270Deg/Actuator/Value"; 00185 keys[LED_FACE_LEFT_BLUE_315DEG] = prefix + "Blue/Left/315Deg/Actuator/Value"; 00186 00187 00188 keys[LED_FACE_RIGHT_RED_0DEG] = prefix + "Red/Right/0Deg/Actuator/Value"; 00189 keys[LED_FACE_RIGHT_RED_45DEG] = prefix + "Red/Right/45Deg/Actuator/Value"; 00190 keys[LED_FACE_RIGHT_RED_90DEG] = prefix + "Red/Right/90Deg/Actuator/Value"; 00191 keys[LED_FACE_RIGHT_RED_135DEG] = prefix + "Red/Right/135Deg/Actuator/Value"; 00192 keys[LED_FACE_RIGHT_RED_180DEG] = prefix + "Red/Right/180Deg/Actuator/Value"; 00193 keys[LED_FACE_RIGHT_RED_225DEG] = prefix + "Red/Right/225Deg/Actuator/Value"; 00194 keys[LED_FACE_RIGHT_RED_270DEG] = prefix + "Red/Right/270Deg/Actuator/Value"; 00195 keys[LED_FACE_RIGHT_RED_315DEG] = prefix + "Red/Right/315Deg/Actuator/Value"; 00196 00197 keys[LED_FACE_RIGHT_GREEN_0DEG] = prefix + "Green/Right/0Deg/Actuator/Value"; 00198 keys[LED_FACE_RIGHT_GREEN_45DEG] = prefix + "Green/Right/45Deg/Actuator/Value"; 00199 keys[LED_FACE_RIGHT_GREEN_90DEG] = prefix + "Green/Right/90Deg/Actuator/Value"; 00200 keys[LED_FACE_RIGHT_GREEN_135DEG] = prefix + "Green/Right/135Deg/Actuator/Value"; 00201 keys[LED_FACE_RIGHT_GREEN_180DEG] = prefix + "Green/Right/180Deg/Actuator/Value"; 00202 keys[LED_FACE_RIGHT_GREEN_225DEG] = prefix + "Green/Right/225Deg/Actuator/Value"; 00203 keys[LED_FACE_RIGHT_GREEN_270DEG] = prefix + "Green/Right/270Deg/Actuator/Value"; 00204 keys[LED_FACE_RIGHT_GREEN_315DEG] = prefix + "Green/Right/315Deg/Actuator/Value"; 00205 00206 keys[LED_FACE_RIGHT_BLUE_0DEG] = prefix + "Blue/Right/0Deg/Actuator/Value"; 00207 keys[LED_FACE_RIGHT_BLUE_45DEG] = prefix + "Blue/Right/45Deg/Actuator/Value"; 00208 keys[LED_FACE_RIGHT_BLUE_90DEG] = prefix + "Blue/Right/90Deg/Actuator/Value"; 00209 keys[LED_FACE_RIGHT_BLUE_135DEG] = prefix + "Blue/Right/135Deg/Actuator/Value"; 00210 keys[LED_FACE_RIGHT_BLUE_180DEG] = prefix + "Blue/Right/180Deg/Actuator/Value"; 00211 keys[LED_FACE_RIGHT_BLUE_225DEG] = prefix + "Blue/Right/225Deg/Actuator/Value"; 00212 keys[LED_FACE_RIGHT_BLUE_270DEG] = prefix + "Blue/Right/270Deg/Actuator/Value"; 00213 keys[LED_FACE_RIGHT_BLUE_315DEG] = prefix + "Blue/Right/315Deg/Actuator/Value"; 00214 00215 00216 prefix = __subd_prefix; 00217 keys[LED_LFOOT_RED] = prefix + "LFoot/Led/Red/Actuator/Value"; 00218 keys[LED_LFOOT_GREEN] = prefix + "LFoot/Led/Green/Actuator/Value"; 00219 keys[LED_LFOOT_BLUE] = prefix + "LFoot/Led/Blue/Actuator/Value"; 00220 00221 keys[LED_RFOOT_RED] = prefix + "RFoot/Led/Red/Actuator/Value"; 00222 keys[LED_RFOOT_GREEN] = prefix + "RFoot/Led/Green/Actuator/Value"; 00223 keys[LED_RFOOT_BLUE] = prefix + "RFoot/Led/Blue/Actuator/Value"; 00224 00225 __memfa.reset(new AL::ALMemoryFastAccess()); 00226 try { 00227 __memfa->ConnectToVariables(naoqi_broker, keys, false); 00228 } catch (AL::ALError &e) { 00229 throw Exception("Failed to setup fast memory access: %s", 00230 e.toString().c_str()); 00231 } 00232 00233 NaoJointPositionInterface *joint_pos_if = 00234 blackboard->open_for_reading<NaoJointPositionInterface>("Nao Joint Positions"); 00235 if (! joint_pos_if->has_writer()) { 00236 blackboard->close(joint_pos_if); 00237 throw Exception("Joint Position interface has no writer"); 00238 } 00239 joint_pos_if->read(); 00240 bool skip_head_leds = 00241 (joint_pos_if->robot_type() != NaoJointPositionInterface::ROBOTYPE_ACADEMIC); 00242 blackboard->close(joint_pos_if); 00243 00244 std::vector<std::string>::iterator l; 00245 for (l = leddevs.begin(); l != leddevs.end(); ++l) { 00246 PathParser pp(*l); 00247 std::string loc = pp[subdpp.size()]; 00248 00249 if (! __cfg_verbose_face) { 00250 PathParser locpp(loc); 00251 if (locpp[0] == "Face") continue; 00252 } 00253 if (skip_head_leds) { 00254 PathParser locpp(loc); 00255 if (locpp[0] == "Head") continue; 00256 } 00257 00258 std::string id = "Nao LED " + loc; 00259 PathParser::size_type i; 00260 for (i = subdpp.size() + 2; (i < pp.size()) && (pp[i] != "Actuator"); ++i) { 00261 id += "/"; 00262 id += pp[i]; 00263 } 00264 00265 try { 00266 LedInterface *iface = 00267 blackboard->open_for_writing<LedInterface>(id.c_str()); 00268 __leds.insert(make_pair(iface, *l + "/Value")); 00269 } catch (Exception &e) { 00270 fawkes::LedInterface *last = NULL; 00271 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) { 00272 if (i->first != last) { 00273 blackboard->close(i->first); 00274 last = i->first; 00275 } 00276 } 00277 __leds.clear(); 00278 throw; 00279 } 00280 } 00281 00282 try { 00283 std::string left_right[2] = { "Left", "Right" }; 00284 std::string rgb[3] = { "Red", "Green", "Blue" }; 00285 std::string angles[8] = {"0", "45", "90", "135", "180", "225", "270", "315"}; 00286 00287 for (unsigned int lr = 0; lr < 2; ++lr) { 00288 for (unsigned int cl = 0; cl < 3; ++cl) { 00289 std::string id = "Nao LED Face/" + rgb[cl] + "/" + left_right[lr]; 00290 LedInterface *iface = 00291 blackboard->open_for_writing<LedInterface>(id.c_str()); 00292 00293 for (unsigned int a = 0; a < 8; ++a) { 00294 std::string entry = "Face/Led/" + rgb[cl] + "/" + left_right[lr]; 00295 std::string memid = 00296 __subd_prefix + entry + "/" + angles[a] + "Deg/Actuator/Value"; 00297 00298 __leds.insert(make_pair(iface, memid)); 00299 } 00300 } 00301 } 00302 00303 } catch (Exception &e) { 00304 fawkes::LedInterface *last = NULL; 00305 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) { 00306 if (i->first != last) { 00307 blackboard->close(i->first); 00308 last = i->first; 00309 } 00310 } 00311 __leds.clear(); 00312 throw; 00313 } 00314 00315 //logger->log_debug(name(), "Interfaces and device IDs:"); 00316 fawkes::LedInterface *last = NULL; 00317 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) { 00318 if (i->first == last) continue; 00319 00320 //logger->log_debug(name(), " %s", i->first->id()); 00321 std::pair<LedMap::iterator, LedMap::iterator> ret = 00322 __leds.equal_range(i->first); 00323 00324 for (LedMap::iterator j = ret.first; j != ret.second; ++j) { 00325 //logger->log_debug(name(), " %s", j->second.c_str()); 00326 00327 for (unsigned int k = 0; k < keys.size(); ++k) { 00328 if (keys[k] == j->second) { 00329 __memids.insert(std::make_pair(i->first, k)); 00330 break; 00331 } 00332 } 00333 00334 last = i->first; 00335 } 00336 } 00337 00338 last = NULL; 00339 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) { 00340 if (i->first != last) { 00341 bbil_add_message_interface(i->first); 00342 last = i->first; 00343 } 00344 } 00345 blackboard->register_listener(this, BlackBoard::BBIL_FLAG_MESSAGES); 00346 } 00347 00348 void 00349 NaoQiLedThread::finalize() 00350 { 00351 blackboard->unregister_listener(this); 00352 00353 fawkes::LedInterface *last = NULL; 00354 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) { 00355 if (i->first != last) { 00356 blackboard->close(i->first); 00357 last = i->first; 00358 } 00359 } 00360 00361 __dcm.reset(); 00362 __almem.reset(); 00363 __memfa.reset(); 00364 } 00365 00366 void 00367 NaoQiLedThread::loop() 00368 { 00369 __memfa->GetValues(__values); 00370 00371 fawkes::LedInterface *last = NULL; 00372 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) { 00373 if (i->first == last) continue; 00374 00375 float maxval = 0.; 00376 00377 std::pair<LedMemMap::iterator, LedMemMap::iterator> ret = 00378 __memids.equal_range(i->first); 00379 for (LedMemMap::iterator j = ret.first; j != ret.second; ++j) { 00380 if (__values[j->second] > maxval) maxval = __values[j->second]; 00381 } 00382 00383 if (maxval != i->first->intensity()) { 00384 i->first->set_intensity(maxval); 00385 i->first->write(); 00386 } 00387 00388 last = i->first; 00389 } 00390 } 00391 00392 00393 bool 00394 NaoQiLedThread::bb_interface_message_received(Interface *interface, 00395 Message *message) throw() 00396 { 00397 // some string magic to find the correct ALValue to write to 00398 std::string kind = "Merge"; 00399 int dcm_time = __dcm->getTime(0); 00400 00401 LedInterface::SetIntensityMessage *sim = 00402 dynamic_cast<LedInterface::SetIntensityMessage *>(message); 00403 00404 LedInterface *led_if = dynamic_cast<LedInterface *>(interface); 00405 if (led_if == NULL) return false; 00406 00407 std::pair<LedMap::iterator, LedMap::iterator> ret = 00408 __leds.equal_range(led_if); 00409 00410 if (sim != NULL) { 00411 for (LedMap::iterator i = ret.first; i != ret.second; ++i) { 00412 printf("Set %s to %f\n", i->second.c_str(), sim->intensity()); 00413 dcm::set_value(__dcm, i->second, kind, sim->intensity(), 00414 (int)roundf(dcm_time + sim->time_sec() * 1000.)); 00415 } 00416 } else if (dynamic_cast<LedInterface::TurnOnMessage *>(message) != NULL) { 00417 for (LedMap::iterator i = ret.first; i != ret.second; ++i) { 00418 dcm::set_value(__dcm, i->second, kind, 1., dcm_time); 00419 } 00420 } else if (dynamic_cast<LedInterface::TurnOffMessage *>(message) != NULL) { 00421 for (LedMap::iterator i = ret.first; i != ret.second; ++i) { 00422 dcm::set_value(__dcm, i->second, kind, 0., dcm_time); 00423 } 00424 } 00425 00426 return false; 00427 }