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

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:14 2005 by Doxygen 1.4.0