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