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