diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/BehaviorBase.cc ./Behaviors/BehaviorBase.cc --- ../Tekkotsu_3.0/Behaviors/BehaviorBase.cc 2006-09-27 17:15:52.000000000 -0400 +++ ./Behaviors/BehaviorBase.cc 2007-06-14 02:24:03.000000000 -0400 @@ -36,7 +36,8 @@ SetAutoDelete(false); if(started) std::cerr << "Behavior " << getName() << " deleted while running: use 'RemoveReference', not 'delete'" << std::endl; - erouter->removeListener(this); + if(erouter!=NULL) + erouter->removeListener(this); getRegistryInstance().erase(this); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controller.cc ./Behaviors/Controller.cc --- ../Tekkotsu_3.0/Behaviors/Controller.cc 2006-09-18 14:08:05.000000000 -0400 +++ ./Behaviors/Controller.cc 2007-11-18 01:47:00.000000000 -0500 @@ -7,13 +7,13 @@ #include "Shared/get_time.h" #include "Sound/SoundManager.h" #include "Events/TextMsgEvent.h" +#include "Shared/RobotInfo.h" #include "Shared/ERS210Info.h" #include "Shared/ERS220Info.h" +#include "Shared/ERS2xxInfo.h" #include "Shared/ERS7Info.h" #include "Shared/string_util.h" -#ifndef PLATFORM_APERIOS -# include "local/sim/Simulator.h" -#endif +#include "Shared/ProjectInterface.h" #include Controller * Controller::theOneController=NULL; @@ -27,7 +27,7 @@ EventBase Controller::cancel; using namespace string_util; - +using namespace std; void Controller::DoStart() { BehaviorBase::DoStart(); @@ -38,7 +38,7 @@ sndman->loadFile(config->controller.cancel_snd); erouter->addListener(this,EventBase::estopEGID); // Turn on wireless - gui_comm=wireless->socket(SocketNS::SOCK_STREAM, 2048, 32000); + gui_comm=wireless->socket(Socket::SOCK_STREAM, 2048, 32000); wireless->setReceiver(gui_comm->sock, gui_comm_callback); wireless->setDaemon(gui_comm,true); wireless->listen(gui_comm->sock, config->controller.gui_port); @@ -61,8 +61,10 @@ motman->removeMotion(display); display=MotionManager::invalid_MC_ID; //these two lines help prevent residual display in case that was the only MotionCommand using LEDs +#ifdef TGT_HAS_LEDS for(unsigned int i=LEDOffset; isetOutput(NULL,i,0.f); +#endif gui_comm->printf("goodbye\n"); wireless->setDaemon(gui_comm,false); wireless->close(gui_comm); @@ -282,38 +284,57 @@ return 0; } +/*! Select which model is running and call initButtons with the appropriate button offsets + * This could be somewhat simplified by using capabilities.getButtonOffset(), (wouldn't need + * the ERS2xx case with essentially duplicated ERS210 and ERS220 cases), but this + * style has the advantage that the symbols are checked by the compiler so there's no + * chance of a typo in a button name going unnoticed. */ void Controller::init() { - if(state->robotDesign & WorldState::ERS210Mask) { - nextItem=EventBase(EventBase::buttonEGID,ERS210Info::HeadFrButOffset,EventBase::deactivateETID,0); - prevItem=EventBase(EventBase::buttonEGID,ERS210Info::HeadBkButOffset,EventBase::deactivateETID,0); - nextItemFast=EventBase(EventBase::buttonEGID,ERS210Info::HeadFrButOffset,EventBase::statusETID,666); - prevItemFast=EventBase(EventBase::buttonEGID,ERS210Info::HeadBkButOffset,EventBase::statusETID,666); - selectItem=EventBase(EventBase::buttonEGID,ERS210Info::ChinButOffset,EventBase::deactivateETID,250); - cancel=EventBase(EventBase::buttonEGID,ERS210Info::BackButOffset,EventBase::deactivateETID,250); - } else if(state->robotDesign & WorldState::ERS220Mask) { - nextItem=EventBase(EventBase::buttonEGID,ERS220Info::TailLeftButOffset,EventBase::deactivateETID,0); - prevItem=EventBase(EventBase::buttonEGID,ERS220Info::TailRightButOffset,EventBase::deactivateETID,0); - //the 220 doesn't really support the next two because it's using boolean buttons + if(TargetName == ERS2xxInfo::TargetName) { + // compatability mode, see which of the targets is actually running + // Note using ERS2xxInfo namespace to get appropriate offsets! + // could remove duplication with "direct" 210/220 cases below by using something like: + // capabilities.getButtonOffset(ERS210Info::outputNames[ERS210Info::fooButOffset]) + if(RobotName == ERS210Info::TargetName) { + initButtons(666,250,ERS2xxInfo::HeadFrButOffset,ERS2xxInfo::HeadBkButOffset,ERS2xxInfo::HeadFrButOffset,ERS2xxInfo::HeadBkButOffset,ERS2xxInfo::ChinButOffset,ERS2xxInfo::BackButOffset); + } else if(RobotName == ERS220Info::TargetName) { + //the 220 doesn't really support "fast" because it's using boolean buttons + //i'm using a "hack" on the 210 because the pressure sensitivity causes status + //events to continually be sent but since this is just on/off, it only gets the + //activate/deactivate. To fix this, override nextItemFast and prevItemFast with + // timers and do timer management in processEvents() + initButtons(666,50,ERS2xxInfo::TailLeftButOffset,ERS2xxInfo::TailRightButOffset,ERS2xxInfo::TailLeftButOffset,ERS2xxInfo::TailRightButOffset,ERS2xxInfo::TailCenterButOffset,ERS2xxInfo::BackButOffset); + } else { + cerr << "Controller: Unsupported 2xx model '" << RobotName << "'! Appears to have buttons, but Controller doesn't know how to use them." << endl; + } + } else if(RobotName == ERS210Info::TargetName) { + initButtons(666,250,ERS210Info::HeadFrButOffset,ERS210Info::HeadBkButOffset,ERS210Info::HeadFrButOffset,ERS210Info::HeadBkButOffset,ERS210Info::ChinButOffset,ERS210Info::BackButOffset); + } else if(RobotName == ERS220Info::TargetName) { + //the 220 doesn't really support "fast" because it's using boolean buttons //i'm using a "hack" on the 210 because the pressure sensitivity causes status //events to continually be sent but since this is just on/off, it only gets the - //activate/deactivate. To fix this, make these timers and do timer management - //in processEvents() - nextItemFast=EventBase(EventBase::buttonEGID,ERS220Info::TailLeftButOffset,EventBase::statusETID,666); - prevItemFast=EventBase(EventBase::buttonEGID,ERS220Info::TailRightButOffset,EventBase::statusETID,666); - selectItem=EventBase(EventBase::buttonEGID,ERS220Info::TailCenterButOffset,EventBase::deactivateETID,50); - cancel=EventBase(EventBase::buttonEGID,ERS220Info::BackButOffset,EventBase::deactivateETID,50); - } else if(state->robotDesign & WorldState::ERS7Mask) { - nextItem=EventBase(EventBase::buttonEGID,ERS7Info::FrontBackButOffset,EventBase::deactivateETID,0); - prevItem=EventBase(EventBase::buttonEGID,ERS7Info::RearBackButOffset,EventBase::deactivateETID,0); - nextItemFast=EventBase(EventBase::buttonEGID,ERS7Info::FrontBackButOffset,EventBase::statusETID,500); - prevItemFast=EventBase(EventBase::buttonEGID,ERS7Info::RearBackButOffset,EventBase::statusETID,500); - selectItem=EventBase(EventBase::buttonEGID,ERS7Info::MiddleBackButOffset,EventBase::deactivateETID,25); - cancel=EventBase(EventBase::buttonEGID,ERS7Info::HeadButOffset,EventBase::deactivateETID,25); + //activate/deactivate. To fix this, override nextItemFast and prevItemFast with + // timers and do timer management in processEvents() + initButtons(666,50,ERS220Info::TailLeftButOffset,ERS220Info::TailRightButOffset,ERS220Info::TailLeftButOffset,ERS220Info::TailRightButOffset,ERS220Info::TailCenterButOffset,ERS220Info::BackButOffset); + } else if(RobotName == ERS7Info::TargetName) { + initButtons(500,25,ERS7Info::FrontBackButOffset,ERS7Info::RearBackButOffset,ERS7Info::FrontBackButOffset,ERS7Info::RearBackButOffset,ERS7Info::MiddleBackButOffset,ERS7Info::HeadButOffset); } else { - serr->printf("Controller: Unsupported model!\n"); +#ifdef TGT_HAS_BUTTONS + cerr << "Controller: Unsupported model '" << RobotName << "'! Appears to have buttons, but Controller doesn't know how to use them." << endl; +#endif } } +void Controller::initButtons(unsigned fastTime, unsigned downTime, unsigned nextB, unsigned prevB, unsigned nextFastB, unsigned prevFastB, unsigned selectB, unsigned cancelB) { + nextItem=EventBase(EventBase::buttonEGID,nextB,EventBase::deactivateETID,0); + prevItem=EventBase(EventBase::buttonEGID,prevB,EventBase::deactivateETID,0); + nextItemFast=EventBase(EventBase::buttonEGID,nextFastB,EventBase::statusETID,fastTime); + prevItemFast=EventBase(EventBase::buttonEGID,prevFastB,EventBase::statusETID,fastTime); + selectItem=EventBase(EventBase::buttonEGID,selectB,EventBase::deactivateETID,downTime); + cancel=EventBase(EventBase::buttonEGID,cancelB,EventBase::deactivateETID,downTime); +} + + bool Controller::select(ControlBase* item, const std::string& name) { // Depth first const std::vector& slots = item->getSlots(); @@ -488,31 +509,31 @@ #ifdef PLATFORM_APERIOS serr->printf("!sim command invalid -- not running in simulator!\n"); #else - Simulator::sendCommand(s.substr(offsets[1])); + ProjectInterface::sendCommand(s.substr(offsets[1])); #endif } else setNext(cmdstack.top()->takeInput(s)); } } -int Controller::setConfig(const char *str) { - char buf[80]; - strncpy(buf, str, 79); - char *value=index(buf, '='); - char *key=index(buf, '.'); - if (key==NULL || value==NULL) return -1; - if (key>=value) return -1; - *key=0; - key++; - *value=0; - value++; - Config::section_t section=config->parseSection(buf); - if (section==Config::sec_invalid) return -2; - config->setValue(section, key, value, true); - //void *val_set=config->setValue(section, key, value, true); - // might want to catch setValue's return value and do - // something special for some config values? - // (such as reboot a subsystem to reload new settings) +int Controller::setConfig(const std::string& str) { + string::size_type eq=str.find('='); + if(eq==string::npos) + return -2; + plist::ObjectBase* entry = config->resolveEntry(string_util::trim(str.substr(0,eq))); + if(entry==NULL) { + string::size_type p=str.find('.'); + string sec=string_util::trim(str.substr(0,p)); + string key=string_util::trim(str.substr(p+1,eq-p-1)); + string val=string_util::trim(str.substr(eq+1)); + if(config->setValue(sec,key,val)==NULL) + return -2; + return 0; + } + plist::PrimitiveBase* prim = dynamic_cast(entry); + if(prim==NULL) + return -2; + prim->set(string_util::trim(str.substr(eq+1))); return 0; } @@ -538,8 +559,10 @@ //these two lines help prevent residual display in case that was the only MotionCommand using LEDs motman->setPriority(display,MotionManager::kIgnoredPriority); isControlling=false; +#ifdef TGT_HAS_LEDS for(unsigned int i=LEDOffset; isetOutput(NULL,i,0.f); +#endif erouter->removeTrapper(this); cmdstack.top()->pause(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controller.h ./Behaviors/Controller.h --- ../Tekkotsu_3.0/Behaviors/Controller.h 2005-09-01 14:53:06.000000000 -0400 +++ ./Behaviors/Controller.h 2007-06-28 00:36:19.000000000 -0400 @@ -147,17 +147,20 @@ static int console_callback(char *buf, int bytes); //!< called by wireless when someone has entered new data on the tekkotsu console (NOT cin) protected: - //! assigns appropriate values to the static event bases + //! calls initButtons with the appropriate button offsets for the host robot model void init(); - + + //! assigns appropriate values to the static event bases + void initButtons(unsigned fastTime, unsigned downTime, unsigned nextB, unsigned prevB, unsigned nextFastB, unsigned prevFastB, unsigned selectB, unsigned cancelB); + //! called with each line that's entered on the tekkotsu console or from the GUI void takeLine(const std::string& s); //! called with slots (options), a name to lookup; will select the named control - bool Controller::select(ControlBase* item, const std::string& name); + bool select(ControlBase* item, const std::string& name); //! sets a config value - some values may require additional processing (done here) to have the new values take effect - int setConfig(const char *str); + int setConfig(const std::string& str); //! maintains top Control /*! @param next one of: @li NULL: pop() ::cmdstack @li ::cmdstack.top(): nothing @li other address: ::push(@a next) diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/BatteryCheckControl.h ./Behaviors/Controls/BatteryCheckControl.h --- ../Tekkotsu_3.0/Behaviors/Controls/BatteryCheckControl.h 2005-01-25 15:06:39.000000000 -0500 +++ ./Behaviors/Controls/BatteryCheckControl.h 2007-06-05 14:03:24.000000000 -0400 @@ -54,8 +54,8 @@ std::string s("refresh\n"); s+=getName()+"\n1\n0\n0\nPower remain: "; s+=tmp; - s+="%\nSee console output for details\n"; - s+="status\nPower remaining: "; + s+="%\n0\nSee console output for details\n"; + s+="status\n1\nPower remaining: "; s+=tmp; s+="%\n"; gui_comm->write((const byte*)s.c_str(),s.size()); @@ -68,7 +68,7 @@ } //! calls refresh() to redisplay with new information if it's not a vibration event virtual void processEvent(const EventBase& event) { - if(event.getSourceID()!=PowerSourceID::VibrationSID) + if(event.getSourceID()!=PowerSrcID::VibrationSID) refresh(); } virtual ControlBase * doSelect() { @@ -83,19 +83,19 @@ sout->printf("\tCurrent:\t%g\n",state->sensors[PowerCurrentOffset]); sout->printf("\tTemperature:\t%g\n",state->sensors[PowerThermoOffset]); sout->printf("\tFlags:\t"); - if(state->powerFlags[PowerSourceID::BatteryConnectSID]) + if(state->powerFlags[PowerSrcID::BatteryConnectSID]) sout->printf("BatteryConnect "); - if(state->powerFlags[PowerSourceID::DischargingSID]) + if(state->powerFlags[PowerSrcID::DischargingSID]) sout->printf("Discharging "); - if(state->powerFlags[PowerSourceID::ChargingSID]) + if(state->powerFlags[PowerSrcID::ChargingSID]) sout->printf("Charging "); - if(state->powerFlags[PowerSourceID::ExternalPowerSID]) + if(state->powerFlags[PowerSrcID::ExternalPowerSID]) sout->printf("ExternalPower "); - if(state->powerFlags[PowerSourceID::PowerGoodSID]) + if(state->powerFlags[PowerSrcID::PowerGoodSID]) sout->printf("PowerGood "); - if(state->powerFlags[PowerSourceID::LowPowerWarnSID]) + if(state->powerFlags[PowerSrcID::LowPowerWarnSID]) sout->printf("LowPowerWarn "); - if(state->powerFlags[PowerSourceID::BatteryEmptySID]) + if(state->powerFlags[PowerSrcID::BatteryEmptySID]) sout->printf("BatteryEmpty "); sout->printf("\n"); if(display_id!=MotionManager::invalid_MC_ID) { diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/BehaviorSwitchControl.h ./Behaviors/Controls/BehaviorSwitchControl.h --- ../Tekkotsu_3.0/Behaviors/Controls/BehaviorSwitchControl.h 2006-09-01 14:21:20.000000000 -0400 +++ ./Behaviors/Controls/BehaviorSwitchControl.h 2007-05-22 00:24:26.000000000 -0400 @@ -5,7 +5,7 @@ #include "ControlBase.h" #include "Behaviors/BehaviorBase.h" #include "Shared/ReferenceCounter.h" -#include "Shared/Factory.h" +#include "Shared/Factories.h" #include "Shared/debuget.h" #include "Events/TextMsgEvent.h" @@ -178,7 +178,7 @@ //! Allows proper switching between major behaviors, calling DoStart and DoStop -template < class B, class Al = Factory< B > > +template < class B, class Al = typename Factory0Arg::template Factory > class BehaviorSwitchControl : public BehaviorSwitchControlBase { public: //! constructor, can use this to toggle a single behavior on and off @@ -237,14 +237,14 @@ virtual void startmine() { if(!retained) { Al allocator; - mybeh=allocator.construct(); + mybeh=allocator(); mybeh->setName(getName()); if(behgrp!=NULL) behgrp->curBehavior=mybeh; } else { if(mybeh==NULL) { Al allocator; - mybeh=allocator.construct(); + mybeh=allocator(); mybeh->setName(getName()); mybeh->AddReference(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/ConfigurationEditor.cc ./Behaviors/Controls/ConfigurationEditor.cc --- ../Tekkotsu_3.0/Behaviors/Controls/ConfigurationEditor.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Controls/ConfigurationEditor.cc 2007-11-12 23:16:00.000000000 -0500 @@ -0,0 +1,238 @@ +#include "ConfigurationEditor.h" +#include "Behaviors/Controls/NullControl.h" +#include "Shared/plistCollections.h" +#include "Shared/Config.h" +#include "Shared/string_util.h" +#include "Wireless/Wireless.h" +#include + +using namespace std; + +void ConfigurationEditor::init() { + if(root==NULL) + setRootCollection(config); + pushSlot(NULL); // just to mark as having sub-nodes, will replace during refresh +} + +void ConfigurationEditor::refresh() { + plist::Collection * curcol = (path.size()==0) ? root : dynamic_cast(root->resolveEntry(path)); + if(curcol==NULL) { + clearSlots(); + pushSlot(new NullControl("[NULL Collection]")); + ControlBase::refresh(); + return; + } + for(std::vector::const_iterator it=options.begin(); it!=options.end(); ++it) { + if(StringInputControl * input = dynamic_cast(*it)) { + if(input->getLastInput().size()>0) { + std::string key=input->getName(); + if(dynamic_cast(curcol)) + key=key.substr(0,key.find('=')); + plist::PrimitiveBase * prim = dynamic_cast(curcol->resolveEntry(key)); + if(prim==NULL) { + std::cerr << "ERROR: key " << key << " was set but does not correspond to a primitive value" << std::endl; + continue; + } + try { + prim->set(input->getLastInput()); + } catch(const std::exception& e) { + std::cerr << "ERROR: unable to set key " << key << " to value " << input->getLastInput() << '\n' + << " An exception occurred: " << e.what() << std::endl; + } catch(...) { + std::cerr << "ERROR: unable to set key '" << key << "' to value '" << input->getLastInput() << "', an exception occurred." << std::endl; + } + } + } + } + + for(std::vector::iterator it=options.begin(); it!=options.end(); ++it) + if(*it!=NULL && *it!=&load && *it!=&save) + delete *it; + options.clear(); + pushSlot(&load); + pushSlot(&save); + pushSlot(NULL); + if(curcol->size()==0) { + pushSlot(new NullControl("[Empty Collection]")); + } else if(plist::Dictionary * d = dynamic_cast(curcol)) { + for(plist::Dictionary::const_iterator it=d->begin(); it!=d->end(); ++it) { + if(plist::NamedEnumerationBase * neb = dynamic_cast(it->second)) { + pushSlot(new NamedEnumerationEditor(it->first+"="+neb->get(),d->getComment(it->first),*neb)); + } else if(plist::PrimitiveBase * po = dynamic_cast(it->second)) { + pushSlot(new StringInputControl(it->first+"="+po->get(),d->getComment(it->first))); + } else if(dynamic_cast(it->second)) { + ConfigurationEditor * sube = new ConfigurationEditor(it->first,d->getComment(it->first),root); + sube->setPath(path.size()==0 ? it->first : path+plist::Collection::subCollectionSep()+it->first); + pushSlot(sube); + } else { + std::cerr << "WARNING: unknown type for " << it->first << ": " << it->second->toString() << std::endl; + } + } + } else if(plist::ArrayBase * a = dynamic_cast(curcol)) { + for(unsigned int i=0; i!=a->size(); ++i) { + if(plist::NamedEnumerationBase * neb = dynamic_cast(&a->getEntry(i))) { + pushSlot(new NamedEnumerationEditor(neb->get(),a->getComment(i),*neb)); + } else if(plist::PrimitiveBase * po = dynamic_cast(&a->getEntry(i))) { + pushSlot(new StringInputControl(po->get(),a->getComment(i))); + } else if(dynamic_cast(&a->getEntry(i))) { + ConfigurationEditor * sube = new ConfigurationEditor("",a->getComment(i),root); + stringstream ss; + if(path.size()==0) + ss << i; + else + ss << path << plist::Collection::subCollectionSep() << i; + sube->setPath(ss.str()); + pushSlot(sube); + } else { + std::cerr << "WARNING: unknown type for entry " << i << ": " << a->getEntry(i).toString() << std::endl; + } + } + } + ControlBase::refresh(); +} + +void ConfigurationEditor::clearSlots() { + // don't erase load or save controls -- we'll reuse them + for(std::vector::iterator it=options.begin(); it!=options.end(); ++it) + if(*it==&load || *it==&save) + *it=NULL; + ControlBase::clearSlots(); +} + +void ConfigurationEditor::setRootCollection(plist::Collection* rootCollection) { + if(root==rootCollection) + return; + root=rootCollection; + load.setRootCollection(root); + save.setRootCollection(root); + for(unsigned int i=0; i(options[i])) + confe->setRootCollection(root); +} + +void ConfigurationEditor::setPath(const std::string& p) { + path=p; +} + +plist::Dictionary& ConfigurationEditor::getObjectTemplates() { + static plist::Dictionary dict; + if(dict.size()==0) { + //first call, set up + dict.addEntry("float",new plist::Primitive); + dict.addEntry("int",new plist::Primitive); + dict.addEntry("int (unsigned)",new plist::Primitive); + dict.addEntry("string",new plist::Primitive); + dict.addEntry("sub-array",new plist::Array); + dict.addEntry("sub-dictionary",new plist::Dictionary); + } + return dict; +} + +ControlBase* ConfigurationEditor::LoadSettings::selectedFile(const std::string& f) { + if(rootcol==NULL) { + std::cerr << "ERROR: Unable to load, no plist::Collection has been provided" << std::endl; + return NULL; + } + rootcol->loadFile(f.c_str()); + return NULL; +} +ControlBase* ConfigurationEditor::SaveSettings::selectedFile(const std::string& f) { + if(rootcol==NULL) { + std::cerr << "ERROR: Unable to save, no plist::Collection has been provided" << std::endl; + return NULL; + } + rootcol->saveFile(f.c_str()); + return NULL; +} + +void ConfigurationEditor::AddCollectionEntry::refresh() { + options.clear(); + for(plist::Dictionary::const_iterator it=ConfigurationEditor::getObjectTemplates().begin(); it!=ConfigurationEditor::getObjectTemplates().end(); ++it) { + if(tgt->canContain(*it->second)) { + pushSlot(new ConfigurationEditor::NewCollectionEntry(it->first,*tgt,static_cast(*it->second->clone()))); + } + } +} + +void ConfigurationEditor::NewCollectionEntry::refresh() { + std::string key; + if(options.size()>0 && options[0]!=NULL) + key=dynamic_cast(*options[0]).getLastInput(); + options.clear(); + StringInputControl * keyEdit=NULL; + if(dynamic_cast(tgt)) { + keyEdit = new StringInputControl("Key: "+key,"Enter key for the dictionary entry"); + } else if(dynamic_cast(tgt)) { + if(key=="") + key="end"; + keyEdit = new StringInputControl("Position: "+key,"Enter an index position or keyword 'end', optionally followed by '-n' to offset from the end"); + } + keyEdit->takeInput(key); + pushSlot(keyEdit); + if(plist::PrimitiveBase * po = dynamic_cast(obj)) { + pushSlot(new StringInputControl(po->get(),"Enter the intial value for the item")); + } else if(plist::Collection * c = dynamic_cast(obj)) { + ConfigurationEditor * sube = new ConfigurationEditor("sub-entries...","Add entries to the new collection",c); + pushSlot(sube); + } else if(obj!=NULL) { + std::cerr << "WARNING: unknown type for NewCollectionEntry editor" << ": " << obj->toString() << std::endl; + } + pushSlot(NULL); + pushSlot(new NullControl("Commit Entry")); +} + +ControlBase * ConfigurationEditor::NewCollectionEntry::doSelect() { + if(hilights.size()!=1 || hilights.front()!=options.size()-1) + return ControlBase::doSelect(); + + if(plist::Dictionary* d = dynamic_cast(tgt)) { + std::string key = dynamic_cast(*options[0]).getLastInput(); + d->addEntry(key,obj); + } else if(plist::ArrayBase* a = dynamic_cast(tgt)) { + std::string key = dynamic_cast(*options[0]).getLastInput(); + int idx; + if(key=="" || string_util::makeLower(key).substr(0,3)=="end") { + idx=a->size(); + if(key.size()>3) + idx+=atoi(key.substr(3).c_str()); + } else { + idx=atoi(key.c_str()) % (options.size()+1); + if(idx<0) + idx+=options.size()+1; + } + reinterpret_cast*>(a)->addEntry(idx,obj); + } + return NULL; +} + +void ConfigurationEditor::NamedEnumerationEditor::refresh() { + options.clear(); + std::map names; + tgt->getPreferredNames(names); + for(std::map::const_iterator it=names.begin(); it!=names.end(); ++it) + pushSlot(new NullControl(it->second)); + ControlBase::refresh(); + if(gui_comm!=NULL && wireless->isConnected(gui_comm->sock)) { + if(userPrompt.size()>0) + gui_comm->printf("status\n%td\n%s\n",std::count(userPrompt.begin(),userPrompt.end(),'\n'),userPrompt.c_str()); + } +} + +ControlBase * ConfigurationEditor::NamedEnumerationEditor::doSelect() { + if(hilights.size()!=1) + return this; + clearLastInput(); + tgt->set(options[hilights.front()]->getName()); + return NULL; +} + +/*! @file + * @brief + * @author Ethan Tira-Thompson (ejt) (Creator) + * + * $Author: ejt $ + * $Name: tekkotsu-4_0 $ + * $Revision: 1.2 $ + * $State: Exp $ + * $Date: 2007/11/13 04:16:00 $ + */ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/ConfigurationEditor.h ./Behaviors/Controls/ConfigurationEditor.h --- ../Tekkotsu_3.0/Behaviors/Controls/ConfigurationEditor.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Controls/ConfigurationEditor.h 2007-01-30 17:56:18.000000000 -0500 @@ -0,0 +1,171 @@ +//-*-c++-*- +#ifndef INCLUDED_ConfigurationEditor_h +#define INCLUDED_ConfigurationEditor_h + +#include "Behaviors/Controls/ControlBase.h" +#include "Behaviors/Controls/FileInputControl.h" +#include "Behaviors/Controls/StringInputControl.h" + +//! Provides interactive editing and loading/saving of a plist::Collection +class ConfigurationEditor : public ControlBase { + + // **************************** // + // ******* CONSTRUCTORS ******* // + // **************************** // +public: + //! default constructor + ConfigurationEditor(plist::Collection * rootCollection=NULL) + : ControlBase("ConfigurationEditor","Provides interactive editing and loading/saving of a plist::Collection"), + root(rootCollection), path(), load(rootCollection), save(rootCollection) + {init();} + //! constructor which allows a custom name + ConfigurationEditor(const std::string& n, plist::Collection * rootCollection=NULL) + : ControlBase(n,"Provides interactive editing and loading/saving of a plist::Collection"), + root(rootCollection), path(), load(rootCollection), save(rootCollection) + {init();} + //! constructor which allows a custom name and tooltip + ConfigurationEditor(const std::string& n, const std::string& d, plist::Collection * rootCollection=NULL) + : ControlBase(n,d), root(rootCollection), path(), load(rootCollection), save(rootCollection) + {init();} + + //! destructor, have to do our custom clearSlots before the superclass tries to + virtual ~ConfigurationEditor() { clearSlots(); } + +protected: + //! common initialization regardless of constructor + virtual void init(); + + + + // **************************** // + // ********* METHODS ********** // + // **************************** // +public: + //! called when a child has deactivated and this control should refresh its display, or some other event (such as the user pressing the refresh button) has happened to cause a refresh to be needed + virtual void refresh(); + + //! need to override clearing so we don't delete #load and #save + virtual void clearSlots(); + + //! sets the root of the collection being managed + virtual void setRootCollection(plist::Collection* rootCollection); + + //! sets the path to the sub-collection being managed + virtual void setPath(const std::string& p); + + //! returns a dictionary of plist objects which may be added as new entries to collections being edited + static plist::Dictionary& getObjectTemplates(); + + + // **************************** // + // ********* MEMBERS ********** // + // **************************** // +protected: + plist::Collection * root; //!< the root of collection being edited (i.e. may be the parent or other ancestor of the collection this editor is targeting, see #path) + std::string path; //!< the names of the sub-collections from #root to get to the collection this instance is actually targeting + + //! provides a file browser to load a plist from the file system + class LoadSettings : public FileInputControl { + public: + //! constructor, if rootCollection is NULL, you must call setRootCollection() with a non-NULL value at some point before user tries to activate the control + LoadSettings(plist::Collection * rootCollection) : FileInputControl("Load...","Load settings from disk","config"), rootcol(rootCollection) {} + //! assigns #rootcol + virtual void setRootCollection(plist::Collection* rootCollection) { rootcol=rootCollection; } + protected: + virtual ControlBase* selectedFile(const std::string& f); + plist::Collection * rootcol; //!< the collection to be saved (generally the top level of the collection, even if currently editing within a sub-collection) + private: + LoadSettings(const LoadSettings&); //!< not supported + LoadSettings& operator=(const LoadSettings&); //!< not supported + }; + LoadSettings load; //!< reuse the same load instance instead of regenerating a new one each refresh (saves user's "working directory") + + //! provides a file browser to save a plist from the file system + class SaveSettings : public FileInputControl { + public: + //! constructor, if rootCollection is NULL, you must call setRootCollection() with a non-NULL value at some point before user tries to activate the control + SaveSettings(plist::Collection * rootCollection) : FileInputControl("Save...","Save settings to disk","config"), rootcol(rootCollection) { setAcceptNonExistant(true); } + //! assigns #rootcol + virtual void setRootCollection(plist::Collection* rootCollection) { rootcol=rootCollection; } + protected: + virtual ControlBase* selectedFile(const std::string& f); + plist::Collection * rootcol; //!< the collection to be saved (generally the top level of the collection, even if currently editing within a sub-collection) + private: + SaveSettings(const SaveSettings&); //!< not supported + SaveSettings& operator=(const SaveSettings&); //!< not supported + }; + SaveSettings save; //!< reuse the same save instance instead of regenerating a new one each refresh (saves user's "working directory") + + //! displays a list of template objects (see ConfigurationEditor::getObjectTemplates()) which can be added to a target collection + class AddCollectionEntry : public ControlBase { + public: + AddCollectionEntry(plist::Collection& target) : ControlBase("Add New Entry"), tgt(&target) {} + virtual void refresh(); + virtual void deactivate() { clearSlots(); ControlBase::deactivate(); } + protected: + plist::Collection * tgt; + private: + AddCollectionEntry(const AddCollectionEntry&); //!< not supported + AddCollectionEntry& operator=(const AddCollectionEntry&); //!< not supported + }; + + //! provides interface to set up a new collection entry before inserting it + class NewCollectionEntry : public ControlBase { + public: + NewCollectionEntry(const std::string& n, plist::Collection& target, plist::ObjectBase& templ) : ControlBase(n), tgt(&target), obj(&templ) {} + virtual void refresh(); + virtual void deactivate() { clearSlots(); ControlBase::deactivate(); } + virtual ControlBase * doSelect(); + protected: + plist::Collection * tgt; + plist::ObjectBase * obj; + private: + NewCollectionEntry(const NewCollectionEntry&); //!< not supported + NewCollectionEntry& operator=(const NewCollectionEntry&); //!< not supported + }; + + //! based on a string input control for easier mixing with other primitives (which just use a basic string input control as an editor) + /*! If selected, provides a submenu with symbolic options to better prompt the user with valid values */ + class NamedEnumerationEditor : public StringInputControl { + public: + //! constructor + NamedEnumerationEditor(const std::string& n, const std::string& prompt, plist::NamedEnumerationBase& target) : StringInputControl(n,prompt), tgt(&target) {} + virtual void refresh(); + //! purposely clearing slots on deactivate to avoid having sub-menus -- don't want to appear as a collection + virtual void deactivate() { clearSlots(); StringInputControl::deactivate(); } + //! handles selecting one of the symbolic values + virtual ControlBase * doSelect(); + //! override to only allow one hilight (first in the list) + virtual void setHilights(const std::vector& hi) { + if(hi.empty()) + StringInputControl::setHilights(hi); + else // go straight to control base + ControlBase::setHilights(std::vector(1,hi.front())); + } + protected: + plist::NamedEnumerationBase * tgt; //!< the value being edited + private: + NamedEnumerationEditor(const NamedEnumerationEditor&); //!< not supported + NamedEnumerationEditor& operator=(const NamedEnumerationEditor&); //!< not supported + }; + + + // **************************** // + // ********** OTHER *********** // + // **************************** // +private: + ConfigurationEditor(const ConfigurationEditor&); //!< you can override, but don't call this... + ConfigurationEditor& operator=(const ConfigurationEditor&); //!< you can override, but don't call this... +}; + +/*! @file + * @brief Defines ConfigurationEditor, which provides interactive editing and loading/saving of a plist::Collection + * @author Ethan Tira-Thompson (ejt) (Creator) + * + * $Author: ejt $ + * $Name: tekkotsu-4_0 $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2007/01/30 22:56:18 $ + */ +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/ControlBase.cc ./Behaviors/Controls/ControlBase.cc --- ../Tekkotsu_3.0/Behaviors/Controls/ControlBase.cc 2006-09-18 14:07:54.000000000 -0400 +++ ./Behaviors/Controls/ControlBase.cc 2007-01-30 17:56:18.000000000 -0500 @@ -70,14 +70,18 @@ ss << "refresh\n" << getName() << '\n' << options.size() << '\n'; - for(unsigned int i=0; igetDescription(); ss << options[i]->options.size() << '\n' << (binary_search(hilights.begin(),hilights.end(),i)?1:0) << '\n' << options[i]->getName() << '\n' - << options[i]->getDescription().c_str() << '\n'; + << std::count(desc.begin(),desc.end(),'\n') << '\n' + << desc << '\n'; + } + } // do { //cout << "Writing " << ss.str().size() << "..."; gui_comm->write((const byte*)ss.str().c_str(),ss.str().size()); @@ -207,8 +211,10 @@ return this; } else { //Level 3: GUI - if(prompt.size()>0) - gui_comm->printf("status\n%s\n",prompt.c_str()); + if(prompt.size()>0) { + unsigned int lines=std::count(prompt.begin(),prompt.end(),'\n'); + gui_comm->printf("status\n%u\n%s\n",lines,prompt.c_str()); + } return this; } } @@ -262,12 +268,7 @@ clearMenu(); doRewrite=false; } - if(ambiguous) - serr->printf("ControlBase(\"%s\")::takeInput(\"%s\") was ambiguous\n",name.c_str(),args[0].c_str()); - else - serr->printf("ControlBase(\"%s\")::takeInput(\"%s\") was not a valid index or option name.\n",name.c_str(),args[0].c_str()); - refresh(); - return this; + return invalidInput(str,ambiguous); } else if(choiceprintf("ControlBase(\"%s\")::takeInput(\"%s\") was ambiguous\n",name.c_str(),args[0].c_str()); - else - serr->printf("ControlBase(\"%s\")::takeInput(\"%d\") is not a valid selection\n",name.c_str(),choice); - refresh(); - return this; + return invalidInput(str,ambiguous); } } //see what we got... @@ -397,6 +393,15 @@ } } +ControlBase* ControlBase::invalidInput(const std::string& msg, bool ambiguous) { + if(ambiguous) + serr->printf("ControlBase(\"%s\")::takeInput(\"%s\") was ambiguous\n",name.c_str(),msg.c_str()); + else + serr->printf("ControlBase(\"%s\")::takeInput(\"%s\") was not a valid index or option name.\n",name.c_str(),msg.c_str()); + refresh(); + return this; +} + float ControlBase::hilightsAvg() const { if(hilights.size()==0) return -1; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/ControlBase.h ./Behaviors/Controls/ControlBase.h --- ../Tekkotsu_3.0/Behaviors/Controls/ControlBase.h 2004-11-03 22:01:31.000000000 -0500 +++ ./Behaviors/Controls/ControlBase.h 2006-10-30 16:26:23.000000000 -0500 @@ -68,7 +68,7 @@ //! @name Constructors/Destructors ControlBase() : name("(null name)"), description(), hilights(), options(), doRewrite(false), display_id(MotionManager::invalid_MC_ID), gui_comm(NULL) {} //!< Contructor - ControlBase(const std::string& n) : name(n), description(), hilights(), options(), doRewrite(false), display_id(MotionManager::invalid_MC_ID), gui_comm(NULL) {} //!< Contructor, initializes with a name + ControlBase(const std::string& n) : name(n), description((n.size()>16)?n:std::string()), hilights(), options(), doRewrite(false), display_id(MotionManager::invalid_MC_ID), gui_comm(NULL) {} //!< Contructor, initializes with a name ControlBase(const std::string& n, const std::string& d) : name(n), description(d), hilights(), options(), doRewrite(false), display_id(MotionManager::invalid_MC_ID), gui_comm(NULL) {} //!< Contructor, initializes with a name //! Destructor @@ -142,6 +142,11 @@ //! clears the display (if use_VT100 is on) virtual void clearMenu(); + + //! called by takeInput if the input doesn't match any slots or matches multiple slots -- the ControlBase implementation displays an error and returns itself + /*! @param msg the input originally sent to takeInput() + * @param ambiguous true if the input matched more than one item, false if it didn't match any */ + virtual ControlBase* invalidInput(const std::string& msg, bool ambiguous); //! returns the average of the hilighted indicies - used to know to play the "next" sound, or the "prev" sound when the hilight changes float hilightsAvg() const; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/EventLogger.cc ./Behaviors/Controls/EventLogger.cc --- ../Tekkotsu_3.0/Behaviors/Controls/EventLogger.cc 2006-09-18 14:07:54.000000000 -0400 +++ ./Behaviors/Controls/EventLogger.cc 2007-11-11 18:57:18.000000000 -0500 @@ -19,12 +19,14 @@ unsigned int EventLogger::logSocketRefCount=0; int EventLogger::port=10080; EventLogger * EventLogger::theOne=NULL; +EventLogger::queuedEvents_t EventLogger::queuedEvents; +EventLogger::transStack_t EventLogger::transStack; EventLogger::StateMachineListener EventLogger::smProcess; EventLogger::EventLogger() : ControlBase("Event Logger","Allows you to see/log all of the un-trapped events as they are generated"), - logfilePath(), logfile(), verbosity(0), expected(), listen(), queuedEvents() { + logfilePath(), logfile(), verbosity(0), listen() { for(unsigned int i=0; isocket(SocketNS::SOCK_STREAM,1024,1<<15); + logSocket=wireless->socket(Socket::SOCK_STREAM,1024,1<<15); wireless->setDaemon(logSocket); wireless->setReceiver(logSocket, callback); wireless->listen(logSocket,port); @@ -45,7 +47,6 @@ } EventLogger::~EventLogger() { - expected.clear(); while(!queuedEvents.empty()) queuedEvents.pop(); clearSlots(); @@ -104,7 +105,6 @@ if(options[EventBase::numEGIDs+2]->getName()[1]=='X') sout->printf("EVENT: %s\n",logdata.c_str()); if(logSocket!=NULL && wireless->isConnected(logSocket->sock)) { - xmlDoc * doc = xmlNewDoc((const xmlChar*)"1.0"); xmlNode * cur = xmlNewNode(NULL,(const xmlChar*)""); xmlSetProp(cur,(const xmlChar*)"type",(const xmlChar*)"log"); xmlNode * desc = xmlNewNode(NULL,(const xmlChar*)"param"); @@ -113,7 +113,9 @@ xmlSetProp(desc,(const xmlChar*)"name",(const xmlChar*)"description"); xmlSetProp(desc,(const xmlChar*)"value",(const xmlChar*)event.getDescription(true,3).c_str()); xmlBuffer* buf=xmlBufferCreate(); + xmlDoc * doc = xmlNewDoc((const xmlChar*)"1.0"); int n=xmlNodeDump(buf,doc,cur,0,1); + xmlFreeNode(cur); xmlFreeDoc(doc); byte * nbuf = logSocket->getWriteBuffer(n+1); if(nbuf!=NULL) { @@ -125,101 +127,79 @@ } checkLogFile(); if(logfile) - logfile << logdata << endl; + logfile << logdata << std::endl; } void EventLogger::logImage(FilterBankGenerator& fbg, unsigned int layer, unsigned int channel, const BehaviorBase* source/*=NULL*/) { - if(logSocket!=NULL && wireless->isConnected(logSocket->sock)) { + if(logSocket==NULL || !wireless->isConnected(logSocket->sock)) + return; - char * binbuf; - unsigned int len; - if(JPEGGenerator* jpeg=dynamic_cast(&fbg)) { - binbuf=(char*)jpeg->getImage(layer,channel); - len=jpeg->getImageSize(layer,channel); - } else { - fbg.selectSaveImage(layer,channel); - len=fbg.getBinSize(); - binbuf=new char[len]; - fbg.saveBuffer(binbuf,len); - } - string b64buf=base64::encode(binbuf,len); - if(binbuf!=(char*)fbg.getImage(layer,channel)) //cached, should be a simple return - delete [] binbuf; - - xmlDoc * doc = xmlNewDoc((const xmlChar*)"1.0"); - xmlNode * cur = xmlNewNode(NULL,(const xmlChar*)"event"); - xmlSetProp(cur,(const xmlChar*)"type",(const xmlChar*)"image"); - if(source!=NULL) - xmlSetProp(cur,(const xmlChar*)"sid",(const xmlChar*)source->getName().c_str()); - char timebuf[20]; - snprintf(timebuf,20,"%d",get_time()); - xmlSetProp(cur,(const xmlChar*)"time",(const xmlChar*)timebuf); - xmlNewChild(cur,NULL,(const xmlChar*)"image",(const xmlChar*)b64buf.c_str()); - xmlBuffer* buf=xmlBufferCreate(); - int n=xmlNodeDump(buf,doc,cur,0,1); - xmlFreeDoc(doc); - byte * nbuf = logSocket->getWriteBuffer(n+1); - if(nbuf!=NULL) { - memcpy(nbuf,xmlBufferContent(buf),n); - nbuf[n]='\n'; - logSocket->write(n+1); - } - xmlBufferFree(buf); - } + char * binbuf; + unsigned int len; + if(JPEGGenerator* jpeg=dynamic_cast(&fbg)) { + binbuf=(char*)jpeg->getImage(layer,channel); + len=jpeg->getImageSize(layer,channel); + } else { + fbg.selectSaveImage(layer,channel); + len=fbg.getBinSize(); + binbuf=new char[len]; + fbg.saveBuffer(binbuf,len); + } + std::string b64buf=base64::encode(binbuf,len); + if(binbuf!=(char*)fbg.getImage(layer,channel)) //cached, should be a simple return + delete [] binbuf; + + xmlNode * cur = xmlNewNode(NULL,(const xmlChar*)"event"); + xmlSetProp(cur,(const xmlChar*)"type",(const xmlChar*)"image"); + if(source!=NULL) + xmlSetProp(cur,(const xmlChar*)"sid",(const xmlChar*)source->getName().c_str()); + char timebuf[20]; + snprintf(timebuf,20,"%d",get_time()); + xmlSetProp(cur,(const xmlChar*)"time",(const xmlChar*)timebuf); + xmlNewChild(cur,NULL,(const xmlChar*)"image",(const xmlChar*)b64buf.c_str()); + queuedEvents.push(cur); + if(transStack.empty()) + dumpQueuedEvents(); } void EventLogger::logMessage(std::string msg, const BehaviorBase* source/*=NULL*/, const char* icon/*=NULL*/, unsigned int placement/*=0*/) { - if(logSocket!=NULL && wireless->isConnected(logSocket->sock)) { - xmlDoc * doc = xmlNewDoc((const xmlChar*)"1.0"); - xmlNode * cur = xmlNewNode(NULL,(const xmlChar*)"event"); - xmlSetProp(cur,(const xmlChar*)"type",(const xmlChar*)"userlog"); - if(source!=NULL) - xmlSetProp(cur,(const xmlChar*)"sid",(const xmlChar*)source->getName().c_str()); - if(icon!=NULL) - xmlSetProp(cur,(const xmlChar*)"icon",(const xmlChar*)icon); - const unsigned int len=20; - char sbuf[len]; - snprintf(sbuf,len,"%d",placement); - xmlSetProp(cur,(const xmlChar*)"voff",(const xmlChar*)sbuf); - snprintf(sbuf,len,"%d",get_time()); - xmlSetProp(cur,(const xmlChar*)"time",(const xmlChar*)sbuf); - xmlNodeSetContent(cur,(const xmlChar*)msg.c_str()); - xmlBuffer* buf=xmlBufferCreate(); - int n=xmlNodeDump(buf,doc,cur,0,1); - xmlFreeDoc(doc); - byte * nbuf = logSocket->getWriteBuffer(n+1); - if(nbuf!=NULL) { - memcpy(nbuf,xmlBufferContent(buf),n); - nbuf[n]='\n'; - logSocket->write(n+1); - } - xmlBufferFree(buf); - } + if(logSocket==NULL || !wireless->isConnected(logSocket->sock)) + return; + + xmlNode * cur = xmlNewNode(NULL,(const xmlChar*)"event"); + xmlSetProp(cur,(const xmlChar*)"type",(const xmlChar*)"userlog"); + if(source!=NULL) + xmlSetProp(cur,(const xmlChar*)"sid",(const xmlChar*)source->getName().c_str()); + if(icon!=NULL) + xmlSetProp(cur,(const xmlChar*)"icon",(const xmlChar*)icon); + const unsigned int len=20; + char sbuf[len]; + snprintf(sbuf,len,"%d",placement); + xmlSetProp(cur,(const xmlChar*)"voff",(const xmlChar*)sbuf); + snprintf(sbuf,len,"%d",get_time()); + xmlSetProp(cur,(const xmlChar*)"time",(const xmlChar*)sbuf); + xmlNodeSetContent(cur,(const xmlChar*)msg.c_str()); + queuedEvents.push(cur); + if(transStack.empty()) + dumpQueuedEvents(); } void EventLogger::logWebcam(const BehaviorBase* source/*=NULL*/) { - if(logSocket!=NULL && wireless->isConnected(logSocket->sock)) { - xmlDoc * doc = xmlNewDoc((const xmlChar*)"1.0"); - xmlNode * cur = xmlNewNode(NULL,(const xmlChar*)"event"); - xmlSetProp(cur,(const xmlChar*)"type",(const xmlChar*)"webcam"); - if(source!=NULL) - xmlSetProp(cur,(const xmlChar*)"sid",(const xmlChar*)source->getName().c_str()); - const unsigned int len=20; - char sbuf[len]; - snprintf(sbuf,len,"%d",get_time()); - xmlSetProp(cur,(const xmlChar*)"time",(const xmlChar*)sbuf); - xmlNodeSetContent(cur,(const xmlChar*)" "); - xmlBuffer* buf=xmlBufferCreate(); - int n=xmlNodeDump(buf,doc,cur,0,1); - xmlFreeDoc(doc); - byte * nbuf = logSocket->getWriteBuffer(n+1); - if(nbuf!=NULL) { - memcpy(nbuf,xmlBufferContent(buf),n); - nbuf[n]='\n'; - logSocket->write(n+1); - } - xmlBufferFree(buf); - } + if(logSocket==NULL || !wireless->isConnected(logSocket->sock)) + return; + + xmlNode * cur = xmlNewNode(NULL,(const xmlChar*)"event"); + xmlSetProp(cur,(const xmlChar*)"type",(const xmlChar*)"webcam"); + if(source!=NULL) + xmlSetProp(cur,(const xmlChar*)"sid",(const xmlChar*)source->getName().c_str()); + const unsigned int len=20; + char sbuf[len]; + snprintf(sbuf,len,"%d",get_time()); + xmlSetProp(cur,(const xmlChar*)"time",(const xmlChar*)sbuf); + xmlNodeSetContent(cur,(const xmlChar*)" "); + queuedEvents.push(cur); + if(transStack.empty()) + dumpQueuedEvents(); } void EventLogger::clearSlots() { @@ -255,50 +235,53 @@ } -void EventLogger::spider(const StateNode* n, unsigned int depth/*=0*/) { +void EventLogger::spider(const StateNode* n, xmlNode* parent/*=NULL*/) { if(n==NULL) return; + xmlNode * cur = xmlNewNode(NULL,(const xmlChar*)"state"); + ASSERTRET(cur!=NULL,"EventLogger::spider() could not allocate new xml state node"); + xmlSetProp(cur,(const xmlChar*)"class",(const xmlChar*)n->getClassName().c_str()); + xmlSetProp(cur,(const xmlChar*)"id",(const xmlChar*)n->getName().c_str()); + const std::vector& subnodes=n->getNodes(); - if(subnodes.size()==0) { - // it's a leaf node, no subnodes or transitions between them - indent(depth); - logSocket->printf("\n", n->getClassName().c_str(), n->getName().c_str()); - } else { - - // first output current node's info - indent(depth); - logSocket->printf("\n", n->getClassName().c_str(), n->getName().c_str()); + if(subnodes.size()>0) { + // it's not a leaf node, has subnodes and transitions between them std::set transitions; // now recurse on sub-nodes, extracting all of the subnodes transitions for(unsigned int i=0; i& curt=subnodes[i]->getTransitions(); transitions.insert(curt.begin(),curt.end()); } // now output transitions between subnodes we collected in previous step for(std::set::const_iterator it=transitions.begin(); it!=transitions.end(); it++) { - indent(depth+1); - logSocket->printf("\n", (*it)->getClassName().c_str(), (*it)->getName().c_str()); - const std::vector& incoming=(*it)->getSources(); - for(unsigned int i=0; iprintf("%s\n",incoming[i]->getName().c_str()); + xmlNode * t = xmlAddChild(cur,xmlNewNode(NULL,(const xmlChar*)"transition")); + ASSERTRET(t!=NULL,"EventLogger::spider() could not allocate new xml transition node"); + xmlSetProp(t,(const xmlChar*)"class",(const xmlChar*)(*it)->getClassName().c_str()); + xmlSetProp(t,(const xmlChar*)"id",(const xmlChar*)(*it)->getName().c_str()); + + typedef std::vector statevec_t; + const statevec_t& incoming=(*it)->getSources(); + for(statevec_t::const_iterator nit=incoming.begin(); nit!=incoming.end(); ++nit) { + xmlNode * sn = xmlAddChild(t,xmlNewNode(NULL,(const xmlChar*)"source")); + ASSERTRET(sn!=NULL,"EventLogger::spider() could not allocate new xml transition source node"); + xmlNodeSetContent(sn,(const xmlChar*)(*nit)->getName().c_str()); } - const std::vector& outgoing=(*it)->getDestinations(); - for(unsigned int i=0; iprintf("%s\n",outgoing[i]->getName().c_str()); + const statevec_t& outgoing=(*it)->getDestinations(); + for(statevec_t::const_iterator nit=outgoing.begin(); nit!=outgoing.end(); ++nit) { + xmlNode * sn = xmlAddChild(t,xmlNewNode(NULL,(const xmlChar*)"destination")); + ASSERTRET(sn!=NULL,"EventLogger::spider() could not allocate new xml transition source node"); + xmlNodeSetContent(sn,(const xmlChar*)(*nit)->getName().c_str()); } - indent(depth+1); - logSocket->printf("\n"); } - - indent(depth); - logSocket->printf("\n"); } + if(parent==NULL) + dumpNode(cur); + else + xmlAddChild(parent,cur); } bool EventLogger::isListening(const StateNode* n) { @@ -389,13 +372,13 @@ return 0; } -void EventLogger::processStateMachineEvent(const EventBase& event) { +void EventLogger::processStateMachineEvent(const EventBase& e) { if(!wireless->isConnected(logSocket->sock) || listen.size()==0) return; - - if(event.getGeneratorID()==EventBase::stateTransitionEGID) { + + if(e.getGeneratorID()==EventBase::stateTransitionEGID) { bool care=false; - const Transition * trans = reinterpret_cast(event.getSourceID()); + const Transition * trans = reinterpret_cast(e.getSourceID()); const std::vector& incoming=trans->getSources(); const std::vector& outgoing=trans->getDestinations(); for(std::vector::const_iterator it=incoming.begin(); it!=incoming.end() && !care; it++) @@ -404,62 +387,75 @@ care=isListening(*it); if(!care) return; - - if(expected.size()!=0) { - queuedEvents.push(event); + + if(e.getTypeID()==EventBase::activateETID) { + xmlNode * root = xmlNewNode(NULL,(const xmlChar*)"event"); + xmlNode * fire = xmlAddChild(root,xmlNewNode(NULL,(const xmlChar*)"fire")); + xmlSetProp(fire,(const xmlChar*)"id",(const xmlChar*)trans->getName().c_str()); + const unsigned int len=20; + char sbuf[len]; + snprintf(sbuf,len,"%d",e.getTimeStamp()); + xmlSetProp(fire,(const xmlChar*)"time",(const xmlChar*)sbuf); + transStack.push(root); + queuedEvents.push(root); } else { - logSocket->printf("\n"); - indent(1); - logSocket->printf("\n",trans->getName().c_str(),event.getTimeStamp()); - expected.insert(incoming.begin(),incoming.end()); - expected.insert(outgoing.begin(),outgoing.end()); - while(queuedEvents.size()>0) { - EventBase qe=queuedEvents.front(); - queuedEvents.pop(); - processEvent(qe); - } + ASSERTRET(!transStack.empty(),"got a transition deactivate that I should care about, but I didn't see the activate"); + transStack.pop(); + if(transStack.empty()) + dumpQueuedEvents(); } - - } else if(event.getGeneratorID()==EventBase::stateMachineEGID) { - if(event.getTypeID()==EventBase::statusETID) + + } else if(e.getGeneratorID()==EventBase::stateMachineEGID) { + if(e.getTypeID()==EventBase::statusETID) + return; + const StateNode * beh=reinterpret_cast(e.getSourceID()); + if(!isListening(beh)) + return; + if(e.getTypeID()!=EventBase::activateETID && e.getTypeID()!=EventBase::deactivateETID) { + serr->printf("WARNING: Unrecognized TypeID %d\n",e.getTypeID()); return; - const StateNode * beh=reinterpret_cast(event.getSourceID()); - expected_t::iterator it=expected.find(beh); - char * format; - if(isListening(beh)) { - if(it==expected.end()) { //if not found - if(queuedEvents.size()==0) - format="\n"; // unexpected - else { - queuedEvents.push(event); - return; - } - } else - format=" \n"; // expected as part of transition - if(event.getTypeID()==EventBase::activateETID) - logSocket->printf(format,"start",beh->getName().c_str(),event.getTimeStamp()); - else if(event.getTypeID()==EventBase::deactivateETID) - logSocket->printf(format,"stop",beh->getName().c_str(),event.getTimeStamp()); - else - serr->printf("WARNING: Unrecognized TypeID %d\n",event.getTypeID()); } - if(it!=expected.end()) { //was found - expected.erase(it); - if(expected.size()==0) { - logSocket->printf("\n"); - while(queuedEvents.size()>0) { - EventBase qe=queuedEvents.front(); - queuedEvents.pop(); - processEvent(qe); - } - } + + xmlNode * root = transStack.empty() ? xmlNewNode(NULL,(const xmlChar*)"event") : transStack.top(); + const char* sttypestr = (e.getTypeID()==EventBase::activateETID) ? "statestart" : "statestop"; + xmlNode * st = xmlAddChild(root,xmlNewNode(NULL,(const xmlChar*)sttypestr)); + xmlSetProp(st,(const xmlChar*)"id",(const xmlChar*)beh->getName().c_str()); + const unsigned int len=20; + char sbuf[len]; + snprintf(sbuf,len,"%d",e.getTimeStamp()); + xmlSetProp(st,(const xmlChar*)"time",(const xmlChar*)sbuf); + + if(transStack.empty()) { + queuedEvents.push(root); + dumpQueuedEvents(); } - + } else { - serr->printf("WARNING: Unknown event %s (%s)\n",event.getName().c_str(),event.getDescription().c_str()); + serr->printf("WARNING: Unknown event %s (%s)\n",e.getName().c_str(),e.getDescription().c_str()); } } +void EventLogger::dumpQueuedEvents() { + xmlDoc * doc = xmlNewDoc((const xmlChar*)"1.0"); + while(!queuedEvents.empty()) { + dumpNode(queuedEvents.front(),doc); + queuedEvents.pop(); + } + xmlFreeDoc(doc); +} + +void EventLogger::dumpNode(xmlNode* node, xmlDoc* doc/*=NULL*/) { + xmlBuffer* buf=xmlBufferCreate(); + int n=xmlNodeDump(buf,doc,node,0,1); + byte * nbuf = logSocket->getWriteBuffer(n+1); + if(nbuf!=NULL) { + memcpy(nbuf,xmlBufferContent(buf),n); + nbuf[n]='\n'; + logSocket->write(n+1); + } + xmlBufferFree(buf); + xmlFreeNode(node); +} diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/EventLogger.h ./Behaviors/Controls/EventLogger.h --- ../Tekkotsu_3.0/Behaviors/Controls/EventLogger.h 2006-09-16 02:28:06.000000000 -0400 +++ ./Behaviors/Controls/EventLogger.h 2007-03-02 12:20:48.000000000 -0500 @@ -7,12 +7,73 @@ #include #include #include +#include class FilterBankGenerator; class BehaviorBase; class StateNode; -//! allows logging of events to the console or a file +//! allows logging of events to the console or a file, also provides some remote logging facilities over #logSocket, required by Storyboard tool +/*! Users' behaviors can call logMessage(), logImage(), and logWebcam() to insert the corresponding data into #logSocket via an XML 'event' node. + * + * The protocol used with #logSocket is: + * - 'list' - send list of all instantiated StateNodes + * - 'spider name' - spider the current structure of StateNode named name + * - 'listen name' - send updates regarding the activation status of name and its subnodes; you can specify a state which is not yet running + * - 'ignore name' - cancels a previous listen command + * - 'clear' - cancels all previous listen commands; should be called at the beginning or end of each connection, preferably both + * + * Each of those commands should be terminated with a newline - + * i.e. one command per line + * + * After a list command, the first line will be the number + * of StateNodes, followed by that number of lines, one StateNode + * name per line. + * + * After a spider command, an XML description of the model + * will be sent. If no matching StateNode is found, an warning will + * be displayed on #serr, and an empty model + * ("") returned over the network + * connection. + * + * All other commands give no direct response - listen can be + * executed before the specified StateNode is yet running, and ignore + * doesn't care whether or not the specified StateNode was actually + * being listened for. + * + * The format of the model is: + @verbatim + + + + + + + + + + + ]>@endverbatim + * + * The format of status updates following a listen command is: + @verbatim + + + + + + + + + + + + ]>@endverbatim + * + * The 'event' node is also used for the results of logImage(), logMessage(), and logWebcam(). +*/ class EventLogger : public ControlBase, public EventListener { public: //!constructor @@ -70,7 +131,8 @@ void checkLogFile(); //! dumps all of the transitions and subnodes of a given statenode - void spider(const StateNode* n, unsigned int depth=0); + /*! if parent is NULL, will dump the results over #logSocket, otherwise adds the xml tree as a child of @a parent */ + void spider(const StateNode* n, xmlNode* parent=NULL); //! returns true iff @a n or one of its parents is found in #listen bool isListening(const StateNode* n); @@ -88,6 +150,13 @@ /*!this is called by the StateMachineListener, which is subscribed to only * those machines which have been requested by the remote monitor */ virtual void processStateMachineEvent(const EventBase& event); + + //! dumps elements of #queuedEvents over #logSocket, popping and freeing as it goes + static void dumpQueuedEvents(); + + //! writes an xmlNode out over #logSocket, freeing @a node when complete + /*! uses @a doc if provided, otherwise makes a new temporary one which is then deleted again before the function returns */ + static void dumpNode(xmlNode* node, xmlDoc* doc=NULL); //!address of the logfile, if any (empty string is no logfile) std::string logfilePath; @@ -109,14 +178,14 @@ typedef std::set registry_t; //!< the type of the behavior registry (BehaviorBase::registry) - typedef std::multiset expected_t; //!< the type of #expected - expected_t expected; //!< a set of behaviors which are involved with an impending transition - their next stateMachineEGID event should be ignored - typedef std::set listen_t; //!< the type of #listen listen_t listen; //!< a set of state machine names which should have their subnodes monitored - typedef std::queue queuedEvents_t; //!< the type of #queuedEvents - queuedEvents_t queuedEvents; //!< used if a transition causes other transitions, those transitions need to be remembered + typedef std::queue queuedEvents_t; //!< the type of #queuedEvents + static queuedEvents_t queuedEvents; //!< if logImage/logMessage/etc. are called during a transition, need to queue them until the transition event is complete + + typedef std::stack transStack_t; //!< the type of #transStack + static transStack_t transStack; //!< if another transition occurs during the processing of another, have to recurse on processing the new transition first }; /*! @file diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/FileBrowserControl.cc ./Behaviors/Controls/FileBrowserControl.cc --- ../Tekkotsu_3.0/Behaviors/Controls/FileBrowserControl.cc 2005-02-02 13:20:27.000000000 -0500 +++ ./Behaviors/Controls/FileBrowserControl.cc 2007-01-28 20:16:57.000000000 -0500 @@ -1,4 +1,6 @@ #include "FileBrowserControl.h" +#include "NullControl.h" +#include "Shared/Config.h" #include #include #include @@ -39,25 +41,49 @@ } ControlBase * FileBrowserControl::takeInput(const std::string& msg) { - if(options.size()==1 && options.front()==NULL) + if(msg.size()==0) + return this; + if(msg.find('/')==string::npos) { + if(options.size()==1 && options.front()==NULL) + rebuildmenu(); + return ControlBase::takeInput(msg); + } else { + string::size_type pos=msg.rfind('/'); + if(msg[0]=='/') { + if(msg.substr(0,root.size())!=root) + return this; + if(msg.size()>root.size() && msg[root.size()]!='/') + return this; + paths.clear(); + if(pos<=root.size()) + return this; + appendPath(msg.substr(root.size(),pos-root.size())); + } else { + appendPath(msg.substr(0,pos)); + } rebuildmenu(); - return ControlBase::takeInput(msg); + if(msg.size()>pos+1) + return ControlBase::takeInput(msg.substr(pos+1)); + return this; + } } void FileBrowserControl::setRoot(const std::string& path) { - root=path; + root=config->portPath(path); if(root[root.size()-1]=='/') root.erase(root.size()-1); paths.clear(); } -void FileBrowserControl::setPath(const std::string& path) { - for(unsigned int i=0; id_name).append(1,'/'))); + pushSlot(new NullControl(std::string(ent->d_name).append(1,'/'),makePath(ent->d_name),this)); } else { std::string nm=(makePath(ent->d_name)); if(match(nm,filter)) - pushSlot(new ControlBase(ent->d_name)); + pushSlot(new NullControl(ent->d_name,makePath(ent->d_name),this)); } } ent=readdir(dir); } closedir(dir); if(options.size()==0) - pushSlot(new ControlBase("[empty directory]")); + pushSlot(new NullControl("[empty directory]",makePath(),this)); else { hilights.push_back(0); /* for(unsigned int i=0; iportPath(config->motion.root)), cp(calp) + : FileBrowserControl("Load Calibration...","",config->motion.root), cp(calp) { setFilter("*.txt"); } //!Constructor LoadCalibration(const std::string& n,WalkMC::CalibrationParam* calp) - : FileBrowserControl(n,"",config->portPath(config->motion.root)), cp(calp) + : FileBrowserControl(n,"",config->motion.root), cp(calp) { setFilter("*.txt"); } //!Constructor LoadCalibration(const std::string& n, const std::string& d,WalkMC::CalibrationParam* calp) - : FileBrowserControl(n,d,config->portPath(config->motion.root)), cp(calp) + : FileBrowserControl(n,d,config->motion.root), cp(calp) { setFilter("*.txt"); } protected: diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/LoadPostureControl.h ./Behaviors/Controls/LoadPostureControl.h --- ../Tekkotsu_3.0/Behaviors/Controls/LoadPostureControl.h 2006-09-18 14:07:54.000000000 -0400 +++ ./Behaviors/Controls/LoadPostureControl.h 2007-08-05 12:16:04.000000000 -0400 @@ -7,7 +7,9 @@ #include "Motion/MMAccessor.h" #include "Motion/EmergencyStopMC.h" #include "Events/EventRouter.h" -#include "Motion/LedMC.h" +#ifdef TGT_HAS_LEDS +# include "Motion/LedMC.h" +#endif #include "Sound/SoundManager.h" #include @@ -17,7 +19,7 @@ public: //! Constructor LoadPostureControl(const std::string& n, MotionManager::MC_ID estop_id) - : FileBrowserControl(n,"Loads a posture from user-selected file",config->portPath(config->motion.root)), + : FileBrowserControl(n,"Loads a posture from user-selected file",config->motion.root), estopid(estop_id), ledid(MotionManager::invalid_MC_ID), file() { setFilter("*.pos"); @@ -27,6 +29,7 @@ virtual ~LoadPostureControl() { erouter->removeListener(this); motman->removeMotion(ledid); + ledid=MotionManager::invalid_MC_ID; } //! only called when e-stop has been turned off and we're waiting to load a file @@ -55,11 +58,19 @@ } else { //we have to wait for the estop to be turned off sndman->playFile("donkey.wav"); +#ifdef TGT_HAS_LEDS SharedObject led; led->cset(FaceLEDMask,0); - led->cycle(BotLLEDMask,1000,3,0,0); - led->cycle(BotRLEDMask,1000,3,0,500); + unsigned int botl = capabilities.findButtonOffset(ERS210Info::buttonNames[ERS210Info::BotLLEDOffset]); + unsigned int botr = capabilities.findButtonOffset(ERS210Info::buttonNames[ERS210Info::BotRLEDOffset]); + if(botl==-1U || botr==-1U) { + botl=LEDOffset; + botr=NumLEDs>1 ? botl+1 : botl; + } + led->cycle(1<<(botl-LEDOffset),1000,3,0,0); + led->cycle(1<<(botr-LEDOffset),1000,3,0,500); ledid=motman->addPersistentMotion(led); +#endif erouter->addListener(this,EventBase::estopEGID,estopid,EventBase::deactivateETID); } return this; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/LoadWalkControl.h ./Behaviors/Controls/LoadWalkControl.h --- ../Tekkotsu_3.0/Behaviors/Controls/LoadWalkControl.h 2006-09-09 00:32:17.000000000 -0400 +++ ./Behaviors/Controls/LoadWalkControl.h 2007-01-28 20:16:57.000000000 -0500 @@ -12,7 +12,7 @@ public: //! constructor, pass the MC_ID of the WalkMC which you want to save LoadWalkControl(const std::string& n, MotionManager::MC_ID w) - : FileBrowserControl(n,"Loads a set of walk parameters from a file specified by user",config->portPath(config->motion.root)), walk_id(w), thewalk(NULL) + : FileBrowserControl(n,"Loads a set of walk parameters from a file specified by user",config->motion.root), walk_id(w), thewalk(NULL) { setFilter("*.prm"); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/NetworkStatusControl.h ./Behaviors/Controls/NetworkStatusControl.h --- ../Tekkotsu_3.0/Behaviors/Controls/NetworkStatusControl.h 2005-10-25 17:25:33.000000000 -0400 +++ ./Behaviors/Controls/NetworkStatusControl.h 2006-11-22 00:19:21.000000000 -0500 @@ -24,7 +24,7 @@ //! stops listening for power events and sets display to invalid virtual void pause() { - erouter->removeListener(this); + erouter->remove(this); ControlBase::pause(); } //! calls report() @@ -60,7 +60,7 @@ ControlBase::refresh(); } virtual void deactivate() { - erouter->removeListener(this); + erouter->remove(this); ControlBase::deactivate(); } //! refresh the control whenever an event is received diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/NullControl.h ./Behaviors/Controls/NullControl.h --- ../Tekkotsu_3.0/Behaviors/Controls/NullControl.h 2003-09-25 11:26:11.000000000 -0400 +++ ./Behaviors/Controls/NullControl.h 2006-10-30 16:57:08.000000000 -0500 @@ -9,12 +9,15 @@ public: //!Constructor - NullControl() : ControlBase() {} + explicit NullControl(ControlBase * inputRedirectTgt=NULL) : ControlBase(), inputRedirect(inputRedirectTgt) {} //!Constructor - NullControl(const std::string& n) : ControlBase(n) {} + explicit NullControl(const std::string& n, ControlBase * inputRedirectTgt=NULL) : ControlBase(n), inputRedirect(inputRedirectTgt) {} //!Constructor - NullControl(const std::string& n, const std::string& d) : ControlBase(n,d) {} + NullControl(const std::string& n, const std::string& d, ControlBase * inputRedirectTgt=NULL) : ControlBase(n,d), inputRedirect(inputRedirectTgt) {} + virtual void setInputRedirect(ControlBase* inputRedirectTgt) { inputRedirect=inputRedirectTgt; } //!< sets #inputRedirect + virtual ControlBase* getInputRedirect() const { return inputRedirect; } //!< returns #inputRedirect + //@{ //! returns NULL virtual ControlBase * activate(MotionManager::MC_ID , Socket * ) { return NULL; } @@ -23,8 +26,20 @@ virtual ControlBase * doNextItem() { return NULL; } virtual ControlBase * doPrevItem() { return NULL; } virtual ControlBase * doReadStdIn(const std::string& /*prompt*/=std::string()) { return NULL; } - virtual ControlBase * takeInput(const std::string& /*msg*/) { return NULL; } //@} + + //! returns NULL unless #inputRedirect is set, in which case it will return inputRedirect->takeInput(msg) + virtual ControlBase * takeInput(const std::string& msg) { return (inputRedirect==NULL) ? NULL : inputRedirect->takeInput(msg); } + +protected: + //! the target to receiving forwarding of any calls to takeInput() + /*! this is handy if this instance is some feedback to the user, and any input they + * enter with this control selected only makes sense to be handled by the parent */ + ControlBase* inputRedirect; + +private: + NullControl(const NullControl&); //!< you can override, but don't call this... + NullControl& operator=(const NullControl&); //!< you can override, but don't call this... }; /*! @file diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/PlaySoundControl.h ./Behaviors/Controls/PlaySoundControl.h --- ../Tekkotsu_3.0/Behaviors/Controls/PlaySoundControl.h 2006-09-18 14:07:54.000000000 -0400 +++ ./Behaviors/Controls/PlaySoundControl.h 2007-01-28 20:16:57.000000000 -0500 @@ -10,7 +10,7 @@ public: //! Constructor PlaySoundControl(const std::string& n) - : FileBrowserControl(n,"Plays a sound from a user specified sound file",config->portPath(config->sound.root)) + : FileBrowserControl(n,"Plays a sound from a user specified sound file",config->sound.root) { setFilter("*.wav"); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/PostureEditor.cc ./Behaviors/Controls/PostureEditor.cc --- ../Tekkotsu_3.0/Behaviors/Controls/PostureEditor.cc 2006-09-09 00:32:18.000000000 -0400 +++ ./Behaviors/Controls/PostureEditor.cc 2007-11-18 01:47:00.000000000 -0500 @@ -48,9 +48,11 @@ // start off with current pose pose.takeSnapshot(); pose.setWeights(1); +#ifdef TGT_HAS_LEDS // clear the LEDs though for(unsigned int i=LEDOffset; i reach; reachID=motman->addPersistentMotion(reach); @@ -124,11 +126,15 @@ } } } else if(e.getGeneratorID()==EventBase::timerEGID) { +#ifndef TGT_HAS_LEDS + pose.takeSnapshot(); +#else //doing a manual copy instead of just takeSnapshot() so we don't disturb the LED settings for(unsigned int i=0; ioutputs[i]; for(unsigned int i=LEDOffset+NumLEDs; ioutputs[i]; +#endif if(e.getSourceID()==0) // source==1 indicates it's a forged event sent from refresh -- don't inf. recurse refresh(); } else { diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/RunSequenceControl.h ./Behaviors/Controls/RunSequenceControl.h --- ../Tekkotsu_3.0/Behaviors/Controls/RunSequenceControl.h 2006-09-18 14:07:54.000000000 -0400 +++ ./Behaviors/Controls/RunSequenceControl.h 2007-08-05 12:16:04.000000000 -0400 @@ -5,7 +5,12 @@ #include "FileBrowserControl.h" #include "Motion/MotionSequenceMC.h" #include "Motion/EmergencyStopMC.h" -#include "Motion/LedMC.h" +#include "Shared/RobotInfo.h" +#ifdef TGT_HAS_LEDS +# include "Shared/ERS210Info.h" +# include "Motion/LedMC.h" +#endif +#include "Events/EventRouter.h" #include "Sound/SoundManager.h" #include "Motion/MMAccessor.h" #include "Shared/TimeET.h" @@ -28,7 +33,7 @@ public: //! Constructor, sets filter to *.mot RunSequenceControl(const std::string& n, MotionManager::MC_ID estop_id) - : FileBrowserControl(n,"Runs a motion sequence from a user-specified file",config->portPath(config->motion.root)), + : FileBrowserControl(n,"Runs a motion sequence from a user-specified file",config->motion.root), estopid(estop_id), ledid(MotionManager::invalid_MC_ID), waitingFile() { setFilter("*.mot"); @@ -38,6 +43,7 @@ virtual ~RunSequenceControl() { erouter->removeListener(this); motman->removeMotion(ledid); + ledid=MotionManager::invalid_MC_ID; } //! only called when e-stop has been turned off and we're waiting to load a file @@ -45,6 +51,7 @@ erouter->removeListener(this); runFile(); motman->removeMotion(ledid); + ledid=MotionManager::invalid_MC_ID; } protected: @@ -65,11 +72,19 @@ } else { //we have to wait for the estop to be turned off sndman->playFile("donkey.wav"); +#ifdef TGT_HAS_LEDS SharedObject led; led->cset(FaceLEDMask,0); - led->cycle(BotLLEDMask,1000,3,0,0); - led->cycle(BotRLEDMask,1000,3,0,500); + unsigned int botl = capabilities.findButtonOffset(ERS210Info::buttonNames[ERS210Info::BotLLEDOffset]); + unsigned int botr = capabilities.findButtonOffset(ERS210Info::buttonNames[ERS210Info::BotRLEDOffset]); + if(botl==-1U || botr==-1U) { + botl=LEDOffset; + botr=NumLEDs>1 ? botl+1 : botl; + } + led->cycle(1<<(botl-LEDOffset),1000,3,0,0); + led->cycle(1<<(botr-LEDOffset),1000,3,0,500); ledid=motman->addPersistentMotion(led); +#endif erouter->addListener(this,EventBase::estopEGID,estopid,EventBase::deactivateETID); } return this; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/SensorObserverControl.cc ./Behaviors/Controls/SensorObserverControl.cc --- ../Tekkotsu_3.0/Behaviors/Controls/SensorObserverControl.cc 2006-10-02 18:11:38.000000000 -0400 +++ ./Behaviors/Controls/SensorObserverControl.cc 2007-06-05 14:04:36.000000000 -0400 @@ -65,7 +65,7 @@ sndman->playFile(config->controller.select_snd); if(wasListening!=(numListeners>0)) { if(numListeners>0) - erouter->addListener(this,EventBase::sensorEGID,SensorSourceID::UpdatedSID); + erouter->addListener(this,EventBase::sensorEGID,SensorSrcID::UpdatedSID); else erouter->removeListener(this); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/ShutdownControl.cc ./Behaviors/Controls/ShutdownControl.cc --- ../Tekkotsu_3.0/Behaviors/Controls/ShutdownControl.cc 2005-08-31 17:55:12.000000000 -0400 +++ ./Behaviors/Controls/ShutdownControl.cc 2007-06-13 14:14:12.000000000 -0400 @@ -2,7 +2,7 @@ #ifdef PLATFORM_APERIOS # include #else -# include "local/sim/SharedGlobals.h" +# include "Shared/ProjectInterface.h" #endif ControlBase * ShutdownControl::doSelect() { @@ -10,8 +10,7 @@ OBootCondition bc(0); OPENR::Shutdown(bc); #else - if(globals!=NULL) - globals->signalShutdown(); + ProjectInterface::sendCommand("quit"); #endif return NULL; } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/SimulatorAdvanceFrameControl.h ./Behaviors/Controls/SimulatorAdvanceFrameControl.h --- ../Tekkotsu_3.0/Behaviors/Controls/SimulatorAdvanceFrameControl.h 2006-08-30 10:26:44.000000000 -0400 +++ ./Behaviors/Controls/SimulatorAdvanceFrameControl.h 2007-11-10 17:58:05.000000000 -0500 @@ -4,18 +4,13 @@ #include "Behaviors/Controls/NullControl.h" #ifdef PLATFORM_APERIOS -# warning SimulatorAdvanceFrameControl is only useful when running in simulation! +# warning SimulatorAdvanceFrameControl is available on Aperios! #else -# include "local/sim/Simulator.h" +# include "Shared/ProjectInterface.h" #endif //! Requests the next camera frame and sensor data, for use when running in simulation -/*! There are a number of options regarding control of input data and - * flow of time. This is only applicable when using a non-realtime - * source (such as loading logged data from disk) and the simulator's - * AdvanceOnAccess is 'false' (this is "advance on request" -- the - * same data may be accessed multiple times until you manually - * request the next frame) */ +/*! Note that this won't increment the simulator time if triggered while paused... */ class SimulatorAdvanceFrameControl : public NullControl { public: @@ -32,7 +27,7 @@ #ifndef PLATFORM_APERIOS virtual ControlBase * activate(MotionManager::MC_ID disp_id, Socket * gui) { - Simulator::sendCommand("advance"); + ProjectInterface::sendCommand("advance"); return NullControl::activate(disp_id,gui); } @@ -49,6 +44,7 @@ } protected: + //! ideally, this should return true only when the simulator is paused or the data source is frozen... bool canManuallyAdvance() const { return true; } #endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/StringInputControl.cc ./Behaviors/Controls/StringInputControl.cc --- ../Tekkotsu_3.0/Behaviors/Controls/StringInputControl.cc 2003-07-28 01:54:32.000000000 -0400 +++ ./Behaviors/Controls/StringInputControl.cc 2007-01-30 17:56:18.000000000 -0500 @@ -27,6 +27,7 @@ << "0\n" << "0\n" << "Waiting for input...\n" + << std::count(userPrompt.begin(),userPrompt.end(),'\n') << '\n' << userPrompt << "\n"; gui_comm->write((const byte*)ss.str().c_str(),ss.str().size()); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/StringInputControl.h ./Behaviors/Controls/StringInputControl.h --- ../Tekkotsu_3.0/Behaviors/Controls/StringInputControl.h 2004-03-22 19:55:01.000000000 -0500 +++ ./Behaviors/Controls/StringInputControl.h 2007-01-30 17:56:18.000000000 -0500 @@ -26,7 +26,7 @@ } //! returns last call to takeInput() - virtual const std::string& getLastInput() { return lastInput; } + virtual const std::string& getLastInput() const { return lastInput; } //! clears the last input (i.e. so you can easily tell later if new input is entered) virtual void clearLastInput() { takeInput(""); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/TorqueCalibrate.cc ./Behaviors/Controls/TorqueCalibrate.cc --- ../Tekkotsu_3.0/Behaviors/Controls/TorqueCalibrate.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Controls/TorqueCalibrate.cc 2007-11-12 23:16:00.000000000 -0500 @@ -0,0 +1,215 @@ +#include "TorqueCalibrate.h" +#include "Behaviors/Controls/NullControl.h" +#include "Shared/WorldState.h" +#include "Shared/Config.h" +#include "Events/EventRouter.h" +#include "Motion/MMAccessor.h" +#include "Motion/PIDMC.h" +#include "Motion/MotionSequenceMC.h" +#include "Sound/SoundManager.h" +#include +#include + +//better to put this here instead of the header +using namespace std; + + +//***************************// +//***** TorqueCalibrate *****// +//***************************// + +void TorqueCalibrate::record(unsigned int joint, float sensorDist, float maxDuty, float maxForce) const { + std::ofstream log(filename.c_str(),std::ios::app); + if(!log) { + serr->printf("ERROR: could not open %s for writing log\n",filename.c_str()); + sndman->playFile("fart.wav"); + } else { + log << joint << '\t' << sensorDist << '\t' << maxDuty << '\t' << maxForce << std::endl; + sndman->playFile("camera.wav"); + } + std::cout << "DATA: " << joint << '\t' << sensorDist << '\t' << maxDuty << '\t' << maxForce << std::endl; +} + +void TorqueCalibrate::refresh() { + if(filenameInput->getLastInput()!=filename) { + filename=filenameInput->getLastInput(); + std::string::size_type f=filename.rfind("/"); + filenameInput->setName("Storage: "+filename.substr(f==string::npos?0:f+1)); + std::string desc="Location where data will be appended to any previous contents"; + filenameInput->setDescription(desc+": "+filename); + std::ofstream log(filename.c_str(),std::ios::app); + if(!log) + serr->printf("ERROR: could not open %s for writing log\n",filename.c_str()); + } + ControlBase::refresh(); +} + + +//******************************************************// +//***** TorqueCalibrate::TakeMeasurementControl *****// +//******************************************************// + +ControlBase * TorqueCalibrate::TakeMeasurementControl::activate(MotionManager::MC_ID disp_id, Socket * gui) { +#ifdef TGT_HAS_BUTTONS +# ifdef TGT_ERS7 + erouter->addListener(this,EventBase::buttonEGID,FrontBackButOffset,EventBase::activateETID); + erouter->addListener(this,EventBase::buttonEGID,MiddleBackButOffset,EventBase::activateETID); + erouter->addListener(this,EventBase::buttonEGID,RearBackButOffset,EventBase::activateETID); +# else + erouter->addListener(this,EventBase::buttonEGID,BackButOffset,EventBase::activateETID); +# endif +#else +# warning TorqueCalibrate control needs some kind of trigger, target model does not have any buttons? +#endif + cstate=ZERO_JOINT; + SharedObject pidmc(0); + pidID=motman->addPersistentMotion(pidmc,MotionManager::kHighPriority); + return ControlBase::activate(disp_id,gui); +} + +void TorqueCalibrate::TakeMeasurementControl::processEvent(const EventBase& event) { + if(cstate==ZERO_JOINT && event.getGeneratorID()==EventBase::buttonEGID) { + basePosition=state->outputs[joint]; + transition(RECORD_POSITION); + } else if(cstate==DO_PULSE && event.getGeneratorID()==EventBase::sensorEGID) { + if(std::abs(state->pidduties[joint-PIDJointOffset]) > maxDuty) + maxDuty=std::abs(state->pidduties[joint-PIDJointOffset]); + std::cout << "Duty: " << state->pidduties[joint-PIDJointOffset] << std::endl; + } else if(cstate==DO_PULSE && event.getGeneratorID()==EventBase::timerEGID) { + erouter->removeListener(this,EventBase::sensorEGID); + } else if(cstate==DO_PULSE && event.getGeneratorID()==EventBase::motmanEGID) { + pulseID=MotionManager::invalid_MC_ID; + std::cout << "Max duty: " << maxDuty << std::endl; + transition(RECORD_FORCE); + } else { + std::cerr << "Unhandled event " << event.getName() << std::endl; + } +} + +void TorqueCalibrate::TakeMeasurementControl::refresh() { + clearSlots(); + switch(cstate) { + case ZERO_JOINT: + pushSlot(new NullControl("Position the joint")); + pushSlot(new NullControl("and press a back button")); + break; + case RECORD_POSITION: + pushSlot(new NullControl("What is the length of")); + pushSlot(new NullControl("the lever arm? (cm)")); + pushSlot(new NullControl("(Dist. from axis of")); + pushSlot(new NullControl("rot. to force sensor)")); + break; + case INPUT_PULSE: + pushSlot(new NullControl("Enter the position")); + pushSlot(new NullControl("offset to attempt")); + pushSlot(new NullControl("(radians -- bigger")); + pushSlot(new NullControl("offset means apply")); + pushSlot(new NullControl("more force...)")); + break; + case DO_PULSE: + pushSlot(new NullControl("Running...")); + break; + case RECORD_FORCE: { + char res[256]; + snprintf(res,256,"Max duty was: %g",maxDuty); + pushSlot(new NullControl(res)); + pushSlot(NULL); + pushSlot(new NullControl("Enter the maximum")); + pushSlot(new NullControl("force (N)")); + break; + } + } + ControlBase::refresh(); +} + +ControlBase * TorqueCalibrate::TakeMeasurementControl::takeInput(const std::string& msg) { + switch(cstate) { + case ZERO_JOINT: + case DO_PULSE: + break; + case RECORD_POSITION: + sensorDist=atof(msg.c_str()); + if(sensorDist==0) { + sndman->playFile("fart.wav"); + return NULL; + } + transition(INPUT_PULSE); + break; + case INPUT_PULSE: { + float offset=atof(msg.c_str()); + SharedObject ms; + ms->advanceTime(350); + if(offset>.1) { + // move slowly at first in case not quite against sensor + ms->setOutputCmd(joint,basePosition+offset/4); + ms->advanceTime(300); + ms->setOutputCmd(joint,basePosition+offset/2); + ms->advanceTime(200); + // hard push at the end + ms->setOutputCmd(joint,basePosition+offset); + ms->advanceTime(500); + } else { + ms->setOutputCmd(joint,basePosition+offset); + ms->advanceTime(700); + } + ms->setOutputCmd(joint,basePosition+offset); + ms->advanceTime(300); + erouter->addTimer(this,0,ms->getTime(),false); + ms->setOutputCmd(joint,basePosition); + ms->advanceTime(200); + ms->setOutputCmd(joint,basePosition); + pulseID=motman->addPrunableMotion(ms); + transition(DO_PULSE); + } break; + case RECORD_FORCE: { + float f=atof(msg.c_str()); + if(f!=0) + parent.record(joint,sensorDist,maxDuty,f); + else + sndman->playFile("fart.wav"); + transition(INPUT_PULSE); + } break; + } + return this; +} + +void TorqueCalibrate::TakeMeasurementControl::deactivate() { + erouter->remove(this); + motman->removeMotion(pidID); + pidID=MotionManager::invalid_MC_ID; + if(pulseID!=MotionManager::invalid_MC_ID) { + motman->removeMotion(pulseID); + pulseID=MotionManager::invalid_MC_ID; + } + ControlBase::deactivate(); +} + +void TorqueCalibrate::TakeMeasurementControl::transition(State_t newstate) { + MMAccessor(pidID)->setAllPowerLevel(1); + erouter->removeListener(this); + cstate=newstate; + if(cstate==RECORD_POSITION || cstate==INPUT_PULSE || cstate==RECORD_FORCE) + sndman->playFile("ping.wav"); + else + sndman->playFile("barkhigh.wav"); + refresh(); + if(cstate==DO_PULSE) { + maxDuty=0; + float pidSetting[] = {DefaultPIDs[joint][0],0,0}; + MMAccessor(pidID)->setPID(joint,pidSetting); + erouter->addListener(this,EventBase::sensorEGID); + erouter->addListener(this,EventBase::motmanEGID,pulseID,EventBase::deactivateETID); + } +} + + +/*! @file + * @brief + * @author Ethan Tira-Thompson (ejt) (Creator) + * + * $Author: ejt $ + * $Name: tekkotsu-4_0 $ + * $Revision: 1.4 $ + * $State: Exp $ + * $Date: 2007/11/13 04:16:00 $ + */ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/TorqueCalibrate.h ./Behaviors/Controls/TorqueCalibrate.h --- ../Tekkotsu_3.0/Behaviors/Controls/TorqueCalibrate.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Controls/TorqueCalibrate.h 2007-04-05 02:24:09.000000000 -0400 @@ -0,0 +1,122 @@ +//-*-c++-*- +#ifndef INCLUDED_TorqueCalibrate_h +#define INCLUDED_TorqueCalibrate_h + +#include "Behaviors/Controls/ControlBase.h" +#include "Behaviors/Controls/FileInputControl.h" +#include "Motion/MotionManager.h" +#include "Events/EventListener.h" + +//! Provides an interface for making measurements to correlate PID duty cycle and actual force output for each of the motors +class TorqueCalibrate : public ControlBase { + + // **************************** // + // ******* CONSTRUCTORS ******* // + // **************************** // + // Not all of these necessarily make sense to implement... feel free + // to remove those which don't -- none are required. + +public: + //! default constructor + TorqueCalibrate() + : ControlBase("TorqueCalibrate","Provides an interface for making measurements to correlate PID duty cycle and actual force output for each of the motors"), + filename(config->portPath("data/torque.dat")), filenameInput(NULL) + {init();} + //! constructor which allows a custom name + TorqueCalibrate(const std::string& n) + : ControlBase(n,"Provides an interface for making measurements to correlate PID duty cycle and actual force output for each of the motors"), + filename(config->portPath("data/torque.dat")), filenameInput(NULL) + {init();} + //! constructor which allows a custom name and tooltip + TorqueCalibrate(const std::string& n, const std::string& d) + : ControlBase(n,d), + filename(config->portPath("data/torque.dat")), filenameInput(NULL) + {init();} + + //! destructor + virtual ~TorqueCalibrate() {} + +protected: + class TakeMeasurementControl : public ControlBase, public EventListener { + public: + TakeMeasurementControl(const TorqueCalibrate& tcParent, unsigned int jointToMeasure) + : ControlBase(outputNames[jointToMeasure]), parent(tcParent), joint(jointToMeasure), + basePosition(state->outputs[jointToMeasure]), maxDuty(0), sensorDist(0), cstate(ZERO_JOINT), + pidID(MotionManager::invalid_MC_ID), pulseID(MotionManager::invalid_MC_ID) + {} + virtual ControlBase * activate(MotionManager::MC_ID disp_id, Socket * gui); + virtual void processEvent(const EventBase& event); + virtual void refresh(); + virtual ControlBase * takeInput(const std::string& msg); + virtual void deactivate(); + protected: + //! the states the TakeMeasurementControl goes through when recording measurements + enum State_t { + ZERO_JOINT, //!< turn off PID of joints, allow user to reposition them to the force sensor + RECORD_POSITION, //!< record the length of the lever arm (distance from point of rotation to force sensor) + INPUT_PULSE, //!< wait for user to specify size of pulse to perform + DO_PULSE, //!< make the joint do the pulse + RECORD_FORCE //!< wait for user to report the recorded force applied + }; + + //! requests a transition to another state + void transition(State_t newstate); + + const TorqueCalibrate& parent; + unsigned int joint; + float basePosition; + float maxDuty; + float sensorDist; + State_t cstate; + MotionManager::MC_ID pidID; + MotionManager::MC_ID pulseID; + }; + + //! initialization + virtual void init() { + pushSlot(filenameInput=new FileInputControl("Storage: ","Location where data will be appended to any previous contents","")); + pushSlot(NULL); + for(unsigned int i=PIDJointOffset; isetAcceptNonExistant(true); + filenameInput->takeInput(filename); + filename=""; //force refresh + } + + + // **************************** // + // ********* METHODS ********** // + // **************************** // +public: + void record(unsigned int joint, float sensorDist, float maxDuty, float maxForce) const; + virtual void refresh(); + + + // **************************** // + // ********* MEMBERS ********** // + // **************************** // +protected: + std::string filename; + FileInputControl * filenameInput; + + + // **************************** // + // ********** OTHER *********** // + // **************************** // +private: + TorqueCalibrate(const TorqueCalibrate&); //!< you can override, but don't call this... + TorqueCalibrate& operator=(const TorqueCalibrate&); //!< you can override, but don't call this... +}; + +/*! @file + * @brief Defines TorqueCalibrate, which provides an interface for making measurements to correlate PID duty cycle and actual force output for each of the motors + * @author Ethan Tira-Thompson (ejt) (Creator) + * + * $Author: ejt $ + * $Name: tekkotsu-4_0 $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2007/04/05 06:24:09 $ + */ +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/ValueEditControl.h ./Behaviors/Controls/ValueEditControl.h --- ../Tekkotsu_3.0/Behaviors/Controls/ValueEditControl.h 2005-08-07 00:11:03.000000000 -0400 +++ ./Behaviors/Controls/ValueEditControl.h 2007-06-28 00:36:19.000000000 -0400 @@ -12,6 +12,7 @@ #include "Shared/ERS220Info.h" #include "Shared/ERS7Info.h" #include "Wireless/Wireless.h" +#include "Behaviors/Controller.h" #include #include @@ -40,32 +41,17 @@ } //! will increment/decrement the current and then assign it to the target when head buttons pressed virtual void processEvent(const EventBase& e) { - if(state->robotDesign&WorldState::ERS210Mask) { - switch(e.getSourceID()) { - case ERS210Info::HeadFrButOffset: doNextItem(); doSelect(); break; - case ERS210Info::HeadBkButOffset: doNextItem(); doSelect(); break; - default: - serr->printf("*** WARNING ValueEditControl got an unasked for event\n"); - break; - } - } else if(state->robotDesign&WorldState::ERS220Mask) { - switch(e.getSourceID()) { - case ERS220Info::HeadFrButOffset: doNextItem(); doSelect(); break; - case ERS220Info::HeadBkButOffset: doNextItem(); doSelect(); break; - default: - serr->printf("*** WARNING ValueEditControl got an unasked for event\n"); - break; - } - } else if(state->robotDesign&WorldState::ERS7Mask) { - switch(e.getSourceID()) { - case ERS7Info::FrontBackButOffset: doNextItem(); doSelect(); break; - case ERS7Info::RearBackButOffset: doNextItem(); doSelect(); break; - default: - serr->printf("*** WARNING ValueEditControl got an unasked for event\n"); - break; - } + if(e==Controller::nextItem) { + doNextItem(); + doSelect(); + } else if(e==Controller::prevItem) { + doPrevItem(); + doSelect(); + } else { + serr->printf("*** WARNING ValueEditControl got an unasked for event\n"); } } + //! displays current value virtual void refresh() { //Do console only if GUI is connected @@ -82,22 +68,8 @@ //! request to continue receiving events so we can modify the value while running virtual void pause() { - unsigned int FrButOffset,BkButOffset; - if(state->robotDesign&WorldState::ERS210Mask) { - FrButOffset=ERS210Info::HeadFrButOffset; - BkButOffset=ERS210Info::HeadBkButOffset; - } else if(state->robotDesign&WorldState::ERS220Mask) { - FrButOffset=ERS220Info::HeadFrButOffset; - BkButOffset=ERS220Info::HeadBkButOffset; - } else if(state->robotDesign&WorldState::ERS7Mask) { - FrButOffset=ERS7Info::FrontBackButOffset; - BkButOffset=ERS7Info::RearBackButOffset; - } else { - serr->printf("ValueEditControl: Unsupported model!\n"); - return; - } - erouter->addListener(this,EventBase(EventBase::buttonEGID,FrButOffset,EventBase::deactivateETID,0)); - erouter->addListener(this,EventBase(EventBase::buttonEGID,BkButOffset,EventBase::deactivateETID,0)); + erouter->addListener(this,Controller::nextItem); + erouter->addListener(this,Controller::prevItem); // erouter->addListener(this,EventBase(EventBase::buttonEGID,ChinButOffset,EventBase::deactivateETID,0)); StringInputControl::pause(); } @@ -120,13 +92,13 @@ } //! adds one to the current value virtual ControlBase * doNextItem() { - cur++; + cur=(T)(cur+1); refresh(); return this; } //! subtracts one from the current value virtual ControlBase * doPrevItem() { - cur--; + cur=(T)(cur-1); refresh(); return this; } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/WalkCalibration.cc ./Behaviors/Controls/WalkCalibration.cc --- ../Tekkotsu_3.0/Behaviors/Controls/WalkCalibration.cc 2006-09-18 14:07:54.000000000 -0400 +++ ./Behaviors/Controls/WalkCalibration.cc 2007-11-12 23:16:00.000000000 -0500 @@ -209,9 +209,9 @@ ControlBase::refresh(); if(gui_comm!=NULL && wireless->isConnected(gui_comm->sock)) { if(status.size()==0) - gui_comm->printf("status\n# Samples: fs=%d fr=%d sr=%d br=%d bs=%d r=%d\n",cnts[0], cnts[1], cnts[2], cnts[3], cnts[4], cnts[5]); + gui_comm->printf("status\n1\n# Samples: fs=%d fr=%d sr=%d br=%d bs=%d r=%d\n",cnts[0], cnts[1], cnts[2], cnts[3], cnts[4], cnts[5]); else - gui_comm->printf("status\n%s\n",status.c_str()); + gui_comm->printf("status\n%td\n%s\n",std::count(status.begin(),status.end(),'\n'),status.c_str()); } } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Controls/WaypointWalkControl.cc ./Behaviors/Controls/WaypointWalkControl.cc --- ../Tekkotsu_3.0/Behaviors/Controls/WaypointWalkControl.cc 2006-09-18 14:07:54.000000000 -0400 +++ ./Behaviors/Controls/WaypointWalkControl.cc 2007-11-11 18:57:18.000000000 -0500 @@ -59,7 +59,7 @@ sscanf(localizationCtl->getLastInput().c_str(),"%g %g %g",&x,&y,&a); MMAccessor walk(walk_id); walk->setCurPos(x+walk->getCurX(),y+walk->getCurY(),a+walk->getCurA()); - cout << "Position is now " << walk->getCurX() << ' ' << walk->getCurY() << ' ' << walk->getCurA() << endl; + std::cout << "Position is now " << walk->getCurX() << ' ' << walk->getCurY() << ' ' << walk->getCurA() << std::endl; localizationCtl->clearLastInput(); } @@ -161,7 +161,7 @@ pushSlot(new ValueEditControl("Turn Speed (in rad/s)",&curway.turnSpeed)); char desc[256]; snprintf(desc,256,"Types: EGO=%d, OFF=%d, ABS=%d",WaypointWalkMC::Waypoint::POSTYPE_EGOCENTRIC,WaypointWalkMC::Waypoint::POSTYPE_OFFSET,WaypointWalkMC::Waypoint::POSTYPE_ABSOLUTE); - pushSlot(new ValueEditControl("Pos. coord. system",desc,(int*)&curway.posType)); + pushSlot(new ValueEditControl("Pos. coord. system",desc,&curway.posType)); ToggleControl * tmptgl; pushSlot(tmptgl=new ToggleControl("Angle is relative")); tmptgl->setStatus(curway.angleIsRelative); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/AlanBehavior.h ./Behaviors/Demos/AlanBehavior.h --- ../Tekkotsu_3.0/Behaviors/Demos/AlanBehavior.h 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Demos/AlanBehavior.h 2007-11-12 13:00:38.000000000 -0500 @@ -29,7 +29,7 @@ // creates a PostureMC class to move the joint(s) and adds it to global MotionManager pose_id=motman->addPersistentMotion(SharedObject()); // subscribe to sensor updated events through the global EventRouter - erouter->addListener(this,EventBase::sensorEGID,SensorSourceID::UpdatedSID); + erouter->addListener(this,EventBase::sensorEGID,SensorSrcID::UpdatedSID); } virtual void DoStop() { @@ -60,16 +60,16 @@ //state is a global instantiation of WorldState, kept up to date by framework; //pressure is in range 0 to 1 - we use the pressure on the front head button here float pressure=0; - if(state->robotDesign&WorldState::ERS210Mask) { - pressure=state->buttons[ERS210Info::HeadFrButOffset]; + if(RobotName == ERS210Info::TargetName) { + pressure=state->buttons[capabilities.getButtonOffset("HeadFrBut")]; std::cout << "HeadFrBut Pressure: " << pressure << std::endl; - } else if(state->robotDesign&WorldState::ERS7Mask) { + } else if(RobotName == ERS7Info::TargetName) { pressure=state->buttons[ERS7Info::HeadButOffset]; std::cout << "HeadBut Pressure: " << pressure << std::endl; } else { //only really works on the ERS-210 or ERS-7 models - the others don't have a proper pressure sensor //(the 220's antenna-thing is close, but doesn't give a continuous range) - std::cout << "Unknown model" << std::endl; + std::cout << "Unsupported/unknown model" << std::endl; erouter->removeListener(this); // stops getting events (and timers, if we had any) return; } @@ -83,9 +83,9 @@ //let's do the whole thing again with the other head button for the other leg: // (cutting out a some of the intermediary steps this time) joint=RFrLegOffset+RotatorOffset; - if(state->robotDesign&WorldState::ERS210Mask) - pose_mc->setOutputCmd(joint,outputRanges[joint][MaxRange]*state->buttons[ERS210Info::HeadBkButOffset]); - else if(state->robotDesign&WorldState::ERS7Mask) //ERS7 doesn't have another head button, we'll use one of its back buttons + if(RobotName == ERS210Info::TargetName) + pose_mc->setOutputCmd(joint,outputRanges[joint][MaxRange]*state->buttons[capabilities.getButtonOffset(ERS210Info::buttonNames[ERS210Info::HeadBkButOffset])]); + else if(RobotName == ERS7Info::TargetName) //ERS7 doesn't have another head button, we'll use one of its back buttons pose_mc->setOutputCmd(joint,outputRanges[joint][MaxRange]*state->buttons[ERS7Info::FrontBackButOffset]); // notice that there's no "check in" for pose_mc @@ -93,7 +93,7 @@ } else { //should never happen - cout << "Unhandled Event:" << event.getName() << endl; + std::cout << "Unhandled Event:" << event.getName() << std::endl; } } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/BanditMachine.h ./Behaviors/Demos/BanditMachine.h --- ../Tekkotsu_3.0/Behaviors/Demos/BanditMachine.h 2006-09-18 14:07:57.000000000 -0400 +++ ./Behaviors/Demos/BanditMachine.h 2007-11-12 13:00:38.000000000 -0500 @@ -50,8 +50,8 @@ StateNode *decide=addNode(new DecideNode("Decide",bandit,left,right)); StateNode *recoverl=addNode(new OutputNode("\nBadPressLeft",std::cout,wait)); StateNode *recoverr=addNode(new OutputNode("\nBadPressRight",std::cout,wait)); - left->addTransition(new SmoothCompareTrans(wait,&state->pidduties[LFrLegOffset+RotatorOffset],CompareTrans::LT,-.07,EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID),.7)); - right->addTransition(new SmoothCompareTrans(wait,&state->pidduties[RFrLegOffset+RotatorOffset],CompareTrans::LT,-.07,EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID),.7)); + left->addTransition(new SmoothCompareTrans(wait,&state->pidduties[LFrLegOffset+RotatorOffset],CompareTrans::LT,-.07,EventBase(EventBase::sensorEGID,SensorSrcID::UpdatedSID,EventBase::statusETID),.7)); + right->addTransition(new SmoothCompareTrans(wait,&state->pidduties[RFrLegOffset+RotatorOffset],CompareTrans::LT,-.07,EventBase(EventBase::sensorEGID,SensorSrcID::UpdatedSID,EventBase::statusETID),.7)); wait->addTransition(new TimeOutTrans(decide,2000)); left->addTransition(new TimeOutTrans(recoverl,1500)); right->addTransition(new TimeOutTrans(recoverr,1500)); @@ -180,7 +180,7 @@ virtual void DoStop() { erouter->removeListener(this); b.reward(reward); - cout << endl; + std::cout << std::endl; reward=false; StateNode::DoStop(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/CameraBehavior.cc ./Behaviors/Demos/CameraBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Demos/CameraBehavior.cc 2006-09-18 14:07:57.000000000 -0400 +++ ./Behaviors/Demos/CameraBehavior.cc 2007-07-16 14:31:40.000000000 -0400 @@ -1,9 +1,7 @@ #include "CameraBehavior.h" #include "Events/EventRouter.h" #include "Events/TextMsgEvent.h" -#include "Shared/ERS210Info.h" -#include "Shared/ERS220Info.h" -#include "Shared/ERS7Info.h" +#include "Shared/RobotInfo.h" #include "Wireless/Socket.h" #include "Shared/WorldState.h" #include "Sound/SoundManager.h" @@ -26,13 +24,9 @@ void CameraBehavior::DoStart() { BehaviorBase::DoStart(); - if(state->robotDesign&WorldState::ERS210Mask) { - camera_click.setSourceID(ERS210Info::HeadFrButOffset); - } else if(state->robotDesign&WorldState::ERS220Mask) { - camera_click.setSourceID(ERS220Info::HeadFrButOffset); - } else if(state->robotDesign&WorldState::ERS7Mask) { - camera_click.setSourceID(ERS7Info::HeadButOffset); - } + if(capabilities.findButtonOffset("HeadBut")!=-1U) // if there is a head button, use it for the trigger + camera_click.setSourceID(capabilities.getButtonOffset("HeadBut")); + // (otherwise, just stick with the default button 0 as set in camera_click constructor) initIndex(); sndman->loadFile("camera.wav"); ledID=motman->addPersistentMotion(SharedObject()); @@ -63,11 +57,15 @@ { MMAccessor leds(ledID); +#if defined(TGT_ERS7) || defined(TGT_ERS210) || defined(TGT_ERS220) || defined(TGT_ERS2xx) leds->cset(FaceLEDMask,2.0/3.0); leds->set(TopBrLEDMask,1); +#else + leds->set(~0,1); +#endif } - if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE) { + if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE) { //this is our own odd little format, would be nice to save a TIFF or something instead // open file @@ -76,7 +74,7 @@ return; //! write actual image data - if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_COLOR) { + if(config->vision.rawcam.encoding==Config::vision_config::RawCamConfig::ENCODE_COLOR) { FilterBankGenerator * gen=ProjectInterface::defInterleavedYUVGenerator; // just an alias for readability gen->selectSaveImage(ProjectInterface::doubleLayer,InterleavedYUVGenerator::CHAN_YUV); unsigned int len=gen->saveFileStream(f); @@ -85,9 +83,9 @@ sndman->playFile(config->controller.error_snd); return; } - } else if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_SINGLE_CHANNEL) { + } else if(config->vision.rawcam.encoding==Config::vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL) { FilterBankGenerator * gen=ProjectInterface::defRawCameraGenerator; // just an alias for readability - gen->selectSaveImage(ProjectInterface::doubleLayer,config->vision.rawcam_channel); + gen->selectSaveImage(ProjectInterface::doubleLayer,config->vision.rawcam.channel); unsigned int len=gen->saveFileStream(f); if(len==0) { serr->printf("Error saving file\n"); @@ -99,15 +97,15 @@ // close file fclose(f); - } else if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG) { + } else if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_JPEG) { //save a JPEG image JPEGGenerator * jpeg=NULL; // we'll set this to pick between the color jpeg or a single channel grayscale jpeg unsigned int chan=0; // and this will hold the channel to use out of that jpeg generator - if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_COLOR) + if(config->vision.rawcam.encoding==Config::vision_config::RawCamConfig::ENCODE_COLOR) jpeg=dynamic_cast(ProjectInterface::defColorJPEGGenerator); - else if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_SINGLE_CHANNEL) { + else if(config->vision.rawcam.encoding==Config::vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL) { jpeg=dynamic_cast(ProjectInterface::defGrayscaleJPEGGenerator); - chan=config->vision.rawcam_channel; + chan=config->vision.rawcam.channel; } if(jpeg!=NULL) { unsigned int tmp_q=jpeg->getQuality(); //temporary storage so we can reset the default @@ -133,15 +131,15 @@ jpeg->setQuality(tmp_q); } - } else if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_PNG) { - //save a JPEG image + } else if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_PNG) { + //save a PNG image PNGGenerator * png=NULL; // we'll set this to pick between the color png or a single channel grayscale png unsigned int chan=0; // and this will hold the channel to use out of that png generator - if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_COLOR) + if(config->vision.rawcam.encoding==Config::vision_config::RawCamConfig::ENCODE_COLOR) png=dynamic_cast(ProjectInterface::defColorPNGGenerator); - else if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_SINGLE_CHANNEL) { + else if(config->vision.rawcam.encoding==Config::vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL) { png=dynamic_cast(ProjectInterface::defGrayscalePNGGenerator); - chan=config->vision.rawcam_channel; + chan=config->vision.rawcam.channel; } if(png!=NULL) { // open file @@ -166,10 +164,21 @@ { MMAccessor leds(ledID); leds->clear(); +#if defined(TGT_ERS7) || defined(TGT_ERS210) || defined(TGT_ERS220) || defined(TGT_ERS2xx) leds->flash(TopBrLEDMask,700); leds->flash(TopLLEDMask|TopRLEDMask,500); leds->flash(MidLLEDMask|MidRLEDMask,300); leds->flash(BotLLEDMask|BotRLEDMask,100); +#else + if(NumLEDs>3) + leds->flash(1<<3,700); + if(NumLEDs>2) + leds->flash(1<<2,500); + if(NumLEDs>1) + leds->flash(1<<1,300); + if(NumLEDs>0) + leds->flash(1<<0,100); +#endif } sout->printf("done\n"); @@ -226,7 +235,6 @@ sout->printf("The next saved image will go to %simg%05d\n",path.c_str(),index); } - /*! @file * @brief Implements CameraBehavior, for taking pictures * @author ejt (Creator) diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/CameraBehavior.h ./Behaviors/Demos/CameraBehavior.h --- ../Tekkotsu_3.0/Behaviors/Demos/CameraBehavior.h 2004-11-10 20:45:35.000000000 -0500 +++ ./Behaviors/Demos/CameraBehavior.h 2007-06-28 00:36:20.000000000 -0400 @@ -15,8 +15,8 @@ * to transmit them over. * * Image format is chosen by current config settings for the - * Config::vision_config::rawcam_compression and - * Config::vision_config::rawcam_channel. However, the double + * Config::vision_config::RawCamConfig::compression and + * Config::vision_config::RawCamConfig::channel. However, the double * resolution layer is always saved instead of whatever the current * config skip value indicates. */ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/ChaseBallBehavior.cc ./Behaviors/Demos/ChaseBallBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Demos/ChaseBallBehavior.cc 2004-10-16 21:16:10.000000000 -0400 +++ ./Behaviors/Demos/ChaseBallBehavior.cc 2007-11-16 10:19:40.000000000 -0500 @@ -6,9 +6,8 @@ #include "Motion/WalkMC.h" #include "Shared/WMclass.h" #include "Shared/ProjectInterface.h" - -//! Converts degrees to radians -inline double DtoR(double deg) { return (deg/180.0*M_PI); } +#include "Shared/ERS7Info.h" +#include "Motion/MMAccessor.h" void ChaseBallBehavior::DoStart() { BehaviorBase::DoStart(); @@ -26,39 +25,58 @@ //this could be cleaned up event-wise (only use a timer when out of view) void ChaseBallBehavior::processEvent(const EventBase& event) { - WMreg(chase_ball_behavior); - WMvari_(float, horiz, 0, chase_ball_behavior); - WMvari_(float, vert, 0, chase_ball_behavior); - + WMreg(chase_ball_behavior); + WMvari_(float, horiz, 0, chase_ball_behavior); + WMvari_(float, vert, 0, chase_ball_behavior); + if(event.getGeneratorID()==EventBase::visObjEGID && event.getTypeID()==EventBase::statusETID) { horiz=static_cast(&event)->getCenterX(); vert=static_cast(&event)->getCenterY(); + std::cout << get_time() << ' ' << horiz << ' ' << vert << std::endl; } - + + // For portability, look to see if the host hardware has a head pan & tilt joints, + // using ERS-7's namespace to look up the canonical "name" of the pan and tilt joints. + // Note if you were coding for a specific robot, could just do "tiltIdx = HeadOffset + TiltOffset" + // directly and not bother with this... + const char * const * ERS7HeadNames = &ERS7Info::outputNames[ERS7Info::HeadOffset]; + const unsigned int panIdx = capabilities.findOutputOffset(ERS7HeadNames[ERS7Info::PanOffset]); + const unsigned int tiltIdx = capabilities.findOutputOffset(ERS7HeadNames[ERS7Info::TiltOffset]); + if(panIdx==-1U || tiltIdx==-1U) + return; // guess we're headless, leave now... + +#if defined(TGT_QBOTPLUS) || defined(TGT_QWERK) + // these *should* be millimeters per second, but we haven't calibrated Qwerk yet + // so these are some unitless speed measurement for now :( + const float FAST = 32, SLOW = 20; +#else + // these are millimeters per second + const float FAST = 160, SLOW = 100; +#endif + + // We use the "Walk" motion command, but if it's wheel based, the WalkMC will still handle it anyway WalkMC * walker = (WalkMC*)motman->checkoutMotion(walker_id); - if(state->outputs[HeadOffset+PanOffset]<-.05 || state->outputs[HeadOffset+PanOffset]>.05) - walker->setTargetVelocity(100,0,state->outputs[HeadOffset+PanOffset]); + if(state->outputs[panIdx]<-.05 || state->outputs[panIdx]>.05) + walker->setTargetVelocity(SLOW,0,state->outputs[panIdx]); else - walker->setTargetVelocity(160,0,0); + walker->setTargetVelocity(FAST,0,0); // target straight ahead, full speed! motman->checkinMotion(walker_id); - - // cout << inview << ' ' << horiz << ' ' << vert << endl; - - double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/7.5; - double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/6; - if(tiltDtoR(40)) - tilt=DtoR(40); - if(pan>DtoR(80)) - pan=DtoR(80); - if(pancheckoutMotion(headpointer_id); - headpointer->setJoints(tilt,pan,0); - motman->checkinMotion(headpointer_id); + + // proportional control to home in on the ball + // http://en.wikipedia.org/wiki/Proportional_control + double tilt=state->outputs[tiltIdx]-vert*CameraFOV/7.5; + double pan=state->outputs[panIdx]-horiz*CameraFOV/6; + + // We'll limit tilt and pan to their range of motion, but this isn't actually necessary, just demonstration: + tilt = std::max(tilt, outputRanges[tiltIdx][MinRange]); + tilt = std::min(tilt, outputRanges[tiltIdx][MaxRange]); + pan = std::max(pan, outputRanges[panIdx][MinRange]); + pan = std::min(pan, outputRanges[panIdx][MaxRange]); + + //! for comparison, this is a one-line version of the checkout/checkin used for the WalkMC + MMAccessor(headpointer_id)->setJoints(tilt,pan,0); } - + /*! @file * @brief Implements ChaseBallBehavior, which runs around after whatever the dog sees * @author ejt (Creator) diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/DrawVisObjBoundBehavior.h ./Behaviors/Demos/DrawVisObjBoundBehavior.h --- ../Tekkotsu_3.0/Behaviors/Demos/DrawVisObjBoundBehavior.h 2006-06-30 12:37:03.000000000 -0400 +++ ./Behaviors/Demos/DrawVisObjBoundBehavior.h 2007-01-30 17:56:19.000000000 -0500 @@ -48,12 +48,12 @@ unsigned char color; const FilterBankEvent& fbe=dynamic_cast(e); if(e.getGeneratorID()==EventBase::visRawCameraEGID) { - layer=fbe.getNumLayers()-1-config->vision.rawcam_y_skip; + layer=fbe.getNumLayers()-1-config->vision.rawcam.y_skip; chan=RawCameraGenerator::CHAN_Y; color=255; } else if(e.getGeneratorID()==EventBase::visSegmentEGID) { - layer=fbe.getNumLayers()-1-config->vision.segcam_skip; - chan=config->vision.segcam_channel; + layer=fbe.getNumLayers()-1-config->vision.segcam.skip; + chan=config->vision.segcam.channel; color=7; } else { std::cerr << "WARNING: " << getClassName() << " received vision event from unknown generator" << std::endl; @@ -68,7 +68,7 @@ //this is only needed until RLEGraphics is in place // in the mean time, we trigger the RLE generator to recompress the modified seg cam image - if(config->vision.segcam_compression==Config::vision_config::COMPRESS_RLE) + if(config->vision.segcam.compression==Config::vision_config::SegCamConfig::COMPRESS_RLE) ProjectInterface::defRLEGenerator->invalidateCaches(); drawn++; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/ExploreMachine.cc ./Behaviors/Demos/ExploreMachine.cc --- ../Tekkotsu_3.0/Behaviors/Demos/ExploreMachine.cc 2005-12-15 13:51:35.000000000 -0500 +++ ./Behaviors/Demos/ExploreMachine.cc 2007-06-26 00:27:43.000000000 -0400 @@ -1,27 +1,15 @@ +#include "Shared/RobotInfo.h" +#ifdef TGT_HAS_IR_DISTANCE + #include "ExploreMachine.h" #include "Behaviors/Nodes/WalkNode.h" #include "Behaviors/Transitions/SmoothCompareTrans.h" #include "Behaviors/Transitions/TimeOutTrans.h" -#include "Shared/ERS210Info.h" -#include "Shared/ERS220Info.h" -#include "Shared/ERS7Info.h" #include "Wireless/Socket.h" #include "Shared/WorldState.h" void ExploreMachine::setup() { //cout << "Explore SETUP " << issetup << "..."; - unsigned int IRDistOffset; - if(state->robotDesign&WorldState::ERS210Mask) - IRDistOffset=ERS210Info::IRDistOffset; - else if(state->robotDesign&WorldState::ERS220Mask) - IRDistOffset=ERS220Info::IRDistOffset; - else if(state->robotDesign&WorldState::ERS7Mask) - IRDistOffset=ERS7Info::NearIRDistOffset; - else { - serr->printf("ExploreMachine: Unsupported model!\n"); - return; - } - SharedObject walk; walkid=motman->addPersistentMotion(walk); @@ -31,7 +19,7 @@ start=addNode(turn=new WalkNode(getName()+"::turn",0,0,0.5f)); turn->setMC(walkid); - move->addTransition(new SmoothCompareTrans(turn,&state->sensors[IRDistOffset],CompareTrans::LT,350,EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID),.7)); + move->addTransition(new SmoothCompareTrans(turn,&state->sensors[IRDistOffset],CompareTrans::LT,350,EventBase(EventBase::sensorEGID,SensorSrcID::UpdatedSID,EventBase::statusETID),.7)); turn->addTransition(new TimeOutTrans(move,2000)); StateNode::setup(); @@ -41,7 +29,7 @@ void ExploreMachine::DoStart() { StateNode::DoStart(); start->DoStart(); - //erouter->addListener(this,EventBase::sensorEGID,SensorSourceID::UpdatedSID); + //erouter->addListener(this,EventBase::sensorEGID,SensorSrcID::UpdatedSID); erouter->addListener(this,EventBase::stateMachineEGID,(size_t)turn,EventBase::activateETID); } @@ -72,8 +60,10 @@ * @author ejt (Creator) * * $Author: ejt $ - * $Name: tekkotsu-3_0 $ - * $Revision: 1.3 $ + * $Name: tekkotsu-4_0 $ + * $Revision: 1.5 $ * $State: Exp $ - * $Date: 2005/12/15 18:51:35 $ + * $Date: 2007/06/26 04:27:43 $ */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/ExploreMachine.h ./Behaviors/Demos/ExploreMachine.h --- ../Tekkotsu_3.0/Behaviors/Demos/ExploreMachine.h 2005-12-15 13:51:36.000000000 -0500 +++ ./Behaviors/Demos/ExploreMachine.h 2007-06-26 00:27:43.000000000 -0400 @@ -6,6 +6,11 @@ #include "Behaviors/Nodes/WalkNode.h" #include "Motion/MotionManager.h" +#include "Shared/RobotInfo.h" +#ifndef TGT_HAS_IR_DISTANCE +#error ExploreMachine requires a distance rangefinder via IRDistOffset +#endif + //! A state machine for exploring an environment (or searching for an object) class ExploreMachine : public StateNode { public: diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/FollowHeadBehavior.cc ./Behaviors/Demos/FollowHeadBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Demos/FollowHeadBehavior.cc 2006-09-05 16:30:21.000000000 -0400 +++ ./Behaviors/Demos/FollowHeadBehavior.cc 2007-11-12 13:00:38.000000000 -0500 @@ -1,3 +1,6 @@ +#include "Shared/RobotInfo.h" +#ifdef TGT_HAS_BUTTONS + #include "FollowHeadBehavior.h" #include "Events/EventRouter.h" #include "Shared/debuget.h" @@ -7,6 +10,8 @@ #include "Motion/WalkMC.h" #include "Motion/PIDMC.h" +using namespace std; + FollowHeadBehavior::FollowHeadBehavior() : BehaviorBase("FollowHeadBehavior"), head_release(EventBase::buttonEGID,ChinButOffset,EventBase::activateETID,0), @@ -68,6 +73,8 @@ } } +#endif + /*! @file * @brief Implements FollowHeadBehavior, walks where the head is pointing * @author ejt (Creator) diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/FollowHeadBehavior.h ./Behaviors/Demos/FollowHeadBehavior.h --- ../Tekkotsu_3.0/Behaviors/Demos/FollowHeadBehavior.h 2004-11-10 20:45:36.000000000 -0500 +++ ./Behaviors/Demos/FollowHeadBehavior.h 2007-03-04 20:19:08.000000000 -0500 @@ -1,5 +1,5 @@ //-*-c++-*- -#ifndef INCLUDED_FollowHeadBehavior_h_ +#if !defined(INCLUDED_FollowHeadBehavior_h_) && defined(TGT_HAS_BUTTONS) #define INCLUDED_FollowHeadBehavior_h_ #include "Behaviors/BehaviorBase.h" diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/GroundPlaneBehavior.h ./Behaviors/Demos/GroundPlaneBehavior.h --- ../Tekkotsu_3.0/Behaviors/Demos/GroundPlaneBehavior.h 2006-03-03 18:08:39.000000000 -0500 +++ ./Behaviors/Demos/GroundPlaneBehavior.h 2007-11-12 13:00:38.000000000 -0500 @@ -10,6 +10,8 @@ #include "Shared/ERS7Info.h" #include "Shared/ERS2xxInfo.h" #include "Shared/WorldState.h" +#include "Events/VisionObjectEvent.h" +#include "Shared/ProjectInterface.h" //! Reports the location of the center of the camera image on the ground plane class GroundPlaneBehavior : public BehaviorBase { @@ -17,24 +19,26 @@ //! constructor GroundPlaneBehavior() : BehaviorBase("GroundPlaneBehavior"), - head_release(EventBase::buttonEGID,(state->robotDesign&WorldState::ERS7Mask)?(int)ERS7Info::HeadButOffset:(int)ERS2xxInfo::HeadFrButOffset,EventBase::activateETID,0), - head_lock(EventBase::buttonEGID,(state->robotDesign&WorldState::ERS7Mask)?(int)ERS7Info::HeadButOffset:(int)ERS2xxInfo::HeadFrButOffset,EventBase::deactivateETID,0), - clock(EventBase::timerEGID,0,EventBase::statusETID,250) + head_release(EventBase::buttonEGID,HeadButOffset,EventBase::activateETID,0), + head_lock(EventBase::buttonEGID,HeadButOffset,EventBase::deactivateETID,0), + target_toggle(EventBase::buttonEGID,ChinButOffset,EventBase::activateETID), + targeting(false), + visual_target(EventBase::visObjEGID,ProjectInterface::visYellowBallSID,EventBase::statusETID), + last_seen(0), + clock(EventBase::timerEGID,0,EventBase::statusETID,250) { } virtual void DoStart() { BehaviorBase::DoStart(); // do this first erouter->addListener(this,head_release); erouter->addListener(this,head_lock); + erouter->addListener(this,target_toggle); processEvent(clock); } - - virtual void DoStop() { - erouter->removeListener(this); - BehaviorBase::DoStop(); // do this last - } - + virtual void processEvent(const EventBase& e) { + using std::cout; + using std::endl; if(e==clock) { //This is the direction gravity is pulling - probably a good way to find out //the attitude of the robot, assuming it is not moving. @@ -65,7 +69,41 @@ processEvent(clock); } else if(e==head_lock) { motman->addPrunableMotion(SharedObject(HeadOffset,HeadOffset+NumHeadJoints,1)); - erouter->removeListener(this,clock); + erouter->removeTimer(this,clock); + } else if(e==target_toggle) { + if(targeting) { + erouter->removeListener(this,visual_target); + targeting=false; + } else { + erouter->addListener(this,visual_target); + targeting=true; + } + sndman->playFile("yap.wav"); + } else if(e==visual_target) { + cout << '.' << std::flush; + if(get_time()(e); + + cout << "Target position: " << visob.getCenterX()<< " " << visob.getCenterY() << " normalized: " << visob.getCenterX()/visob.getXrange()<< " " << visob.getCenterY()/visob.getYrange() << endl; + + //First we determine the ground plane + NEWMAT::ColumnVector p=kine->calculateGroundPlane(); + cout << "Ground plane: " << p(1)<<"x + " << p(2)<<"y + " << p(3)<<"z = 1" << endl; + + //Project to ground plane - we do it twice here, once for camera frame and once for base frame + NEWMAT::ColumnVector ray(4); + config->vision.computeRay(visob.getCenterX()/visob.getXrange(),visob.getCenterY()/visob.getYrange(),ray(1),ray(2),ray(3)); + ray(4)=1; + NEWMAT::ColumnVector hit; + //cout <<"Current head:\n"<outputs[HeadOffset] <<' '<< state->outputs[HeadOffset+1] <<' '<< state->outputs[HeadOffset+2] << endl <getTransform(CameraFrameOffset); + hit=kine->projectToPlane(CameraFrameOffset,ray,BaseFrameOffset,p,CameraFrameOffset); + cout << "Intersection_camera: (" << hit(1)<<','<projectToPlane(CameraFrameOffset,ray,BaseFrameOffset,p,BaseFrameOffset); + cout << "Intersection_base: (" << hit(1)<<','<robotDesign & WorldState::ERS7Mask) { - head_release.setSourceID(ERS7Info::HeadButOffset); - head_lock.setSourceID(ERS7Info::HeadButOffset); - } else if(state->robotDesign & WorldState::ERS210Mask) { - head_release.setSourceID(ERS210Info::HeadFrButOffset); - head_lock.setSour