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 "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 {
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,SensorSourceID::UpdatedSID);
00069 else
00070 erouter->removeListener(this);
00071 }
00072 if(ans==this)
00073 refresh();
00074 return ans;
00075 }
00076
00077 void SensorObserverControl::refresh() {
00078
00079 checkLogFile();
00080
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);
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->removeListener(this);
00195 ControlBase::pause();
00196 }
00197 void SensorObserverControl::RTViewControl::deactivate() {
00198 erouter->removeListener(this);
00199 ControlBase::deactivate();
00200 }
00201
00202 void SensorObserverControl::RTViewControl::setPeriod(unsigned int x) {
00203 period=x;
00204 }
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
|