00001 #include "WalkCalibration.h"
00002 #include "FileInputControl.h"
00003 #include "StringInputControl.h"
00004 #include "NullControl.h"
00005 #include "Wireless/Wireless.h"
00006 #include "Behaviors/Controller.h"
00007 #include "Shared/WorldState.h"
00008 #include "Shared/string_util.h"
00009 #include "Sound/SoundManager.h"
00010 #include <stdlib.h>
00011 #include <fstream>
00012
00013 using namespace std;
00014
00015 char * WalkCalibration::datanames[WalkCalibration::NUM_SRC] = { "fs","fr","sr","br","bs","rr" };
00016
00017
00018 WalkCalibration::WalkCalibration()
00019 : ControlBase("Interactive Calibration","Leads you through the process of calibrating the current walk"),
00020 st(ROOT), curType(NUM_SRC),
00021 old_x(0), old_y(0), old_a(0), startTime(0), stopTime(0),
00022 help(NULL), load(NULL), save(NULL), measure(NULL), clear(NULL), polar(NULL), rect(NULL), isPolar(true),
00023 lastLoad(), firstIn(0), secondIn(0), status()
00024 {
00025 help=new ControlBase("Help");
00026 help->pushSlot(new NullControl("This control will"));
00027 help->pushSlot(new NullControl("lead you through the"));
00028 help->pushSlot(new NullControl("process of taking"));
00029 help->pushSlot(new NullControl("measurements."));
00030 help->pushSlot(new NullControl(""));
00031 help->pushSlot(new NullControl("In order to take"));
00032 help->pushSlot(new NullControl("accurate measurements,"));
00033 help->pushSlot(new NullControl("the types of motion"));
00034 help->pushSlot(new NullControl("have been broken"));
00035 help->pushSlot(new NullControl("down into 6"));
00036 help->pushSlot(new NullControl("categories:"));
00037 help->pushSlot(new NullControl(""));
00038 help->pushSlot(new NullControl("fs - forward/strafe"));
00039 help->pushSlot(new NullControl("fr - forward/rotate"));
00040 help->pushSlot(new NullControl("sr - strafe/rotate"));
00041 help->pushSlot(new NullControl("br - backwards/rotate"));
00042 help->pushSlot(new NullControl("bs - backwards/strafe"));
00043 help->pushSlot(new NullControl("rr - pure rotation"));
00044 help->pushSlot(new NullControl(""));
00045 help->pushSlot(new NullControl("In each of these"));
00046 help->pushSlot(new NullControl("categories, you will"));
00047 help->pushSlot(new NullControl("set the aibo to walk"));
00048 help->pushSlot(new NullControl("in a direction using"));
00049 help->pushSlot(new NullControl("mainly the two noted"));
00050 help->pushSlot(new NullControl("parameters, and then"));
00051 help->pushSlot(new NullControl("tweak the \"unused\""));
00052 help->pushSlot(new NullControl("parameter to cancel"));
00053 help->pushSlot(new NullControl("out any drift in"));
00054 help->pushSlot(new NullControl("that direction."));
00055 help->pushSlot(new NullControl(""));
00056 help->pushSlot(new NullControl("For example, if"));
00057 help->pushSlot(new NullControl("taking an 'fs'"));
00058 help->pushSlot(new NullControl("measurement, you"));
00059 help->pushSlot(new NullControl("would pick any"));
00060 help->pushSlot(new NullControl("forward and strafe"));
00061 help->pushSlot(new NullControl("values, and then"));
00062 help->pushSlot(new NullControl("fiddle with the"));
00063 help->pushSlot(new NullControl("rotational speed"));
00064 help->pushSlot(new NullControl("until it maintained"));
00065 help->pushSlot(new NullControl("a constant heading."));
00066 help->pushSlot(new NullControl(""));
00067 help->pushSlot(new NullControl("If measuring one of"));
00068 help->pushSlot(new NullControl("the rotational"));
00069 help->pushSlot(new NullControl("types, you will"));
00070 help->pushSlot(new NullControl("tweak the unused"));
00071 help->pushSlot(new NullControl("directional movement"));
00072 help->pushSlot(new NullControl("so the aibo is"));
00073 help->pushSlot(new NullControl("facing exactly 180"));
00074 help->pushSlot(new NullControl("degrees from its"));
00075 help->pushSlot(new NullControl("original heading"));
00076 help->pushSlot(new NullControl("whenever it is on"));
00077 help->pushSlot(new NullControl("the opposite side of"));
00078 help->pushSlot(new NullControl("the circle from"));
00079 help->pushSlot(new NullControl("where it started."));
00080 help->pushSlot(new NullControl(""));
00081 help->pushSlot(new NullControl("For pure rotations,"));
00082 help->pushSlot(new NullControl("tweak both strafe"));
00083 help->pushSlot(new NullControl("and forward so the"));
00084 help->pushSlot(new NullControl("rotation is centered"));
00085 help->pushSlot(new NullControl("around the middle of"));
00086 help->pushSlot(new NullControl("the body. Take"));
00087 help->pushSlot(new NullControl("displacement"));
00088 help->pushSlot(new NullControl("readings for other"));
00089 help->pushSlot(new NullControl("measurements"));
00090 help->pushSlot(new NullControl("relative to this"));
00091 help->pushSlot(new NullControl("point."));
00092 help->pushSlot(new NullControl(""));
00093 help->pushSlot(new NullControl("Enter angular"));
00094 help->pushSlot(new NullControl("measurements in"));
00095 help->pushSlot(new NullControl("degrees. Enter"));
00096 help->pushSlot(new NullControl("distances as "));
00097 help->pushSlot(new NullControl("centimeters."));
00098 help->pushSlot(new NullControl(""));
00099 help->pushSlot(new NullControl("You will almost"));
00100 help->pushSlot(new NullControl("always enter"));
00101 help->pushSlot(new NullControl("positive values for"));
00102 help->pushSlot(new NullControl("all measurements."));
00103 help->pushSlot(new NullControl("Enter negative"));
00104 help->pushSlot(new NullControl("values only when"));
00105 help->pushSlot(new NullControl("using cartesian"));
00106 help->pushSlot(new NullControl("measurements with a"));
00107 help->pushSlot(new NullControl("right strafe or"));
00108 help->pushSlot(new NullControl("backward motion, or"));
00109 help->pushSlot(new NullControl("when a rotation is"));
00110 help->pushSlot(new NullControl("opposite of the"));
00111 help->pushSlot(new NullControl("intended direction."));
00112 help->pushSlot(new NullControl(""));
00113 help->pushSlot(new NullControl("Remember that"));
00114 help->pushSlot(new NullControl("POSITIVE strafe is"));
00115 help->pushSlot(new NullControl("to the Aibo's LEFT."));
00116 help->pushSlot(new NullControl(""));
00117 help->pushSlot(new NullControl("Finally, when you"));
00118 help->pushSlot(new NullControl("have taken at least"));
00119 help->pushSlot(new NullControl("6 (I suggest at"));
00120 help->pushSlot(new NullControl("least 16)"));
00121 help->pushSlot(new NullControl("measurements of each"));
00122 help->pushSlot(new NullControl("type, you can save"));
00123 help->pushSlot(new NullControl("the data set."));
00124 help->pushSlot(new NullControl(""));
00125 help->pushSlot(new NullControl("You can also save"));
00126 help->pushSlot(new NullControl("intermediate sets"));
00127 help->pushSlot(new NullControl("and concatenate them"));
00128 help->pushSlot(new NullControl("later."));
00129 help->pushSlot(new NullControl(""));
00130 help->pushSlot(new NullControl("IMPORTANT: It is"));
00131 help->pushSlot(new NullControl("recommended to keep"));
00132 help->pushSlot(new NullControl("a telnet connection"));
00133 help->pushSlot(new NullControl("to port 59000 open"));
00134 help->pushSlot(new NullControl("so you can log data"));
00135 help->pushSlot(new NullControl("samples there, in"));
00136 help->pushSlot(new NullControl("case of an"));
00137 help->pushSlot(new NullControl("unexpected crash,"));
00138 help->pushSlot(new NullControl("freeze, or shutdown."));
00139 help->pushSlot(new NullControl("Each data sample is"));
00140 help->pushSlot(new NullControl("a line of 6 numbers."));
00141 help->pushSlot(new NullControl(""));
00142 help->pushSlot(new NullControl("You will then need"));
00143 help->pushSlot(new NullControl("to run the matlab"));
00144 help->pushSlot(new NullControl("function in"));
00145 help->pushSlot(new NullControl("tools/WalkCalibration.m"));
00146 help->pushSlot(new NullControl("(Linear Least"));
00147 help->pushSlot(new NullControl("Squares) on these"));
00148 help->pushSlot(new NullControl("files to get the"));
00149 help->pushSlot(new NullControl("calibration matrix."));
00150 help->pushSlot(new NullControl(""));
00151 help->pushSlot(new NullControl("Copy this matrix"));
00152 help->pushSlot(new NullControl("back on to the"));
00153 help->pushSlot(new NullControl("memory stick and use"));
00154 help->pushSlot(new NullControl("the \"Load"));
00155 help->pushSlot(new NullControl("Calibration\""));
00156 help->pushSlot(new NullControl("control to apply it"));
00157 help->pushSlot(new NullControl("to the current walk."));
00158 help->pushSlot(new NullControl(""));
00159 help->pushSlot(new NullControl("You will then need"));
00160 help->pushSlot(new NullControl("to save the walk"));
00161 help->pushSlot(new NullControl("itself to retain the"));
00162 help->pushSlot(new NullControl("calibration for the"));
00163 help->pushSlot(new NullControl("next reboot."));
00164
00165
00166 load=new FileInputControl("Load Data Set","Load data files - select any file from the set, all will be loaded","/");
00167 save=new StringInputControl("Save Data Set","Saves current data","Enter the base name (up to 6 char)");
00168 measure=new NullControl("Take Measurements","Begins the data gathering process");
00169 measure->pushSlot(new NullControl());
00170 clear=new NullControl("Clear Data","Clear the current data and start over");
00171 clear->pushSlot(new NullControl());
00172 for(unsigned int i=0; i<NUM_SRC; i++)
00173 cnts[i]=0;
00174 setupRoot();
00175 }
00176
00177
00178 WalkCalibration::~WalkCalibration() {
00179 options.clear();
00180 delete help;
00181 delete load;
00182 delete save;
00183 delete measure;
00184 delete clear;
00185 }
00186
00187 ControlBase * WalkCalibration::activate(MotionManager::MC_ID disp_id, Socket * gui) {
00188 erouter->addListener(this,EventBase::locomotionEGID);
00189 return ControlBase::activate(disp_id, gui);
00190 }
00191
00192
00193 void WalkCalibration::refresh() {
00194 if(load->getLastInput()!=lastLoad) {
00195 for(int i=0; i<NUM_SRC; i++) {
00196 unsigned int strlen=load->getLastInput().size();
00197 loadData(load->getLastInput().substr(0,strlen-6)+datanames[i]+std::string(".txt"),data[i]);
00198 cnts[i]=data[i].size();
00199 }
00200 lastLoad=load->getLastInput();
00201 }
00202 if(save->getLastInput().size()>0) {
00203 for(int i=0; i<NUM_SRC; i++)
00204 saveData(save->getLastInput()+datanames[i]+std::string(".txt"),data[i]);
00205 save->takeInput("");
00206 }
00207 if(st==READY && (curType==fs || curType==bs))
00208 isPolar=polar->getStatus();
00209 ControlBase::refresh();
00210 if(gui_comm!=NULL && wireless->isConnected(gui_comm->sock)) {
00211 if(status.size()==0)
00212 gui_comm->printf("status\n1\n# Samples: fs=%d fr=%d sr=%d br=%d bs=%d r=%d\n",cnts[0], cnts[1], cnts[2], cnts[3], cnts[4], cnts[5]);
00213 else
00214 gui_comm->printf("status\n%td\n%s\n",std::count(status.begin(),status.end(),'\n'),status.c_str());
00215 }
00216 }
00217
00218 ControlBase* WalkCalibration::doSelect() {
00219 if(st==ROOT) {
00220 if(hilights.size()==0)
00221 return this;
00222 if(options[hilights.front()]==measure) {
00223 sndman->playFile(config->controller.select_snd);
00224 setupChoose();
00225 refresh();
00226 return this;
00227 } else if(options[hilights.front()]==clear) {
00228 sndman->playFile(config->controller.select_snd);
00229 setupClear();
00230 refresh();
00231 return this;
00232 } else {
00233 return ControlBase::doSelect();
00234 }
00235 } else if(st==CLEAR) {
00236 if(hilights.size()==0)
00237 return this;
00238 sndman->playFile(config->controller.select_snd);
00239 if(hilights.front()!=0) {
00240 sout->printf("Clearing data...\n");
00241 sndman->playFile(config->controller.cancel_snd);
00242 for(int i=0; i<NUM_SRC; i++) {
00243 clearData(data[i]);
00244 cnts[i]=0;
00245 }
00246 }
00247 setupRoot();
00248 refresh();
00249 return this;
00250 } else if(st==CHOOSE) {
00251 if(hilights.size()==0)
00252 return this;
00253 if(hilights.front()-2<NUM_SRC) {
00254 curType=static_cast<dataSource>(hilights.front()-2);
00255 sndman->playFile(config->controller.select_snd);
00256 setupReady();
00257 refresh();
00258 return this;
00259 }
00260 sndman->playFile(config->controller.cancel_snd);
00261 setupRoot();
00262 refresh();
00263 return this;
00264 } else if(st==READY) {
00265 if(hilights.size()==0 || options[hilights.front()]->getName()=="Go!") {
00266 sndman->playFile(config->controller.select_snd);
00267 setupMoving();
00268 refresh();
00269 return this;
00270 }
00271 if(options[hilights.front()]==polar || options[hilights.front()]==rect) {
00272 return ControlBase::doSelect();
00273 }
00274 sndman->playFile(config->controller.cancel_snd);
00275 setupChoose();
00276 refresh();
00277 return this;
00278 } else {
00279 sndman->playFile(config->controller.cancel_snd);
00280 setupChoose();
00281 refresh();
00282 return this;
00283 }
00284 }
00285
00286 void WalkCalibration::processEvent(const EventBase& e) {
00287 if(st==MOVING) {
00288 stopTime=e.getTimeStamp();
00289 sout->printf("Ran for %g seconds\n",(stopTime-startTime)/1000.f);
00290 sndman->playFile(config->controller.select_snd);
00291 setupReading1();
00292 refresh();
00293 if(curType!=5)
00294 doReadStdIn(std::string("Enter ")+getFirstMeasure(curType));
00295 else
00296 doReadStdIn(std::string("Enter ")+getSecondMeasure(curType));
00297 }
00298 }
00299
00300
00301 ControlBase * WalkCalibration::takeInput(const std::string& msg) {
00302 if(st!=READING_1 && st!=READING_2)
00303 return ControlBase::takeInput(msg);
00304 else {
00305 char * end;
00306 const char * str=msg.c_str();
00307 if(st==READING_1) {
00308 firstIn=strtod(str,&end);
00309 if(end==str) {
00310 err("Invalid input: "+msg);
00311 return doReadStdIn(std::string("Enter ")+