Fawkes API  Fawkes Development Version
led_thread.cpp
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 }