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 "Events/EventRouter.h"
00007 #include "ValueEditControl.h"
00008 #include "NullControl.h"
00009 #include "SoundPlay/SoundManager.h"
00010 
00011 typedef MotionSequenceMC<MotionSequence::SizeSmall> reach_t; //!< just to save some typing
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), lastSlot(NULL), loadPose(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   pushSlot(savePose=new StringInputControl("Save Posture","Please enter the filename to save to (in "+config->motion.root+")"));
00022 
00023   // add submenu for weight editors
00024   ControlBase * weights;
00025   pushSlot(weights=new ControlBase("Weights","Set the weights for outputs"));
00026   for(unsigned int i=0; i<NumOutputs; i++)
00027     weights->pushSlot(new ValueEditControl<float>(outputNames[i],&pose(i).weight));
00028 
00029   pushSlot(NULL); // a separator for clarity
00030 
00031   // add actual value editors
00032   for(unsigned int i=0; i<NumOutputs; i++)
00033     pushSlot(new ValueEditControl<float>(outputNames[i],&pose(i).value));
00034 }
00035 
00036 ControlBase *
00037 PostureEditor::activate(MotionManager::MC_ID disp_id, Socket * gui) {
00038   //cout << "activate" << endl;
00039   // start off with current pose
00040   pose.takeSnapshot();
00041   // clear the LEDs though
00042   for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00043     pose.setOutputCmd(i,0);
00044   // add it the motion sequence we'll be using to move to changes
00045   SharedObject<reach_t> reach;
00046   reachID=motman->addPersistentMotion(reach);
00047   // we'll need to know when estop is turned on or off
00048   erouter->addListener(this,EventBase::estopEGID);
00049   // call super class
00050   return ControlBase::activate(disp_id,gui);
00051 }
00052 
00053 void
00054 PostureEditor::refresh() {
00055   //cout << "refresh" << endl;
00056   if(isEStopped()) {
00057     erouter->addTimer(this,0,500);
00058     options[0]=NULL;
00059   } else {
00060     options[0]=loadPose;
00061   }
00062   if(lastSlot==loadPose) {
00063     // we just got back from the load menu
00064     pose.LoadFile(loadPose->getLastInput().c_str());
00065     updatePose(moveTime);
00066   } else if(lastSlot==savePose || savePose->getLastInput().size()>0) {
00067     // we just got back from the save menu
00068     pose.SaveFile(config->motion.makePath(savePose->getLastInput()).c_str());
00069     savePose->takeInput("");
00070   } else {
00071     updatePose(moveTime/2);
00072   }
00073   lastSlot=NULL;
00074   pauseCalled=false;
00075   ControlBase::refresh();
00076 }
00077 
00078 void
00079 PostureEditor::pause() {
00080   //cout << "paused" << endl;
00081   pauseCalled=true;
00082   erouter->removeListener(this,EventBase::timerEGID);
00083 }
00084 
00085 void
00086 PostureEditor::deactivate() {
00087   //cout << "deactivate" << endl;
00088   motman->removeMotion(reachID);
00089   reachID=MotionManager::invalid_MC_ID;
00090   erouter->removeListener(this);
00091   ControlBase::deactivate();
00092 }
00093 
00094 ControlBase*
00095 PostureEditor::doSelect() {
00096   // record the option that is being selected, in case it's the load or save
00097   lastSlot=options[hilights.front()];
00098   // but do what we'd normally do (select that option)
00099   return ControlBase::doSelect();
00100 }
00101 
00102 void
00103 PostureEditor::processEvent(const EventBase& e) {
00104   if(e.getGeneratorID()==EventBase::estopEGID) {
00105     if(e.getTypeID()==EventBase::deactivateETID) {
00106       MMAccessor<reach_t>(reachID)->play();
00107       erouter->removeListener(this,EventBase::timerEGID);
00108       if(!pauseCalled)
00109         refresh();
00110     } else {
00111       if(!pauseCalled) {
00112         erouter->addTimer(this,0,500);
00113         processEvent(EventBase(EventBase::timerEGID,0,EventBase::statusETID));
00114       }
00115     }
00116   } else if(e.getGeneratorID()==EventBase::timerEGID) {
00117     //doing a manual copy instead of just takeSnapshot() so we don't disturb the LED settings
00118     for(unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++)
00119       pose(i).value=state->outputs[i];
00120     refresh();
00121   } else {
00122     serr->printf("WARNING: PostureEditor unexpected event: %s\n",e.getName().c_str());
00123   }
00124 }
00125 
00126 bool
00127 PostureEditor::isEStopped() {
00128   return MMAccessor<EmergencyStopMC>(estopID)->getStopped();
00129 }
00130 
00131 void
00132 PostureEditor::updatePose(unsigned int delay) {
00133   bool paused=isEStopped();
00134   MMAccessor<reach_t> reach_acc(reachID);
00135   reach_acc->clear();
00136   reach_acc->setPlayTime(delay);
00137   reach_acc->setPose(pose);
00138   reach_acc->setPlayTime(delay+100);
00139   reach_acc->setPose(pose);
00140   if(paused)
00141     reach_acc->pause();
00142   else
00143     reach_acc->play();
00144 }
00145 
00146 
00147 /*! @file
00148  * @brief Describes PostureEditor, which allows numeric control of joints and LEDs
00149  * @author ejt (Creator)
00150  *
00151  * $Author: ejt $
00152  * $Name: tekkotsu-2_2_1 $
00153  * $Revision: 1.7 $
00154  * $State: Exp $
00155  * $Date: 2004/10/18 19:53:02 $
00156  */

Tekkotsu v2.2.1
Generated Tue Nov 23 16:36:39 2004 by Doxygen 1.3.9.1