SensorObserverControl.ccGo 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 "SoundPlay/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), numListeners(0)
00014 {
00015 const unsigned int TMP_LEN=64;
00016 char tmp[TMP_LEN];
00017 pushSlot(consoleCtl=new ToggleControl("Console Output","If selected, outputs events to the console"));
00018 pushSlot(fileCtl=new StringInputControl("[ ] File Output","Please enter the filename to log to (in /ms/...)"));
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 snprintf(tmp,TMP_LEN,"%d",i);
00030 sensorCtl->pushSlot(new ToggleControl(tmp,"Turns logging of this sensor on/off"));
00031 }
00032 pushSlot(buttonCtl=new ControlBase("Buttons:","Toggles logging of various buttons"));
00033 for(unsigned int i=0; i<NumButtons; i++) {
00034 snprintf(tmp,TMP_LEN,"%d",i);
00035 buttonCtl->pushSlot(new ToggleControl(tmp,"Turns logging of this button on/off"));
00036 }
00037 pushSlot(outputCtl=new ControlBase("Outputs:","Toggles logging of various outputs' positions"));
00038 for(unsigned int i=0; i<NumOutputs; i++)
00039 outputCtl->pushSlot(new ToggleControl(outputNames[i],"Turns logging of this output's values on/off"));
00040 pushSlot(dutyCtl=new ControlBase("Duties:","Toggles logging of various PID joint's duty cycles"));
00041 for(unsigned int i=0; i<NumPIDJoints; i++)
00042 dutyCtl->pushSlot(new ToggleControl(outputNames[i+PIDJointOffset],"Turns logging of how hard this output is working on/off"));
00043 }
00044
00045 ControlBase* SensorObserverControl::doSelect() {
00046 ControlBase* ans=this;
00047 bool wasListening=(numListeners>0);
00048 for(unsigned int i=0; i<hilights.size(); i++) {
00049 unsigned int cur=hilights[i];
00050 if(options[cur]==fileCtl) {
00051 if(options[cur]->getName()[1]!=' ') {
00052 logfile.close();
00053 options[cur]->setName("[ ] File Output");
00054 numListeners--;
00055 } else {
00056 ans=options[cur];
00057 numListeners++;
00058 }
00059 } else if(options[cur]==consoleCtl) {
00060 options[cur]->doSelect();
00061 if(consoleCtl->getStatus())
00062 numListeners--;
00063 else
00064 numListeners++;
00065 } else {
00066 ans=options[cur];
00067 }
00068 }
00069 sndman->PlayFile(config->controller.select_snd);
00070 if(wasListening!=(numListeners>0)) {
00071 if(numListeners>0)
00072 erouter->addListener(this,EventBase::sensorEGID,0);
00073 else
00074 erouter->removeListener(this);
00075 }
00076 if(ans==this)
00077 refresh();
00078 return ans;
00079 }
00080
00081 void SensorObserverControl::refresh() {
00082 checkLogFile();
00083 ControlBase::refresh();
00084 }
00085
00086
00087 void SensorObserverControl::processEvent(const EventBase& ) {
00088 std::ostringstream logdata;
00089 for(unsigned int i=0; i<NumSensors; i++)
00090 if(ToggleControl * tgl=dynamic_cast<ToggleControl*>(sensorCtl->getSlots()[i]))
00091 if(tgl->getStatus())
00092 logdata << state->lastSensorUpdateTime << "\tSENSOR:\t" << i << '\t' << state->sensors[i] << '\n';
00093 for(unsigned int i=0; i<NumButtons; i++)
00094 if(ToggleControl * tgl=dynamic_cast<ToggleControl*>(buttonCtl->getSlots()[i]))
00095 if(tgl->getStatus())
00096 logdata << state->lastSensorUpdateTime << "\tBUTTON:\t" << i << '\t' << state->buttons[i] << '\n';
00097 for(unsigned int i=0; i<NumOutputs; i++)
00098 if(ToggleControl * tgl=dynamic_cast<ToggleControl*>(outputCtl->getSlots()[i]))
00099 if(tgl->getStatus())
00100 logdata << state->lastSensorUpdateTime << "\tOUTPUT:\t" << i << '\t' << state->outputs[i] << '\n';
00101 for(unsigned int i=0; i<NumPIDJoints; i++)
00102 if(ToggleControl * tgl=dynamic_cast<ToggleControl*>(dutyCtl->getSlots()[i]))
00103 if(tgl->getStatus())
00104 logdata << state->lastSensorUpdateTime << "\tDUTY:\t" << i << '\t' << state->pidduties[i] << '\n';
00105 if(consoleCtl->getStatus())
00106 sout->printf("%s",logdata.str().c_str());
00107 checkLogFile();
00108 if(logfile)
00109 logfile << logdata.str() << std::flush;
00110 }
00111
00112 void SensorObserverControl::checkLogFile() {
00113 if(fileCtl->getLastInput()!=logfilePath) {
00114 logfile.close();
00115 logfilePath=fileCtl->getLastInput();
00116 logfile.clear();
00117 if(logfilePath.size()!=0) {
00118 sout->printf("Opening `/ms/%s'\n",logfilePath.c_str());
00119 logfile.open(("/ms/"+logfilePath).c_str());
00120 if(!logfile.fail()) {
00121 std::string tmp=fileCtl->getName();
00122 tmp[1]='X';
00123 fileCtl->setName(tmp+": "+logfilePath);
00124 } else {
00125 serr->printf("Opening `/ms/%s' failed\n",logfilePath.c_str());
00126 }
00127 }
00128 }
00129 }
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
|