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