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