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   // clear the LEDs though
00052   for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00053     pose.setOutputCmd(i,0);
00054   // add it the motion sequence we'll be using to move to changes
00055   SharedObject<SmallMotionSequenceMC> reach;
00056   reachID=motman->addPersistentMotion(reach);
00057   // we'll need to know when estop is turned on or off
00058   erouter->addListener(this,EventBase::estopEGID);
00059   // call super class
00060   return ControlBase::activate(disp_id,gui);
00061 }
00062 
00063 void
00064 PostureEditor::refresh() {
00065   //cout << "refresh" << endl;
00066   if(isEStopped()) {
00067     erouter->addTimer(this,0,500);
00068     options[0]=disabledLoadPose;
00069   } else {
00070     options[0]=loadPose;
00071   }
00072   if(loadPose->getLastInput().size()>0) {
00073     pose.LoadFile(loadPose->getLastInput().c_str());
00074     updatePose(moveTime);
00075     loadPose->clearLastInput();
00076   } else if(savePose->getLastInput().size()>0) {
00077     // we just got back from the save menu
00078     std::string filename=savePose->getLastInput();
00079     if(filename.find(".")==std::string::npos)
00080       filename+=".pos";
00081     pose.SaveFile(config->motion.makePath(filename).c_str());
00082     savePose->takeInput("");
00083   } else {
00084     updatePose(moveTime/2);
00085   }
00086   pauseCalled=false;
00087   ControlBase::refresh();
00088 }
00089 
00090 void
00091 PostureEditor::pause() {
00092   //cout << "pause" << endl;
00093   refresh(); //one last time, in case this pause is due to un-estop putting Controller into low-profile mode
00094   pauseCalled=true;
00095   erouter->removeTimer(this);
00096   ControlBase::pause();
00097 }
00098 
00099 void
00100 PostureEditor::deactivate() {
00101   //cout << "deactivate" << endl;
00102   //cout << "removeMotion(" << reachID << ")" << endl;
00103   motman->removeMotion(reachID);
00104   reachID=MotionManager::invalid_MC_ID;
00105   erouter->removeListener(this);
00106   erouter->removeTimer(this);
00107   ControlBase::deactivate();
00108 }
00109 
00110 void
00111 PostureEditor::processEvent(const EventBase& e) {
00112   //cout << "processEvent(" << e.getName() << ")" << endl;
00113   if(e.getGeneratorID()==EventBase::estopEGID) {
00114     if(e.getTypeID()==EventBase::deactivateETID) {
00115       MMAccessor<SmallMotionSequenceMC>(reachID)->play();
00116       erouter->removeTimer(this);
00117       if(!pauseCalled)
00118         refresh();
00119     } else {
00120       if(!pauseCalled) {
00121         erouter->addTimer(this,0,500); // timer to allow updates on joint positions
00122         processEvent(EventBase(EventBase::timerEGID,0,EventBase::statusETID)); // but also do one right now
00123       }
00124     }
00125   } else if(e.getGeneratorID()==EventBase::timerEGID) {
00126     //doing a manual copy instead of just takeSnapshot() so we don't disturb the LED settings
00127     for(unsigned int i=0; i<LEDOffset; i++)
00128       pose(i).value=state->outputs[i];
00129     for(unsigned int i=LEDOffset+NumLEDs; i<NumOutputs; i++)
00130       pose(i).value=state->outputs[i];
00131     refresh();
00132   } else {
00133     serr->printf("WARNING: PostureEditor unexpected event: %s\n",e.getName().c_str());
00134   }
00135 }
00136 
00137 bool
00138 PostureEditor::isEStopped() {
00139   return MMAccessor<EmergencyStopMC>(estopID)->getStopped();
00140 }
00141 
00142 void
00143 PostureEditor::updatePose(unsigned int delay) {
00144   //cout << "updatePose" << endl;
00145   bool paused=isEStopped();
00146   MMAccessor<SmallMotionSequenceMC> reach_acc(reachID);
00147   if(paused) {
00148     reach_acc->clear();
00149     return;
00150   }
00151   PostureEngine curpose;
00152   reach_acc->getPose(curpose);
00153   reach_acc->clear();
00154   //we want to keep the current pose to avoid any twitching
00155   reach_acc->setTime(1);
00156   for(unsigned int i=0; i<NumOutputs; i++) //only set weighted joints
00157     if(curpose(i).weight!=0)
00158       reach_acc->setOutputCmd(i,curpose(i));
00159   //now move to desired posture
00160   reach_acc->setTime(delay);
00161   reach_acc->setPose(pose);
00162   reach_acc->play();
00163 }
00164 
00165 
00166 /*! @file
00167  * @brief Describes PostureEditor, which allows numeric control of joints and LEDs
00168  * @author ejt (Creator)
00169  *
00170  * $Author: ejt $
00171  * $Name: tekkotsu-2_4_1 $
00172  * $Revision: 1.18 $
00173  * $State: Exp $
00174  * $Date: 2005/08/02 22:24:20 $
00175  */

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:48 2005 by Doxygen 1.4.4