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;
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
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
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);
00030
00031
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
00039
00040 pose.takeSnapshot();
00041
00042 for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00043 pose.setOutputCmd(i,0);
00044
00045 SharedObject<reach_t> reach;
00046 reachID=motman->addPersistentMotion(reach);
00047
00048 erouter->addListener(this,EventBase::estopEGID);
00049
00050 return ControlBase::activate(disp_id,gui);
00051 }
00052
00053 void
00054 PostureEditor::refresh() {
00055
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
00064 pose.LoadFile(loadPose->getLastInput().c_str());
00065 updatePose(moveTime);
00066 } else if(lastSlot==savePose || savePose->getLastInput().size()>0) {
00067
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
00081 pauseCalled=true;
00082 erouter->removeListener(this,EventBase::timerEGID);
00083 }
00084
00085 void
00086 PostureEditor::deactivate() {
00087
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
00097 lastSlot=options[hilights.front()];
00098
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
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
00148
00149
00150
00151
00152
00153
00154
00155
00156