Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

SensorObserverControl.cc

Go to the documentation of this file.
00001 #include "SensorObserverControl.h"
00002 #include "Events/EventRouter.h"
00003 #include "Motion/MMAccessor.h"
00004 #include "Motion/LedMC.h"
00005 #include <sstream>
00006 #include "Events/LocomotionEvent.h"
00007 #include "Events/TextMsgEvent.h"
00008 #include "Events/VisionObjectEvent.h"
00009 #include "Shared/WorldState.h"
00010 #include "Sound/SoundManager.h"
00011 
00012 SensorObserverControl::SensorObserverControl()
00013   : ControlBase("Sensor Observer","Allows you to see/log the sensor data"), logfilePath(), logfile(), helpCtl(NULL), sensorCtl(NULL), buttonCtl(NULL), outputCtl(NULL), dutyCtl(NULL), consoleCtl(NULL), fileCtl(NULL), rtCtl(NULL), rtFreqCtl(NULL), numListeners(0)
00014 {
00015   pushSlot(consoleCtl=new ToggleControl("Console Output","If selected, outputs events to the console"));
00016   pushSlot(fileCtl=new StringInputControl("[ ] File Output","Please enter the filename to log to (in /ms/...)"));
00017   pushSlot(rtCtl=new RTViewControl(this));
00018   pushSlot(rtFreqCtl=new StringInputControl("Real-time Update Period","Please enter the time between refreshes, in milliseconds; '0' to require manual refresh"));
00019   pushSlot(helpCtl=new ControlBase("Help"));
00020   pushSlot(NULL);
00021   helpCtl->pushSlot(new NullControl("The indexes listed here"));
00022   helpCtl->pushSlot(new NullControl("correspond to offsets"));
00023   helpCtl->pushSlot(new NullControl("given in the __Info.h"));
00024   helpCtl->pushSlot(new NullControl("file for the model"));
00025   helpCtl->pushSlot(new NullControl("robot which you are "));
00026   helpCtl->pushSlot(new NullControl("currently using."));
00027   pushSlot(sensorCtl=new ControlBase("Sensors:","Toggles logging of various sensors"));
00028   for(unsigned int i=0; i<NumSensors; i++)
00029     sensorCtl->pushSlot(new ToggleControl(sensorNames[i],"Turns logging of this sensor on/off"));
00030   pushSlot(buttonCtl=new ControlBase("Buttons:","Toggles logging of various buttons"));
00031   for(unsigned int i=0; i<NumButtons; i++)
00032     buttonCtl->pushSlot(new ToggleControl(buttonNames[i],"Turns logging of this button on/off"));
00033   pushSlot(outputCtl=new ControlBase("Outputs:","Toggles logging of various outputs' positions"));
00034   for(unsigned int i=0; i<NumOutputs; i++)
00035     outputCtl->pushSlot(new ToggleControl(outputNames[i],"Turns logging of this output's values on/off"));
00036   pushSlot(dutyCtl=new ControlBase("Duties:","Toggles logging of various PID joint's duty cycles"));
00037   for(unsigned int i=0; i<NumPIDJoints; i++)
00038     dutyCtl->pushSlot(new ToggleControl(outputNames[i+PIDJointOffset],"Turns logging of how hard this output is working on/off"));
00039 }
00040 
00041 ControlBase* SensorObserverControl::doSelect() {
00042   ControlBase* ans=this;
00043   bool wasListening=(numListeners>0);
00044   for(unsigned int i=0; i<hilights.size(); i++) {
00045     unsigned int cur=hilights[i];
00046     if(options[cur]==fileCtl) {
00047       if(options[cur]->getName()[1]!=' ') {
00048         logfile.close();
00049         options[cur]->setName("[ ] File Output");
00050         numListeners--;
00051       } else {
00052         ans=options[cur];
00053         numListeners++;
00054       }
00055     } else if(options[cur]==consoleCtl) {
00056       options[cur]->doSelect();
00057       if(consoleCtl->getStatus())
00058         numListeners--;
00059       else
00060         numListeners++;
00061     } else { //(options[cur]==helpCtl || options[cur]==sensorCtl || options[cur]==buttonCtl || options[cur]==outputCtl || options[cur]==dutyCtl || options[cur]==consoleCtl)
00062       ans=options[cur];
00063     }
00064   }
00065   sndman->playFile(config->controller.select_snd);
00066   if(wasListening!=(numListeners>0)) {
00067     if(numListeners>0)
00068       erouter->addListener(this,EventBase::sensorEGID,SensorSrcID::UpdatedSID);
00069     else
00070       erouter->removeListener(this);
00071   }
00072   if(ans==this)
00073     refresh();
00074   return ans;
00075 }
00076 
00077 void SensorObserverControl::refresh() {
00078   //check for change in log file
00079   checkLogFile();
00080   //check for change in real-time refresh period
00081   if(rtFreqCtl->getLastInput().size()>0) {
00082     unsigned int fr=atoi(rtFreqCtl->getLastInput().c_str());
00083     rtFreqCtl->clearLastInput();
00084     if(fr!=0 && rtCtl!=NULL)
00085       rtCtl->setPeriod(fr<100?100:fr); //limit to minimum period of 100ms
00086   }
00087   ControlBase::refresh();
00088 }
00089 
00090 void SensorObserverControl::processEvent(const EventBase& event) {
00091   if(event.getGeneratorID()==EventBase::sensorEGID) {
00092     std::ostringstream logdata;
00093     for(unsigned int i=0; i<NumSensors; i++)
00094       if(ToggleControl * tgl=dynamic_cast<ToggleControl*>(sensorCtl->getSlots()[i]))
00095         if(tgl->getStatus())
00096           logdata << state->lastSensorUpdateTime << "\tSENSOR:\t" << i << '\t' << state->sensors[i] << '\n';
00097     for(unsigned int i=0; i<NumButtons; i++)
00098       if(ToggleControl * tgl=dynamic_cast<ToggleControl*>(buttonCtl->getSlots()[i]))
00099         if(tgl->getStatus())
00100           logdata << state->lastSensorUpdateTime << "\tBUTTON:\t" << i << '\t' << state->buttons[i] << '\n';
00101     for(unsigned int i=0; i<NumOutputs; i++)
00102       if(ToggleControl * tgl=dynamic_cast<ToggleControl*>(outputCtl->getSlots()[i]))
00103         if(tgl->getStatus())
00104           logdata << state->lastSensorUpdateTime << "\tOUTPUT:\t" << i << '\t' << state->outputs[i] << '\n';
00105     for(unsigned int i=0; i<NumPIDJoints; i++)
00106       if(ToggleControl * tgl=dynamic_cast<ToggleControl*>(dutyCtl->getSlots()[i]))
00107         if(tgl->getStatus())
00108           logdata << state->lastSensorUpdateTime << "\tDUTY:\t" << i << '\t' << state->pidduties[i] << '\n';
00109     if(consoleCtl->getStatus())
00110       sout->printf("%s",logdata.str().c_str());
00111     checkLogFile();
00112     if(logfile)
00113       logfile << logdata.str() << std::flush;
00114   } else {
00115     serr->printf("WARNING: Unexpected event: %s\n",event.getName().c_str());
00116   }
00117 }
00118 
00119 void SensorObserverControl::checkLogFile() {
00120   if(fileCtl->getLastInput()!=logfilePath) {
00121     logfile.close();
00122     logfilePath=fileCtl->getLastInput();
00123     logfile.clear();
00124     if(logfilePath.size()!=0) {
00125       sout->printf("Opening `%s'\n",config->portPath(logfilePath).c_str());
00126       logfile.open(config->portPath(logfilePath).c_str());
00127       if(!logfile.fail()) {
00128         std::string tmp=fileCtl->getName();
00129         tmp[1]='X';
00130         fileCtl->setName(tmp+": "+logfilePath);
00131       } else {
00132         serr->printf("Opening `%s' failed\n",config->portPath(logfilePath).c_str());
00133       }
00134     }
00135   }
00136 }
00137 
00138 void SensorObserverControl::updateRT() {
00139   const unsigned int FTOS_LEN=50;
00140   char ftos[FTOS_LEN];
00141   rtCtl->clearSlots();
00142   bool first=true;
00143   for(unsigned int i=0; i<NumSensors; i++)
00144     if(ToggleControl * tgl=dynamic_cast<ToggleControl*>(sensorCtl->getSlots()[i]))
00145       if(tgl->getStatus()) {
00146         if(first)
00147           rtCtl->pushSlot(new NullControl("SENSORS:"));
00148         first=false;
00149         snprintf(ftos,FTOS_LEN,"%s: %g",sensorNames[i],state->sensors[i]);
00150         rtCtl->pushSlot(new NullControl(ftos));
00151       }
00152   first=true;
00153   for(unsigned int i=0; i<NumButtons; i++)
00154     if(ToggleControl * tgl=dynamic_cast<ToggleControl*>(buttonCtl->getSlots()[i]))
00155       if(tgl->getStatus()) {
00156         if(first)
00157           rtCtl->pushSlot(new NullControl("BUTTONS:"));
00158         first=false;
00159         snprintf(ftos,FTOS_LEN,"%s: %g",buttonNames[i],state->buttons[i]);
00160         rtCtl->pushSlot(new NullControl(ftos));
00161       }
00162   first=true;
00163   for(unsigned int i=0; i<NumOutputs; i++)
00164     if(ToggleControl * tgl=dynamic_cast<ToggleControl*>(outputCtl->getSlots()[i]))
00165       if(tgl->getStatus()) {
00166         if(first)
00167           rtCtl->pushSlot(new NullControl("OUTPUTS:"));
00168         first=false;
00169         snprintf(ftos,FTOS_LEN,"%s: %g",outputNames[i],state->outputs[i]);
00170         rtCtl->pushSlot(new NullControl(ftos));
00171       }
00172   first=true;
00173   for(unsigned int i=0; i<NumPIDJoints; i++)
00174     if(ToggleControl * tgl=dynamic_cast<ToggleControl*>(dutyCtl->getSlots()[i]))
00175       if(tgl->getStatus()) {
00176         if(first)
00177           rtCtl->pushSlot(new NullControl("DUTIES:"));
00178         first=false;
00179         snprintf(ftos,FTOS_LEN,"%s: %g",outputNames[i],state->pidduties[i]);
00180         rtCtl->pushSlot(new NullControl(ftos));
00181       }
00182   if(rtCtl->slotsSize()==0)
00183     rtCtl->pushSlot(new NullControl("No sensors selected","You need to select which sensors to view"));
00184 }
00185 
00186 void SensorObserverControl::RTViewControl::refresh() {
00187   if(parent!=NULL)
00188     parent->updateRT();
00189   if(period!=0)
00190     erouter->addTimer(this,0,period);
00191   ControlBase::refresh();
00192 }
00193 void SensorObserverControl::RTViewControl::pause() {
00194   erouter->removeTimer(this);
00195   ControlBase::pause();
00196 }
00197 void SensorObserverControl::RTViewControl::deactivate() {
00198   erouter->removeTimer(this);
00199   ControlBase::deactivate();
00200 }
00201 /*! The change doesn't get picked up until next call to refresh() */
00202 void SensorObserverControl::RTViewControl::setPeriod(unsigned int x) {
00203   period=x;
00204 }
00205 
00206 
00207 /*! @file
00208  * @brief Describes SensorObserverControl, which allows logging of sensor information to the console or file
00209  * @author ejt (Creator)
00210  *
00211  * $Author: ejt $
00212  * $Name: tekkotsu-4_0 $
00213  * $Revision: 1.8 $
00214  * $State: Exp $
00215  * $Date: 2007/06/05 18:04:36 $
00216  */

Tekkotsu v4.0
Generated Thu Nov 22 00:54:55 2007 by Doxygen 1.5.4