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