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