00001 #include "WaypointWalkControl.h"
00002 #include "ToggleControl.h"
00003 #include "NullControl.h"
00004 #include "FileInputControl.h"
00005 #include "StringInputControl.h"
00006 #include "ValueEditControl.h"
00007 #include "Motion/MotionManager.h"
00008 #include "Motion/WaypointWalkMC.h"
00009 #include "Motion/WalkMC.h"
00010 #include "Motion/MMAccessor.h"
00011 #include "Sound/SoundManager.h"
00012
00013 WaypointWalkControl::WaypointWalkControl()
00014 : ControlBase("WaypointWalkControl","Allows interactive control and execution of a set of waypoints"),
00015 isRunning(false), startstopCtl(NULL), loopCtl(NULL), addEgoWPCtl(NULL),
00016 addOffWPCtl(NULL), addAbsWPCtl(NULL), loadCtl(NULL), saveCtl(NULL),
00017 localizationCtl(NULL), listOffset(0), walk_id(MotionManager::invalid_MC_ID)
00018 {
00019 pushSlot(startstopCtl=new NullControl("Execute","Begin running waypoint list"));
00020 pushSlot(loopCtl=new ToggleControl("Loop Waypoints","When last waypoint is reached, start over"));
00021 pushSlot(addEgoWPCtl=new NullControl("Add Egocentric Waypoint","Appends a new egocentric waypoint (heading and location relative) at the end of the list"));
00022 pushSlot(addOffWPCtl=new NullControl("Add Offset Waypoint","Appends a new offset waypoint (location relative) at the end of the list"));
00023 pushSlot(addAbsWPCtl=new NullControl("Add Absolute Waypoint","Appends a new absolute waypoint at the end of the list"));
00024 pushSlot(loadCtl=new FileInputControl("Load Waypoints","Reads a path from a file",config->portPath(config->motion.root)));
00025 loadCtl->setFilter("*.wyp");
00026 pushSlot(saveCtl=new StringInputControl("Save Waypoints","Writes the current path to a file"));
00027 pushSlot(localizationCtl=new StringInputControl("Drift Error Correction","Enter 3 numbers 'x y a' reprenting current error"));
00028 pushSlot(NULL);
00029 listOffset=slotsSize();
00030 }
00031
00032 ControlBase *
00033 WaypointWalkControl::activate(MotionManager::MC_ID disp_id, Socket * gui) {
00034 if(walk_id==MotionManager::invalid_MC_ID) {
00035 SharedObject<WaypointWalkMC> walk;
00036 walk_id=motman->addPersistentMotion(walk);
00037 }
00038 return ControlBase::activate(disp_id,gui);
00039 }
00040
00041 void
00042 WaypointWalkControl::refresh() {
00043 if(saveCtl->getLastInput().size()>0) {
00044 std::string filename=saveCtl->getLastInput();
00045 if(filename.find(".")==std::string::npos)
00046 filename+=".wyp";
00047 std::string path=config->motion.makePath(filename);
00048 sout->printf("Attempting save to %s...\n",path.c_str());
00049 MMAccessor<WaypointWalkMC>(walk_id)->SaveWaypointFile(path.c_str());
00050 saveCtl->clearLastInput();
00051 }
00052 if(loadCtl->getLastInput().size()>0) {
00053 sout->printf("Attempting load from %s...\n",loadCtl->getLastInput().c_str());
00054 MMAccessor<WaypointWalkMC>(walk_id)->LoadWaypointFile(loadCtl->getLastInput().c_str());
00055 loadCtl->clearLastInput();
00056 }
00057 if(localizationCtl->getLastInput().size()>0) {
00058 float x=0,y=0,a=0;
00059 sscanf(localizationCtl->getLastInput().c_str(),"%g %g %g",&x,&y,&a);
00060 MMAccessor<WaypointWalkMC> walk(walk_id);
00061 walk->setCurPos(x+walk->getCurX(),y+walk->getCurY(),a+walk->getCurA());
00062 cout << "Position is now " << walk->getCurX() << ' ' << walk->getCurY() << ' ' << walk->getCurA() << endl;
00063 localizationCtl->clearLastInput();
00064 }
00065
00066 MMAccessor<WaypointWalkMC> walk(walk_id);
00067
00068 loopCtl->setStatus(walk->getIsLooping());
00069
00070
00071
00072
00073 for(unsigned int i=listOffset; i<slotsSize(); i++)
00074 delete options[i];
00075 options.resize(listOffset);
00076
00077
00078 WaypointWalkMC::WaypointList_t& wplist=walk->getWaypointList();
00079 unsigned int wpcnt=1;
00080 for(WaypointWalkMC::WaypointListIter_t it=wplist.begin(); it!=wplist.end(); it=wplist.next(it)) {
00081 char cname[50];
00082 sprintf(cname,"Waypoint %d",wpcnt++);
00083 char desc[100];
00084 const char * pt=NULL;
00085 switch(wplist[it].posType) {
00086 case WaypointWalkMC::Waypoint::POSTYPE_EGOCENTRIC:
00087 pt="^="; break;
00088 case WaypointWalkMC::Waypoint::POSTYPE_OFFSET:
00089 pt="+="; break;
00090 case WaypointWalkMC::Waypoint::POSTYPE_ABSOLUTE:
00091 pt="="; break;
00092 }
00093 sprintf(desc,"x%s%g, y%s%g, a%s%g, arc=%g", pt,wplist[it].x,pt,wplist[it].y,
00094 (wplist[it].angleIsRelative?"+=":"="),wplist[it].angle,wplist[it].arc);
00095 pushSlot(new WaypointEditControl(cname,desc,walk_id,it));
00096 }
00097 ControlBase::refresh();
00098 }
00099
00100 void
00101 WaypointWalkControl::deactivate() {
00102 motman->removeMotion(walk_id);
00103 walk_id=MotionManager::invalid_MC_ID;
00104 ControlBase::deactivate();
00105 }
00106
00107 ControlBase*
00108 WaypointWalkControl::doSelect() {
00109 for(unsigned int i=0; i<hilights.size(); i++) {
00110 ControlBase * curctl=options[hilights[i]];
00111 if(curctl==startstopCtl) {
00112 if(isRunning) {
00113 isRunning=false;
00114 startstopCtl->setName("Execute");
00115 startstopCtl->setDescription("Begin running waypoint list");
00116 MMAccessor<WaypointWalkMC>(walk_id)->pause();
00117 } else {
00118 isRunning=true;
00119 startstopCtl->setName("Stop");
00120 startstopCtl->setDescription("Halt locomotion");
00121 MMAccessor<WaypointWalkMC>(walk_id)->go();
00122 }
00123 sndman->PlayFile(config->controller.select_snd);
00124 return curctl;
00125 } else if(curctl==loopCtl) {
00126 MMAccessor<WaypointWalkMC>(walk_id)->setIsLooping(!loopCtl->getStatus());
00127 sndman->PlayFile(config->controller.select_snd);
00128 return curctl;
00129 } else if(curctl==addEgoWPCtl) {
00130 MMAccessor<WaypointWalkMC>(walk_id)->addEgocentricWaypoint(0,0,false,true,.1);
00131 sndman->PlayFile(config->controller.select_snd);
00132 return curctl;
00133 } else if(curctl==addOffWPCtl) {
00134 MMAccessor<WaypointWalkMC>(walk_id)->addOffsetWaypoint(0,0,false,true,.1);
00135 sndman->PlayFile(config->controller.select_snd);
00136 return curctl;
00137 } else if(curctl==addAbsWPCtl) {
00138 MMAccessor<WaypointWalkMC>(walk_id)->addAbsoluteWaypoint(0,0,false,true,.1);
00139 sndman->PlayFile(config->controller.select_snd);
00140 return curctl;
00141 }
00142 }
00143 return ControlBase::doSelect();
00144 }
00145
00146 WaypointWalkControl::WaypointEditControl::WaypointEditControl(const std::string& n, const std::string& d, MotionManager::MC_ID walkid, unsigned int waypointid)
00147 : ControlBase(n,d), walk_id(walkid), waypoint_id(waypointid), up(NULL), down(NULL), del(NULL), set(NULL)
00148 {
00149 pushSlot(up=new NullControl("Up (Move up)","Moves up in waypoint list"));
00150 pushSlot(down=new NullControl("Down (Move down)","Moves down in waypoint list"));
00151 pushSlot(del=new NullControl("Delete","Removes from waypoint list"));
00152 pushSlot(set=new NullControl("Set as current goal","Starts trying to reach this location"));
00153 pushSlot(NULL);
00154 MMAccessor<WaypointWalkMC> walk(walk_id);
00155 WaypointWalkMC::Waypoint& curway=walk->getWaypointList()[waypoint_id];
00156 pushSlot(new ValueEditControl<float>("X",&curway.x));
00157 pushSlot(new ValueEditControl<float>("Y",&curway.y));
00158 pushSlot(new ValueEditControl<float>("A",&curway.angle));
00159 pushSlot(new ValueEditControl<float>("Arc",&curway.arc));
00160 pushSlot(new ValueEditControl<float>("Speed (in m/s)",&curway.speed));
00161 pushSlot(new ValueEditControl<float>("Turn Speed (in rad/s)",&curway.turnSpeed));
00162 char desc[256];
00163 snprintf(desc,256,"Types: EGO=%d, OFF=%d, ABS=%d",WaypointWalkMC::Waypoint::POSTYPE_EGOCENTRIC,WaypointWalkMC::Waypoint::POSTYPE_OFFSET,WaypointWalkMC::Waypoint::POSTYPE_ABSOLUTE);
00164 pushSlot(new ValueEditControl<int>("Pos. coord. system",desc,(int*)&curway.posType));
00165 ToggleControl * tmptgl;
00166 pushSlot(tmptgl=new ToggleControl("Angle is relative"));
00167 tmptgl->setStatus(curway.angleIsRelative);
00168 tmptgl->setStore(&curway.angleIsRelative);
00169 pushSlot(tmptgl=new ToggleControl("Track path"));
00170 tmptgl->setStatus(curway.trackPath);
00171 tmptgl->setStore(&curway.trackPath);
00172 }
00173
00174 ControlBase* WaypointWalkControl::WaypointEditControl::doSelect() {
00175 for(unsigned int i=0; i<hilights.size(); i++) {
00176 ControlBase * curctl=options[hilights[i]];
00177 if(curctl==up) {
00178 WaypointWalkMC::WaypointList_t& list=MMAccessor<WaypointWalkMC>(walk_id)->getWaypointList();
00179 list.swap(list.prev(waypoint_id),waypoint_id);
00180 sndman->PlayFile(config->controller.select_snd);
00181 return NULL;
00182 } else if(curctl==down) {
00183 WaypointWalkMC::WaypointList_t& list=MMAccessor<WaypointWalkMC>(walk_id)->getWaypointList();
00184 list.swap(waypoint_id,list.next(waypoint_id));
00185 sndman->PlayFile(config->controller.select_snd);
00186 return NULL;
00187 } else if(curctl==del) {
00188 MMAccessor<WaypointWalkMC>(walk_id)->getWaypointList().erase(waypoint_id);
00189 sndman->PlayFile(config->controller.select_snd);
00190 return NULL;
00191 } else if(curctl==set) {
00192 MMAccessor<WaypointWalkMC>(walk_id)->setTargetWaypoint(waypoint_id);
00193 sndman->PlayFile(config->controller.select_snd);
00194 return NULL;
00195 }
00196 }
00197 return ControlBase::doSelect();
00198 }
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255