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
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
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);
00031
00032
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
00046 if(reachID!=MotionManager::invalid_MC_ID)
00047 return ControlBase::activate(disp_id,gui);
00048
00049 pose.takeSnapshot();
00050 pose.setWeights(1);
00051 #ifdef TGT_HAS_LEDS
00052
00053 for(unsigned int i=LEDOffset; i<LEDOffset+NumLEDs; i++)
00054 pose.setOutputCmd(i,0);
00055 #endif
00056
00057 SharedObject<SmallMotionSequenceMC> reach;
00058 reachID=motman->addPersistentMotion(reach);
00059
00060 erouter->addListener(this,EventBase::estopEGID);
00061
00062 return ControlBase::activate(disp_id,gui);
00063 }
00064
00065 void
00066 PostureEditor::refresh() {
00067
00068 if(isEStopped()) {
00069 processEvent(EventBase(EventBase::timerEGID,1,EventBase::statusETID,0));
00070 erouter->addTimer(this,0,500);
00071 options[0]=disabledLoadPose;
00072 } else {
00073 options[0]=loadPose;
00074 }
00075 if(loadPose->getLastInput().size()>0) {
00076 pose.loadFile(loadPose->getLastInput().c_str());
00077 updatePose(moveTime);
00078 loadPose->clearLastInput();
00079 } else if(savePose->getLastInput().size()>0) {
00080
00081 std::string filename=savePose->getLastInput();
00082 if(filename.find(".")==std::string::npos)
00083 filename+=".pos";
00084 pose.saveFile(config->motion.makePath(filename).c_str());
00085 savePose->takeInput("");
00086 } else {
00087 updatePose(moveTime/2);
00088 }
00089 pauseCalled=false;
00090 ControlBase::refresh();
00091 }
00092
00093 void
00094 PostureEditor::pause() {
00095
00096 refresh();
00097 pauseCalled=true;
00098 erouter->removeTimer(this);
00099 ControlBase::pause();
00100 }
00101
00102 void
00103 PostureEditor::deactivate() {
00104
00105
00106 motman->removeMotion(reachID);
00107 reachID=MotionManager::invalid_MC_ID;
00108 erouter->removeListener(this);
00109 erouter->removeTimer(this);
00110 ControlBase::deactivate();
00111 }
00112
00113 void
00114 PostureEditor::processEvent(const EventBase& e) {
00115
00116 if(e.getGeneratorID()==EventBase::estopEGID) {
00117 if(e.getTypeID()==EventBase::deactivateETID) {
00118 MMAccessor<SmallMotionSequenceMC>(reachID)->play();
00119 erouter->removeTimer(this);
00120 if(!pauseCalled)
00121 refresh();
00122 } else {
00123 if(!pauseCalled) {
00124 erouter->addTimer(this,0,500);
00125 processEvent(EventBase(EventBase::timerEGID,0,EventBase::statusETID));
00126 }
00127 }
00128 } else if(e.getGeneratorID()==EventBase::timerEGID) {
00129 #ifndef TGT_HAS_LEDS
00130 pose.takeSnapshot();
00131 #else
00132
00133 for(unsigned int i=0; i<LEDOffset; i++)
00134 pose(i).value=state->outputs[i];
00135 for(unsigned int i=LEDOffset+NumLEDs; i<NumOutputs; i++)
00136 pose(i).value=state->outputs[i];
00137 #endif
00138 if(e.getSourceID()==0)
00139 refresh();
00140 } else {
00141 serr->printf("WARNING: PostureEditor unexpected event: %s\n",e.getName().c_str());
00142 }
00143 }
00144
00145 bool
00146 PostureEditor::isEStopped() {
00147 return MMAccessor<EmergencyStopMC>(estopID)->getStopped();
00148 }
00149
00150 void
00151 PostureEditor::updatePose(unsigned int delay) {
00152
00153 bool paused=isEStopped();
00154 MMAccessor<SmallMotionSequenceMC> reach_acc(reachID);
00155 if(paused) {
00156 reach_acc->clear();
00157 return;
00158 }
00159 PostureEngine curpose;
00160 reach_acc->getPose(curpose);
00161 reach_acc->clear();
00162
00163 reach_acc->setTime(1);
00164 for(unsigned int i=0; i<NumOutputs; i++)
00165 if(curpose(i).weight!=0)
00166 reach_acc->setOutputCmd(i,curpose(i));
00167
00168 reach_acc->setTime(delay);
00169 reach_acc->setPose(pose);
00170 reach_acc->play();
00171 }
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183