Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PostureEditor.cc

Go to the documentation of this file.
00001 #include "PostureEditor.h"
00002 #include "Motion/MMAccessor.h"
00003 #include "Motion/EmergencyStopMC.h"
00004 #include "Motion/MotionSequenceMC.h"
00005 #include "Motion/LedMC.h"
00006 #include "Sound/SoundManager.h"
00007 #include "Events/EventRouter.h"
00008 #include "ValueEditControl.h"
00009 #include "NullControl.h"
00010 #include "StringInputControl.h"
00011 #include "FileInputControl.h"
00012 
00013 PostureEditor::PostureEditor(MotionManager::MC_ID estop_ID)
00014   : ControlBase("Posture Editor","Allows you to load, save, and numerically edit the posture"), 
00015     pose(), reachID(MotionManager::invalid_MC_ID),
00016     estopID(estop_ID), loadPose(NULL), disabledLoadPose(NULL), savePose(NULL), pauseCalled(false)
00017 {
00018   // add load and save menus
00019   pushSlot(loadPose=new FileInputControl("Load Posture","Select a posture to open",config->portPath(config->motion.root)));
00020   loadPose->setFilter("*.pos");
00021   disabledLoadPose=new NullControl("[Load disabled by EStop]","Cannot load new postures while EStop is active");
00022   pushSlot(savePose=new StringInputControl("Save Posture","Please enter the filename to save to (in "+config->motion.root+")"));
00023 
00024   // add submenu for weight editors
00025   ControlBase * weights;
00026   pushSlot(weights=new ControlBase("Weights","Set the weights for outputs"));
00027   for(unsigned int i=0; i<NumOutputs; i++)
00028     weights->pushSlot(new ValueEditControl<float>(outputNames[i],&pose(i).weight));
00029 
00030   pushSlot(NULL); // a separator for clarity
00031 
00032   // add actual value editors
00033   for(unsigned int i=0; i<NumOutputs; i++)
00034     pushSlot(new ValueEditControl<float>(outputNames[i],&pose(i).value));
00035 }
00036 
00037 PostureEditor::~PostureEditor() {
00038   delete loadPose;
00039   delete disabledLoadPose;
00040   options[0]=NULL;
00041 }
00042 
00043 ControlBase *
00044 PostureEditor::activate(MotionManager::MC_ID disp_id, Socket * gui) {
00045   //cout << "activate" << endl;
00046   if(reachID!=MotionManager::invalid_MC_ID) // was already activated
00047     return ControlBase::activate(disp_id,gui); // happens when estop is turned on, causing "reactivation"
00048   // start off with current pose
00049   pose.takeSnapshot();
00050   pose.setWeights(1);
00051 #ifdef TGT_HAS_LEDS
00052   // clear the LEDs though
00053   for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00054     pose.setOutputCmd(i,0);
00055 #endif
00056   // add it the motion sequence we'll be using to move to changes
00057   SharedObject<SmallMotionSequenceMC> reach;
00058   reachID=motman->addPersistentMotion(reach);
00059   // we'll need to know when estop is turned on or off
00060   erouter->addListener(this,EventBase::estopEGID);
00061   // call super class
00062   return ControlBase::activate(disp_id,gui);
00063 }
00064 
00065 void
00066 PostureEditor::refresh() {
00067   //cout << "refresh" << endl;
00068   if(isEStopped()) {
00069     processEvent(EventBase(EventBase::timerEGID,1,EventBase::statusETID,0));
00070     erouter->addTimer(this,0,500);
00071     options[0]=disabledLoadPose;
00072   } else {
00073     options[0]=loadPose;
00074   }
00075   if(loadPose->getLastInput().size()>0) {
00076     pose.loadFile(loadPose->getLastInput().c_str());
00077     updatePose(moveTime);
00078     loadPose->clearLastInput();
00079   } else if(savePose->getLastInput().size()>0) {
00080     // we just got back from the save menu
00081     std::string filename=savePose->getLastInput();
00082     if(filename.find(".")==std::string::npos)
00083       filename+=".pos";
00084     pose.saveFile(config->motion.makePath(filename).c_str());
00085     savePose->takeInput("");
00086   } else {
00087     updatePose(moveTime/2);
00088   }
00089   pauseCalled=false;
00090   ControlBase::refresh();
00091 }
00092 
00093 void
00094 PostureEditor::pause() {
00095   //cout << "pause" << endl;
00096   refresh(); //one last time, in case this pause is due to un-estop putting Controller into low-profile mode
00097   pauseCalled=true;
00098   erouter->removeTimer(this);
00099   ControlBase::pause();
00100 }
00101 
00102 void
00103 PostureEditor::deactivate() {
00104   //cout << "deactivate" << endl;
00105   //cout << "removeMotion(" << reachID << ")" << endl;
00106   motman->removeMotion(reachID);
00107   reachID=MotionManager::invalid_MC_ID;
00108   erouter->removeListener(this);
00109   erouter->removeTimer(this);
00110   ControlBase::deactivate();
00111 }
00112 
00113 void
00114 PostureEditor::processEvent(const EventBase& e) {
00115   //cout << "processEvent(" << e.getName() << ")" << endl;
00116   if(e.getGeneratorID()==EventBase::estopEGID) {
00117     if(e.getTypeID()==EventBase::deactivateETID) {
00118       MMAccessor<SmallMotionSequenceMC>(reachID)->play();
00119       erouter->removeTimer(this);
00120       if(!pauseCalled)
00121         refresh();
00122     } else {
00123       if(!pauseCalled) {
00124         erouter->addTimer(this,0,500); // timer to allow updates on joint positions
00125         processEvent(EventBase(EventBase::timerEGID,0,EventBase::statusETID)); // but also do one right now
00126       }
00127     }
00128   } else if(e.getGeneratorID()==EventBase::timerEGID) {
00129 #ifndef TGT_HAS_LEDS
00130     pose.takeSnapshot();
00131 #else
00132     //doing a manual copy instead of just takeSnapshot() so we don't disturb the LED settings
00133     for(unsigned int i=0; i<LEDOffset; i++)
00134       pose(i).value=state->outputs[i];
00135     for(unsigned int i=LEDOffset+NumLEDs; i<NumOutputs; i++)
00136       pose(i).value=state->outputs[i];
00137 #endif
00138     if(e.getSourceID()==0) // source==1 indicates it's a forged event sent from refresh -- don't inf. recurse
00139       refresh();
00140   } else {
00141     serr->printf("WARNING: PostureEditor unexpected event: %s\n",e.getName().c_str());
00142   }
00143 }
00144 
00145 bool
00146 PostureEditor::isEStopped() {
00147   return MMAccessor<EmergencyStopMC>(estopID)->getStopped();
00148 }
00149 
00150 void
00151 PostureEditor::updatePose(unsigned int delay) {
00152   //cout << "updatePose" << endl;
00153   bool paused=isEStopped();
00154   MMAccessor<SmallMotionSequenceMC> reach_acc(reachID);
00155   if(paused) {
00156     reach_acc->clear();
00157     return;
00158   }
00159   PostureEngine curpose;
00160   reach_acc->getPose(curpose);
00161   reach_acc->clear();
00162   //we want to keep the current pose to avoid any twitching
00163   reach_acc->setTime(1);
00164   for(unsigned int i=0; i<NumOutputs; i++) //only set weighted joints
00165     if(curpose(i).weight!=0)
00166       reach_acc->setOutputCmd(i,curpose(i));
00167   //now move to desired posture
00168   reach_acc->setTime(delay);
00169   reach_acc->setPose(pose);
00170   reach_acc->play();
00171 }
00172 
00173 
00174 /*! @file
00175  * @brief Describes PostureEditor, which allows numeric control of joints and LEDs
00176  * @author ejt (Creator)
00177  *
00178  * $Author: ejt $
00179  * $Name: tekkotsu-4_0 $
00180  * $Revision: 1.21 $
00181  * $State: Exp $
00182  * $Date: 2007/11/18 06:47:00 $
00183  */

Tekkotsu v4.0
Generated Thu Nov 22 00:54:55 2007 by Doxygen 1.5.4