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.setSourceID(ERS210Info::HeadFrButOffset); - } else if(state->robotDesign & WorldState::ERS220Mask) { - head_release.setSourceID(ERS220Info::HeadFrButOffset); - head_lock.setSourceID(ERS220Info::HeadFrButOffset); - } + head_release.setSourceID(HeadButOffset); + head_lock.setSourceID(HeadButOffset); head.getRegion()->AddReference(); } //! destructor diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/HelloWorldBehavior.h ./Behaviors/Demos/HelloWorldBehavior.h --- ../Tekkotsu_3.0/Behaviors/Demos/HelloWorldBehavior.h 2004-11-10 20:45:36.000000000 -0500 +++ ./Behaviors/Demos/HelloWorldBehavior.h 2007-11-21 16:58:24.000000000 -0500 @@ -4,6 +4,7 @@ #include "Behaviors/BehaviorBase.h" #include "Wireless/Wireless.h" +#include "Behaviors/Controller.h" //! Demonstrates serr, sout, and cout class HelloWorldBehavior : public BehaviorBase { @@ -19,27 +20,37 @@ BehaviorBase::DoStart(); //now do your code: - for(unsigned int i=0; i<100; i++) - serr->printf("Hello serr! This is %d\n",i); - for(unsigned int i=0; i<10; i++) - sout->printf("Hello sout! This is %d\n",i); - for(unsigned int i=0; i<10; i++) - cout << "Hello cout! This is " << i << endl; - for(unsigned int i=0; i<10; i++) - printf("Hello printf! This is %d\n",i); + const unsigned int LINES=3; + for(unsigned int i=1; i<=LINES; i++) + serr->printf("Hello serr! This is %d of %d\n", i, LINES); + for(unsigned int i=1; i<=LINES; i++) + sout->printf("Hello sout! This is %d of %d\n", i, LINES); + for(unsigned int i=1; i<=LINES; i++) + std::cout << "Hello cout! This is " << i << " of " << LINES << std::endl; + for(unsigned int i=1; i<=LINES; i++) + printf("Hello printf! This is %d of %d\n", i, LINES); - //we'll just stop right away since this Behavior has no 'active' state. - DoStop(); //Note that you could also override this DoStop function... + // This will popup a message in the ControllerGUI, if one is connected + std::vector errmsg; + errmsg.push_back("Hello World! This is an informational message via the Controller\n" + "More messages have been sent on the console (cout, sout, etc.)"); + Controller::loadGUI("org.tekkotsu.mon.ControllerErr","",0,errmsg); + + // just stop right away since this Behavior has no 'active' state. + DoStop(); // Note that you could also override this DoStop function... } + //! static function allows GUI to present tooltip before any allocation + /*! Not required to implement, but nice! */ static std::string getClassDescription() { - // This string will be shown by the HelpControl or by the tooltips - // of the Controller GUI (not required, but nice) - return "A little demo of text output"; + return "A little demo of text output, sends some lines of text " + "to serr, sout, cout, and printf."; } + + //! virtual function provides description of instance of unknown type + /*! Usually just a matter of forwarding call to getClassDescription(). + * Again, not required to do, but nice! :) */ virtual std::string getDescription() const { - // We override this function to return the string we supplied - // above (not required, but nice) return getClassDescription(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/KinematicSampleBehavior2.h ./Behaviors/Demos/KinematicSampleBehavior2.h --- ../Tekkotsu_3.0/Behaviors/Demos/KinematicSampleBehavior2.h 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Demos/KinematicSampleBehavior2.h 2007-11-12 13:00:38.000000000 -0500 @@ -35,6 +35,8 @@ } virtual void processEvent(const EventBase& e) { + using std::cout; + using std::endl; if(e.getGeneratorID()==EventBase::buttonEGID) { switch(e.getSourceID()) { case LFrPawOffset: diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/LocalizationBehavior.h ./Behaviors/Demos/LocalizationBehavior.h --- ../Tekkotsu_3.0/Behaviors/Demos/LocalizationBehavior.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Demos/LocalizationBehavior.h 2007-11-11 18:57:18.000000000 -0500 @@ -0,0 +1,275 @@ +//-*-c++-*- +#ifndef INCLUDED_LocalizationBehavior_h_ +#define INCLUDED_LocalizationBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Events/EventRouter.h" +#include "DualCoding/DualCoding.h" +//these are included indirectly via DualCoding.h: +//#include "Shared/ParticleFilter.h" +//#include "Shared/LocalizationParticle.h" + +#include "Motion/LedMC.h" +#include "Motion/MMAccessor.h" +#include "Motion/MotionManager.h" + +//! A behavior which tracks the robots location using a dual-coding-based "shape sensor" +/*! The behavior draws 10% of the particles on the world map for visualization, and updates + * the motion model once a second if no landmarks have been seen, so you can + * press "refresh" in the world map and get an updated view of how the particles + * have moved. + * + * The landmarks are expected to be arranged as defined by setupLandmarksOnMap(). + * If you want to define what your world looks like, edit the function to provide your own layout.*/ +class LocalizationBehavior : public DualCoding::VisualRoutinesBehavior { +public: + + //! constructor + LocalizationBehavior() : VisualRoutinesBehavior("LocalizationBehavior"), + mapreq(DualCoding::MapBuilderRequest::localMap), lastvis(-1U), leds_id(MotionManager::invalid_MC_ID) + {} + + virtual void DoStart() { + VisualRoutinesBehavior::DoStart(); // do this first (required) + + // PARTICLE FILTER SETUP + // have to wait to do this in DoStart instead of constructor because the global + // VRmixin::particleFilter isn't created until a client behavior is started. + + // changing some parameters in the default resampler + typedef DualCoding::PFShapeLocalization::LowVarianceResamplingPolicy ResamplingPolicy; + if(ResamplingPolicy* resample = dynamic_cast(particleFilter->getResamplingPolicy())) { + // by default, resampling occurs on each update (delay=0) + // Since groundplane projection is quite noisy while walking, we'll need quite a few more + // samples before we get a good estimate of particle accuracy + resample->resampleDelay=30; + + // by default the minimum is -FLT_MAX (essentially no minimum) + // we'll require particles to have at least some feasability or we'll randomize to try to re-localize + resample->minAcceptableWeight=log(1.0e-13); // logarthmic scale, avoids numeric issues (configurable) + } + + // If you want to change the resampling variance: + // This controls how "tight" the cluster can get after we evaluate the particles + // Regarding the typedef: every resampling policy embeds a distribution policy, the + // default policy is specified by the particle's own DistributionPolicy typedef + typedef DualCoding::PFShapeLocalization::particle_type::DistributionPolicy DistributionPolicy; + if(DistributionPolicy* dp = dynamic_cast(&particleFilter->getResamplingPolicy()->getDistributionPolicy())) { + dp->positionVariance*=1; + dp->orientationVariance*=1; + } + + // specifying motion model variance (how much do particles spread over time) + // note that if we tracked robot velocity as a particle parameter, we could use resampling variance instead... + // (and then get an estimate of actual velocity as well!) + typedef HolonomicMotionModel MotionModel; + if(MotionModel * motion = dynamic_cast(particleFilter->getMotionModel())) { + // these are currently the default parameters, but explicitly reset for demonstration: + motion->setVariance(.25,.25,.25); + } + + + // MAP BUILDER REQUEST SETUP + const int pink_index = ProjectInterface::getColorIndex("pink"); + const int yellow_index = ProjectInterface::getColorIndex("yellow"); + //const int orange_index = ProjectInterface::getColorIndex("orange"); + mapreq.objectColors[DualCoding::ellipseDataType].insert(pink_index); + mapreq.objectColors[DualCoding::ellipseDataType].insert(yellow_index); + //mapreq.objectColors[ellipseDataType].insert(orange_index); + + // a hard coded gound plane corresponding to the default walk parameters + // better to let it figure it out from sensors while walking, but this is better if standing still... (but only in the walk position!) + //mapreq.setCustomGroundPlane(0.20944, 115); + mapreq.maxDist=2000; + + + // EVENT LISTENER SETUP + // only do processing on camera frames where there's possibly something to see + erouter->addListener(this, EventBase::visObjEGID, ProjectInterface::visPinkBallSID, EventBase::statusETID); + erouter->addListener(this, EventBase::visObjEGID, ProjectInterface::visYellowBallSID, EventBase::statusETID); + //erouter->addListener(this, EventBase::visObjEGID, //ProjectInterface::visOrangeSID, EventBase::statusETID); + + erouter->addTimer(this,-1U,1000,true); // update motion model regularly (even if not seeing anything) + + + // MOTION COMMAND SETUP + // using leds for displaying confidence + SharedObject leds_mc; + leds_id = motman->addPersistentMotion(leds_mc); + } + + virtual void DoStop() { + motman->removeMotion(leds_id); + VisualRoutinesBehavior::DoStop(); // do this last (required) + } + + //! this function defines the expected layout of the world + void setupLandmarksOnMap() { + // a pair of ellipses (pink and yellow) + NEW_SHAPE(pinkm,DualCoding::EllipseData,new DualCoding::EllipseData(worldShS,DualCoding::Point(65,0,0,DualCoding::allocentric),27.5,27.5)); + pinkm->setColor("pink"); + pinkm->setMobile(false); + NEW_SHAPE(yellowm,DualCoding::EllipseData,new DualCoding::EllipseData(worldShS,DualCoding::Point(-65,0,0,DualCoding::allocentric),27.5,27.5)); + yellowm->setColor("yellow"); + yellowm->setMobile(false); + + // a square marked by ellipses in each corner + // pink on the "top" edge, and yellow on the "bottom" + /* + NEW_SHAPE(pinkm,EllipseData,new EllipseData(worldShS,Point(700,700,0,allocentric),27.5,27.5)); + pinkm->setColor("pink"); + pinkm->setMobile(false); + NEW_SHAPE(yellowm,EllipseData,new EllipseData(worldShS,Point(700,0,0,allocentric),27.5,27.5)); + yellowm->setColor("yellow"); + yellowm->setMobile(false); + NEW_SHAPE(pink2m,EllipseData,new EllipseData(worldShS,Point(0,700,0,allocentric),27.5,27.5)); + pink2m->setColor("pink"); + pink2m->setMobile(false); + NEW_SHAPE(yellow2m,EllipseData,new EllipseData(worldShS,Point(0,0,0,allocentric),27.5,27.5)); + yellow2m->setColor("yellow"); + yellow2m->setMobile(false); + */ + } + + //! by uncommenting some of the erouter calls, you can make it skip frames (currently tries to process all) + virtual void processEvent(const EventBase& event) { + switch (event.getGeneratorID()){ + + case EventBase::visObjEGID: + { + // update local map + const VisionObjectEvent &visev = dynamic_cast(event); + unsigned int curvis = visev.getFrame(); + if (curvis == lastvis){ + std::cout << "Current = Previous" << std::endl; + //erouter->removeListener(this,event); + //erouter->addTimer(this,event.getSourceID(),1000,false); + return ; + } + lastvis = curvis; + std::cout << "Localizing off of " << event.getName() << "... " << std::endl; + + //unsigned int mapreq_id = + mapBuilder.executeRequest(mapreq); + // we're using an "immediate" map builder request, so we don't have to wait for the event... + //erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id, EventBase::statusETID); + + // now update the particle filter + worldShS.clear(); // clear particles and other junk we may have drawn on the map as feedback + setupLandmarksOnMap(); // refresh the landmarks + particleFilter->update(); // ask the particle filter to match the local space against the world space for each particle + erouter->addTimer(this,-1U,1000,true); // delay motion model update (since we just updated on previous line) + + // now update the display + updateAgentOnMap(); + displayParticlesOnMap(); + ledSettings(); + + //erouter->removeListener(this,event); + //erouter->addTimer(this,event.getSourceID(), 1000,false); + break; + } + + case EventBase::timerEGID: + { + if(event.getSourceID()==-1U) { + std::cout << "Updating motion model!" << std::endl; + particleFilter->updateMotion(); + + // refresh the display + worldShS.clear(); + setupLandmarksOnMap(); + displayParticlesOnMap(); + updateAgentOnMap(); + + } else { + // this won't be called unless you comment-in the lines which skip camera frames + std::cout << "Re-enabling vision listener" << std::endl; + erouter->addListener(this, EventBase::visObjEGID, event.getSourceID(), EventBase::statusETID); + } + break; + } + + default: + std::cout << "Unexpected event: " << event.getDescription() << std::endl; + break; + } + } + + //! places the agent at the most likely particle + void updateAgentOnMap() { + const DualCoding::PFShapeLocalization::particle_type& p = particleFilter->getBestParticle(); + std::cout << "Best index: " << particleFilter->getBestIndex() << " with score: " << p.weight << std::endl; + mapBuilder.setAgent(DualCoding::Point(p.x,p.y), p.theta); + std::cout << "DATA1 Current positions: " << p.x << " " << p.y << " " << p.theta << std::endl; + } + + //! draws a sample (10%) of the particles on the map + void displayParticlesOnMap() { + particleFilter->displayParticles(); + //or manually: + /*if(particleFilter->getParticles().size() < 10) + cout << "CURRENT PARTICLES LESS THAN 10!" << endl; + for(unsigned int i = 0; i < particleFilter->getParticles().size(); i+=10) { + const ShapeLocalizationPF::particle_type& p = particleFilter->getParticles()[i]; + NEW_SHAPE(pt, PointData, new PointData(worldShS, Point(p.x,p.y,0,allocentric))); + }*/ + } + + //! updates the LEDs to show the confidence interval; the smaller the confidence interval, the more LEDs will turn on + void ledSettings() { + float confidence = particleFilter->getConfidenceInterval(); + std::cout << std::endl << "Confidence: " << confidence << std::endl << std::endl; + MMAccessor leds_acc(leds_id); + + float c = 0; + if(confidence < 500 && confidence>=0) { + //linear dropoff: + // c=(1-confidence/500); + + //or exponential dropoff: + c=exp(-(confidence-60)/150); + } + leds_acc->displayPercent(c,LedEngine::major,LedEngine::major); + } + + static std::string getClassDescription() { return "A behavior which tracks the robots location using a dual-coding-based \"shape sensor\""; } + virtual std::string getDescription() const { return getClassDescription(); } + + +protected: + //! used to request the DualCoding::MapBuilder project cam-space shapes to the local-space + DualCoding::MapBuilderRequest mapreq; + + //! serial number of the last processed camera frame + /*! if there are multiple landmarks in the same image, we don't want to process + * the same image multiple times for each landmark. Tracking the camera frame + * makes sure we only do each frame at most once */ + unsigned int lastvis; + + //! used to display the confidence of the particle filter on the LEDs + MotionManager::MC_ID leds_id; + + +private: + // Providing declarations for these functions will avoid a compiler warning if + // you have any class members which are pointers. However, as it is, an error + // will result if you inadvertantly cause a call to either (which is probably + // a good thing, unless you really intended to copy/assign a behavior, in + // which case simply provide implementations for the functions) + LocalizationBehavior(const LocalizationBehavior&); //!< don't call (copy constructor) + LocalizationBehavior& operator=(const LocalizationBehavior&); //!< don't call (assignment operator) +}; + +/*! @file + * @brief Defines LocalizationBehavior, which tracks the robots location using a dual-coding-based "shape sensor" + * @author Ashley Johnson and Ethan Tira-Thompson (Creators) + * + * $Author: ejt $ + * $Name: tekkotsu-4_0 $ + * $Revision: 1.6 $ + * $State: Exp $ + * $Date: 2007/11/11 23:57:18 $ + */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/LookForSoundBehavior.h ./Behaviors/Demos/LookForSoundBehavior.h --- ../Tekkotsu_3.0/Behaviors/Demos/LookForSoundBehavior.h 2004-12-22 20:47:06.000000000 -0500 +++ ./Behaviors/Demos/LookForSoundBehavior.h 2007-08-05 12:16:04.000000000 -0400 @@ -5,6 +5,9 @@ #include "Behaviors/BehaviorBase.h" #include "Motion/HeadPointerMC.h" #include "Events/DataEvent.h" +#include "Events/EventRouter.h" +#include "Shared/ODataFormats.h" +#include "Motion/MMAccessor.h" //! Turns head to sound source, estimated by average volume difference between left and right ears class LookForSoundBehavior : public BehaviorBase { @@ -51,7 +54,7 @@ // If there is sufficient energy coming in if( l+r > sz*1000.){ MMAccessor mc(mc_id); - double cur = state->outputs[ERS7Info::HeadOffset+1]; + double cur = state->outputs[HeadOffset+PanOffset]; if( r > 1.3*l) // Move your head righward mc->setJoints( 0, cur-.2*M_PI/(r/l), 0); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/MotionStressTestBehavior.h ./Behaviors/Demos/MotionStressTestBehavior.h --- ../Tekkotsu_3.0/Behaviors/Demos/MotionStressTestBehavior.h 2005-08-07 00:11:03.000000000 -0400 +++ ./Behaviors/Demos/MotionStressTestBehavior.h 2007-11-12 13:00:38.000000000 -0500 @@ -31,7 +31,7 @@ ms->setOutputCmd(i,0); MotionManager::MC_ID id=motman->addPrunableMotion(ms); curMotions.push(id); - cout << get_time() << "\tAdded id " << id << endl; + std::cout << get_time() << "\tAdded id " << id << std::endl; addMS(LFrLegOrder,3000); addMS(RFrLegOrder,4000); addMS(LBkLegOrder,5000); @@ -50,11 +50,11 @@ virtual void processEvent(const EventBase& e) { if(e.getTypeID()==EventBase::deactivateETID) { if(e.getSourceID()!=curMotions.front()) { - cout << e.getSourceID() << " is not mine or is out of order" << endl; + std::cout << e.getSourceID() << " is not mine or is out of order" << std::endl; } else { curMotions.pop(); } - cout << get_time() << "\t Removed id " << e.getSourceID() << endl; + std::cout << get_time() << "\t Removed id " << e.getSourceID() << std::endl; addMS(nextLeg,3000); nextLeg=static_cast((nextLeg+1)%4); } @@ -72,7 +72,7 @@ ms->setOutputCmd(index,outputRanges[index][MaxRange]); MotionManager::MC_ID id=motman->addPrunableMotion(ms); curMotions.push(id); - cout << get_time() << "\tAdded id " << id << endl; + std::cout << get_time() << "\tAdded id " << id << std::endl; } static std::string getClassDescription() { return "uses a separate MotionCommand for each of several joints to test for region leaks"; } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/PaceTargetsMachine.cc ./Behaviors/Demos/PaceTargetsMachine.cc --- ../Tekkotsu_3.0/Behaviors/Demos/PaceTargetsMachine.cc 2006-09-18 14:07:57.000000000 -0400 +++ ./Behaviors/Demos/PaceTargetsMachine.cc 2007-07-16 14:31:39.000000000 -0400 @@ -1,3 +1,6 @@ +#include "Shared/RobotInfo.h" +#ifdef TGT_HAS_IR_DISTANCE + #include "PaceTargetsMachine.h" #include "Behaviors/Transition.h" #include "Behaviors/Nodes/WalkToTargetNode.h" @@ -88,9 +91,10 @@ * @author ejt (Creator) * * $Author: ejt $ - * $Name: tekkotsu-3_0 $ - * $Revision: 1.20 $ + * $Name: tekkotsu-4_0 $ + * $Revision: 1.22 $ * $State: Exp $ - * $Date: 2006/09/18 18:07:57 $ + * $Date: 2007/07/16 18:31:39 $ */ +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/PaceTargetsMachine.h ./Behaviors/Demos/PaceTargetsMachine.h --- ../Tekkotsu_3.0/Behaviors/Demos/PaceTargetsMachine.h 2005-01-24 17:23:46.000000000 -0500 +++ ./Behaviors/Demos/PaceTargetsMachine.h 2007-07-16 14:31:39.000000000 -0400 @@ -2,6 +2,11 @@ #ifndef INCLUDED_PaceTargetsMachine_h_ #define INCLUDED_PaceTargetsMachine_h_ +#include "Shared/RobotInfo.h" +#ifndef TGT_HAS_IR_DISTANCE +#error PaceTargetsMachine requires a distance rangefinder via IRDistOffset +#endif + #include "Behaviors/StateNode.h" #include "Shared/ProjectInterface.h" diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/RelaxBehavior.h ./Behaviors/Demos/RelaxBehavior.h --- ../Tekkotsu_3.0/Behaviors/Demos/RelaxBehavior.h 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Demos/RelaxBehavior.h 2007-11-11 18:57:18.000000000 -0500 @@ -8,8 +8,6 @@ #include "IPC/SharedObject.h" #include "Motion/MotionManager.h" -using namespace std; - //! A behavior that sets all the pids to zero for the tail and legs servos. /*! This should hopefully make the robot quieter and consume less power. */ class RelaxBehavior : public BehaviorBase { diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/SimpleChaseBallBehavior.h ./Behaviors/Demos/SimpleChaseBallBehavior.h --- ../Tekkotsu_3.0/Behaviors/Demos/SimpleChaseBallBehavior.h 2005-05-03 12:30:29.000000000 -0400 +++ ./Behaviors/Demos/SimpleChaseBallBehavior.h 2007-11-15 00:19:22.000000000 -0500 @@ -10,6 +10,7 @@ #include "Events/VisionObjectEvent.h" #include "Shared/WorldState.h" #include "Motion/WalkMC.h" +#include "Shared/ERS7Info.h" //! A simple behavior to chase after any objects seen by the vision system /*! Similar to ChaseBallBehavior, but this one doesn't try to move the head, so @@ -41,10 +42,14 @@ //! sets the head to point at the object and sets the body to move where the head points virtual void processEvent(const EventBase& event) { if(event.getGeneratorID()==EventBase::visObjEGID && event.getTypeID()==EventBase::statusETID) { + // see if the camera is on a pan joint, we're using the ERS-7's pan joint as a prototype of what to look for... + const unsigned int panIdx = capabilities.findOutputOffset(ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::PanOffset]); + float panAngle = (panIdx!=-1U) ? state->outputs[panIdx] : 0; // get its angle, or assume 0 if not found + //x and y are the direction to walk; positive x is forward and positive y is left //so these calculations walk the direction the head is pointing (at 120 mm/sec) - float x=120.0f*cos(state->outputs[HeadOffset+PanOffset]); - float y=120.0f*sin(state->outputs[HeadOffset+PanOffset]); + float x=120.0f*cos(panAngle); + float y=120.0f*sin(panAngle); //z is the amount to turn in radians; conveniently enough, we can use the //x parameter from the vision event as the speed to turn -- you could diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/SoundTestBehavior.h ./Behaviors/Demos/SoundTestBehavior.h --- ../Tekkotsu_3.0/Behaviors/Demos/SoundTestBehavior.h 2006-09-18 14:07:57.000000000 -0400 +++ ./Behaviors/Demos/SoundTestBehavior.h 2007-06-28 00:36:20.000000000 -0400 @@ -14,7 +14,7 @@ * up and then played successively once the chin button is released. * * Notice that this doesn't preload all needed sounds:\n - * - @c barkmed.wav is listed in ms/config/tekkotsu.cfg as a preloaded system sound + * - @c barkmed.wav is listed in ms/config/tekkotsu.xml as a preloaded system sound * - @c growl.wav will be loaded before being played automatically - notice the * hiccup this can cause. */ @@ -27,15 +27,11 @@ RFr(EventBase::buttonEGID,RFrPawOffset,EventBase::activateETID), LBk(EventBase::buttonEGID,LBkPawOffset,EventBase::activateETID), RBk(EventBase::buttonEGID,RBkPawOffset,EventBase::activateETID), - Back(EventBase::buttonEGID,0,EventBase::activateETID) + Back(EventBase::buttonEGID,NumButtons-1,EventBase::activateETID) { - //different models vary widely in their back buttons - if(state->robotDesign & WorldState::ERS210Mask) - Back.setSourceID(ERS210Info::BackButOffset); - else if(state->robotDesign & WorldState::ERS220Mask) - Back.setSourceID(ERS220Info::BackButOffset); - else if(state->robotDesign & WorldState::ERS7Mask) - Back.setSourceID(ERS7Info::FrontBackButOffset); + if(capabilities.getButtonOffset("BackBut")!=-1U) + Back.setSourceID(capabilities.getButtonOffset("BackBut")); + // otherwise unknown model, just use last button (NumButtons-1, as set in constructor) } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/StareAtBallBehavior.cc ./Behaviors/Demos/StareAtBallBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Demos/StareAtBallBehavior.cc 2006-09-26 17:47:00.000000000 -0400 +++ ./Behaviors/Demos/StareAtBallBehavior.cc 2007-11-15 16:29:17.000000000 -0500 @@ -5,6 +5,7 @@ #include "Motion/HeadPointerMC.h" #include "Motion/MMAccessor.h" #include "Shared/ProjectInterface.h" +#include "Shared/ERS7Info.h" //! Converts degrees to radians inline double DtoR(double deg) { return (deg/180.0*M_PI); } @@ -30,22 +31,37 @@ vert=objev.getCenterY(); } + // for portability, look to see if the host hardware has a head pan & tilt joints + const unsigned int panIdx = capabilities.findOutputOffset(ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::PanOffset]); + const unsigned int tiltIdx = capabilities.findOutputOffset(ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::TiltOffset]); + if(panIdx==-1U || tiltIdx==-1U) + return; // guess not... + //cout << horiz << ' ' << vert << endl; // Very simple visual servoing control -- move the head a small distance in the direction of the target // This is "proportional" control, because we move the head proportionally further when the error (horiz and vert) is larger - double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/7; - double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/6; + double tilt=state->outputs[tiltIdx]-vert*CameraFOV/7; + double pan=state->outputs[panIdx]-horiz*CameraFOV/6; // now request access to the headpointer we added in DoStart and set the joint angles MMAccessor headpointer(headpointer_id); - if(state->robotDesign & WorldState::ERS7Mask) { +#ifdef TGT_IS_AIBO + if(RobotName == ERS7Info::TargetName) { //on an ers-7, we want to set the nod joint to look up (maximum value), since tilt can only look down headpointer->setJoints(tilt,pan,outputRanges[HeadOffset+NodOffset][MaxRange]); } else { //on other models (we'll just assume ers-2xx), center the roll joint headpointer->setJoints(tilt,pan,0); } +#else + /* really should do a kinematic solution with lookInDirection, but that assumes + * user has done a .kin file for this robot. Let's just keep it simple and try to + * set the joints directly */ + if(NumHeadJoints>2) + tilt/=2; // we're going to replicate the tilt parameter in the next call, so divide by 2 + headpointer->setJoints(tilt,pan,tilt); +#endif } /*! @file diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/StareAtPawBehavior2.cc ./Behaviors/Demos/StareAtPawBehavior2.cc --- ../Tekkotsu_3.0/Behaviors/Demos/StareAtPawBehavior2.cc 2005-08-04 17:09:26.000000000 -0400 +++ ./Behaviors/Demos/StareAtPawBehavior2.cc 2007-06-28 00:36:20.000000000 -0400 @@ -1,3 +1,6 @@ +#include "Shared/RobotInfo.h" +#if defined(TGT_HAS_LEGS) + #include "StareAtPawBehavior2.h" #include "Events/EventRouter.h" @@ -10,6 +13,7 @@ #include "Motion/roboop/robot.h" #include "Shared/Config.h" #include "Motion/Kinematics.h" +#include "Shared/ERS7Info.h" // these are for drawing into the camera frame #include "Shared/ProjectInterface.h" @@ -33,7 +37,7 @@ //visRawCameraEGID to draw a dot on the camera image where we think the toe is erouter->addListener(this,EventBase::visRawCameraEGID,ProjectInterface::visRawCameraSID,EventBase::statusETID); - if(state->robotDesign == WorldState::ERS7Mask) + if(RobotName == ERS7Info::TargetName) sout->printf("NOTICE: The ERS-7 has a rather \"sticky\" nod joint\n" "(the upper tilt joint). This can cause it to hesitate\n" "or altogether fail to precisely center the target position\n" @@ -125,6 +129,8 @@ } } +#endif // has legs + /*! @file * @brief Implements StareAtPawBehavior2, which uses new-style ROBOOP kinematics to track the paw which last received a button press with the camera * @author ejt (Creator) diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/TestBehaviors.h ./Behaviors/Demos/TestBehaviors.h --- ../Tekkotsu_3.0/Behaviors/Demos/TestBehaviors.h 2006-09-05 15:40:07.000000000 -0400 +++ ./Behaviors/Demos/TestBehaviors.h 2007-11-11 18:57:18.000000000 -0500 @@ -51,7 +51,7 @@ class SuicidalBehavior : public BehaviorBase { public: SuicidalBehavior() : BehaviorBase("SuicidalBehavior") {} //!< constructor - ~SuicidalBehavior() { std::cout << getName() << " destructed" << endl; } //!< destructor + ~SuicidalBehavior() { std::cout << getName() << " destructed" << std::endl; } //!< destructor virtual void DoStart() { BehaviorBase::DoStart(); erouter->addTimer(this,0,1000); std::cout << "One second to live!" << std::endl; } virtual void processEvent(const EventBase& /*event*/) { std::cout << "I'm stopping -- refresh controller to see if it worked!" << std::endl; DoStop(); } static std::string getClassDescription() { return "Stops itself after a second via timer event"; } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/ToggleHeadLightBehavior.h ./Behaviors/Demos/ToggleHeadLightBehavior.h --- ../Tekkotsu_3.0/Behaviors/Demos/ToggleHeadLightBehavior.h 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Demos/ToggleHeadLightBehavior.h 2007-06-26 00:27:43.000000000 -0400 @@ -15,7 +15,7 @@ //! opens the head light virtual void DoStart() { BehaviorBase::DoStart(); - if(state->robotDesign & WorldState::ERS220Mask) { + if(RobotName == ERS220Info::TargetName) { SharedObject pose; pose->setOutputCmd(ERS220Info::RetractableHeadLEDOffset,true); light_id=motman->addPersistentMotion(pose); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/WallTestBehavior.cc ./Behaviors/Demos/WallTestBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Demos/WallTestBehavior.cc 2006-07-13 13:25:50.000000000 -0400 +++ ./Behaviors/Demos/WallTestBehavior.cc 2007-11-12 13:00:38.000000000 -0500 @@ -1,3 +1,6 @@ +#include "Shared/RobotInfo.h" +#ifdef TGT_HAS_IR_DISTANCE + #include "WallTestBehavior.h" #include "Shared/newmat/newmat.h" #include "Shared/newmat/newmatap.h" @@ -10,6 +13,8 @@ #include "Shared/TimeET.h" #include +using namespace std; + void WallTestBehavior::DoStart() { BehaviorBase::DoStart(); // do this first @@ -89,7 +94,7 @@ } else if(e.getSourceID()==0) { // must be a timer event -- SID 0 indicates start of recording // so start subscribing to sensor events - erouter->addListener(this,EventBase::sensorEGID,SensorSourceID::UpdatedSID); + erouter->addListener(this,EventBase::sensorEGID,SensorSrcID::UpdatedSID); } else if(e.getSourceID()==1) { // must be a timer event -- SID 1 indicates stop of recording @@ -283,3 +288,5 @@ x0=V(1,V.ncols())/V(3,V.ncols()); x1=V(2,V.ncols())/V(3,V.ncols()); } + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Demos/karmedbandit.h ./Behaviors/Demos/karmedbandit.h --- ../Tekkotsu_3.0/Behaviors/Demos/karmedbandit.h 2003-09-18 18:40:27.000000000 -0400 +++ ./Behaviors/Demos/karmedbandit.h 2007-11-12 13:00:38.000000000 -0500 @@ -24,18 +24,18 @@ unsigned int decide() { std::vector p(w.size()); double wsum=0; - cout << "w ="; + std::cout << "w ="; for(unsigned int i=0; iaddListener(this, EventBase::estopEGID); // Turn on wireless - cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 300, 300); + cmdsock=wireless->socket(Socket::SOCK_STREAM, 300, 300); wireless->setDaemon(cmdsock,true); wireless->setReceiver(cmdsock->sock, callback); wireless->listen(cmdsock->sock, config->main.estopControl_port); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/EStopControllerBehavior.h ./Behaviors/Mon/EStopControllerBehavior.h --- ../Tekkotsu_3.0/Behaviors/Mon/EStopControllerBehavior.h 2004-12-10 18:18:10.000000000 -0500 +++ ./Behaviors/Mon/EStopControllerBehavior.h 2007-01-30 17:56:19.000000000 -0500 @@ -41,7 +41,7 @@ static std::string getClassDescription() { char tmp[20]; - sprintf(tmp,"%d",config->main.estopControl_port); + sprintf(tmp,"%d",*config->main.estopControl_port); return std::string("Listens to estop commands coming in from port ")+tmp; } virtual std::string getDescription() const { return getClassDescription(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/EchoBehavior.cc ./Behaviors/Mon/EchoBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Mon/EchoBehavior.cc 2005-08-01 19:17:59.000000000 -0400 +++ ./Behaviors/Mon/EchoBehavior.cc 2007-05-21 16:51:20.000000000 -0400 @@ -27,8 +27,8 @@ } void EchoBehavior::setupNetwork() { - sockets[STCP]=wireless->socket(SocketNS::SOCK_STREAM); - sockets[SUDP]=wireless->socket(SocketNS::SOCK_DGRAM); + sockets[STCP]=wireless->socket(Socket::SOCK_STREAM); + sockets[SUDP]=wireless->socket(Socket::SOCK_DGRAM); socks[STCP]=sockets[STCP]->sock; socks[SUDP]=sockets[SUDP]->sock; wireless->setDaemon(sockets[STCP],true); @@ -72,14 +72,14 @@ } if(string_util::makeLower(args[1])=="tcp") { if(sockets[CTCP]==NULL) { - sockets[CTCP]=wireless->socket(SocketNS::SOCK_STREAM); + sockets[CTCP]=wireless->socket(Socket::SOCK_STREAM); socks[CTCP]=sockets[CTCP]->sock; wireless->setReceiver(sockets[CTCP], client_callbackT); } wireless->connect(sockets[CTCP],args[2].c_str(),atoi(args[3].c_str())); } else if(string_util::makeLower(args[1])=="udp") { if(sockets[CUDP]==NULL) { - sockets[CUDP]=wireless->socket(SocketNS::SOCK_DGRAM); + sockets[CUDP]=wireless->socket(Socket::SOCK_DGRAM); socks[CUDP]=sockets[CUDP]->sock; wireless->setReceiver(sockets[CUDP], client_callbackU); } @@ -100,8 +100,8 @@ cout << endl; } } else if(args[0]=="relay" || args[0]=="unlink") { - unsigned char from=-1U; - unsigned char to=-1U; + unsigned char from=(unsigned char)-1U; + unsigned char to=(unsigned char)-1U; bool val = (args[0]=="relay"); unsigned int i=1; for(; i0?MaxRange:MinRange]; @@ -34,8 +37,38 @@ r = fabs(param)*outputRanges[HeadOffset+RollOffset][param>0?MaxRange:MinRange]; break; default: - cout << "MECHA: unknown command " << command[0] << endl; + std::cout << "MECHA: unknown command " << command[0] << std::endl; } +#else + switch(command[0]) { + case CMD_tilt: { + const char* n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::TiltOffset]; + unsigned int i = capabilities.findOutputOffset(n); + if(i!=-1U) + t = fabs(param)*outputRanges[i][param>0?MaxRange:MinRange]; + } break; + case CMD_pan: { + const char* n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::PanOffset]; + unsigned int i = capabilities.findOutputOffset(n); + if(i!=-1U) + p = fabs(param)*outputRanges[i][param>0?MaxRange:MinRange]; + } break; + case CMD_roll: { + const char* n = ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::NodOffset]; + unsigned int i = capabilities.findOutputOffset(n); + if(i!=-1U) + r = fabs(param)*outputRanges[i][param>0?MaxRange:MinRange]; + else { + n = ERS210Info::outputNames[ERS210Info::HeadOffset+ERS210Info::RollOffset]; + i = capabilities.findOutputOffset(n); + if(i!=-1U) + r = fabs(param)*outputRanges[i][param>0?MaxRange:MinRange]; + } + } break; + default: + std::cout << "MECHA: unknown command " << command[0] << std::endl; + } +#endif // If the command was a new motion command, apply the // new motion parameters: @@ -54,11 +87,13 @@ // Behavior startup BehaviorBase::DoStart(); // Enable head control - head_id = motman->addPersistentMotion(SharedObject()); + SharedObject head; + head->setHold(false); + head_id = motman->addPersistentMotion(head); // Turn on wireless theLastOne=theOne; theOne=this; - cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 2048, 2048); + cmdsock=wireless->socket(Socket::SOCK_STREAM, 2048, 2048); wireless->setReceiver(cmdsock->sock, mechacmd_callback); wireless->setDaemon(cmdsock,true); wireless->listen(cmdsock->sock, config->main.headControl_port); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/HeadPointControllerBehavior.h ./Behaviors/Mon/HeadPointControllerBehavior.h --- ../Tekkotsu_3.0/Behaviors/Mon/HeadPointControllerBehavior.h 2005-09-13 18:10:36.000000000 -0400 +++ ./Behaviors/Mon/HeadPointControllerBehavior.h 2007-01-30 17:56:19.000000000 -0500 @@ -79,7 +79,7 @@ static std::string getClassDescription() { char tmp[20]; - sprintf(tmp,"%d",config->main.headControl_port); + sprintf(tmp,"%d",*config->main.headControl_port); return std::string("Listens to head control commands coming in from port ")+tmp; } virtual std::string getDescription() const { return getClassDescription(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/MicrophoneServer.cc ./Behaviors/Mon/MicrophoneServer.cc --- ../Tekkotsu_3.0/Behaviors/Mon/MicrophoneServer.cc 2005-08-07 00:11:03.000000000 -0400 +++ ./Behaviors/Mon/MicrophoneServer.cc 2007-11-21 15:57:13.000000000 -0500 @@ -13,11 +13,11 @@ MicrophoneServer* MicrophoneServer::instance = 0; const char* const MicrophoneServer::MIC_LOCATOR = "PRM:/r1/c1/c2/c3/m1-Mic:M1"; -MicrophoneServer* MicrophoneServer::GetInstance() { +MicrophoneServer& MicrophoneServer::getInstance() { if (instance == 0) { instance = new MicrophoneServer(); } - return instance; + return *instance; } MicrophoneServer::MicrophoneServer() @@ -42,7 +42,7 @@ socket = 0; } - socket = wireless->socket(SocketNS::SOCK_STREAM, 512, SEND_BUFFER_SIZE); + socket = wireless->socket(Socket::SOCK_STREAM, 512, SEND_BUFFER_SIZE); wireless->setDaemon(socket, true); wireless->listen(socket->sock, config->sound.streaming.mic_port); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/MicrophoneServer.h ./Behaviors/Mon/MicrophoneServer.h --- ../Tekkotsu_3.0/Behaviors/Mon/MicrophoneServer.h 2006-09-16 13:32:38.000000000 -0400 +++ ./Behaviors/Mon/MicrophoneServer.h 2007-11-21 15:57:13.000000000 -0500 @@ -9,7 +9,7 @@ class MicrophoneServer : public BehaviorBase { public: //!enforces singleton status - static MicrophoneServer* GetInstance(); + static MicrophoneServer& getInstance(); virtual ~MicrophoneServer(); //!< destructor virtual void DoStart(); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/RawCamBehavior.cc ./Behaviors/Mon/RawCamBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Mon/RawCamBehavior.cc 2006-09-14 14:57:03.000000000 -0400 +++ ./Behaviors/Mon/RawCamBehavior.cc 2007-06-26 00:27:44.000000000 -0400 @@ -36,8 +36,8 @@ RawCamBehavior::processEvent(const EventBase& e) { if(!wireless->isConnected(visRaw->sock)) return; - if(config->vision.rawcam_transport==0 && visRaw->getTransport()==SocketNS::SOCK_STREAM - || config->vision.rawcam_transport==1 && visRaw->getTransport()==SocketNS::SOCK_DGRAM) { + if(config->vision.rawcam.transport==0 && visRaw->getTransport()==Socket::SOCK_STREAM + || config->vision.rawcam.transport==1 && visRaw->getTransport()==Socket::SOCK_DGRAM) { //reset the socket closeServer(); setupServer(); @@ -49,15 +49,15 @@ CameraStreamBehavior::processEvent(e); return; } - if ((get_time() - lastProcessedTime) < config->vision.rawcam_interval) {// not enough time has gone by + if ((get_time() - lastProcessedTime) < config->vision.rawcam.interval) {// not enough time has gone by return; } /* // turning these off enables individual channel compression - if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE && e.getGeneratorID()!=EventBase::visRawCameraEGID) + if(config->vision.rawcam.compression==Config::vision_config::COMPRESS_NONE && e.getGeneratorID()!=EventBase::visRawCameraEGID) return; - if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG && e.getGeneratorID()!=EventBase::visJPEGEGID) + if(config->vision.rawcam.compression==Config::vision_config::COMPRESS_JPEG && e.getGeneratorID()!=EventBase::visJPEGEGID) return; */ - if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_COLOR) { + if(config->vision.rawcam.encoding==Config::vision_config::RawCamConfig::ENCODE_COLOR) { if(!writeColor(*fbke)) { if(packet) { //packet was opened, need to close it cur=packet; // don't send anything @@ -66,7 +66,7 @@ //error message should already be printed in writeColor //ASSERTRET(false,"serialization failed"); } - } 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) { if(!writeSingleChannel(*fbke)) { if(packet) { //packet was opened, need to close it cur=packet; // don't send anything @@ -76,7 +76,7 @@ //ASSERTRET(false,"serialization failed"); } } else { - serr->printf("%s: Bad rawcam_encoding setting\n",getName().c_str()); + serr->printf("%s: Bad rawcam.encoding setting\n",getName().c_str()); } } catch(...) { if(packet) { //packet was opened, need to close it @@ -91,26 +91,26 @@ } unsigned int RawCamBehavior::getSourceLayer(unsigned int chan, unsigned int numLayers) { - if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_SINGLE_CHANNEL) { - if(config->vision.rawcam_channel!=(int)chan) + if(config->vision.rawcam.encoding==Config::vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL) { + if(config->vision.rawcam.channel!=(int)chan) return -1U; - return numLayers-1-config->vision.rawcam_y_skip; + return numLayers-1-config->vision.rawcam.y_skip; } // must be full-color switch(chan) { case RawCameraGenerator::CHAN_Y: - if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG) { - if(config->vision.rawcam_y_skip-config->vision.rawcam_uv_skip == 1) - return numLayers-1-config->vision.rawcam_uv_skip; + if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_JPEG) { + if(config->vision.rawcam.y_skip-config->vision.rawcam.uv_skip == 1) + return numLayers-1-config->vision.rawcam.uv_skip; } - return numLayers-1-config->vision.rawcam_y_skip; + return numLayers-1-config->vision.rawcam.y_skip; case RawCameraGenerator::CHAN_U: case RawCameraGenerator::CHAN_V: - if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG) { - if(config->vision.rawcam_uv_skip-config->vision.rawcam_y_skip == 1) - return numLayers-1-config->vision.rawcam_y_skip; + if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_JPEG) { + if(config->vision.rawcam.uv_skip-config->vision.rawcam.y_skip == 1) + return numLayers-1-config->vision.rawcam.y_skip; } - return numLayers-1-config->vision.rawcam_uv_skip; + return numLayers-1-config->vision.rawcam.uv_skip; default: // other channels, i.e. Y-derivatives return -1U; } @@ -142,25 +142,25 @@ std::vector args; args.push_back("raw"); char port[50]; - snprintf(port,50,"%d",config->vision.rawcam_port); + snprintf(port,50,"%d",*config->vision.rawcam.port); args.push_back(port); - if(config->vision.rawcam_transport==0) { + if(config->vision.rawcam.transport==0) { max_buf=UDP_WIRELESS_BUFFER_SIZE; - visRaw=wireless->socket(SocketNS::SOCK_DGRAM, 1024, max_buf); + visRaw=wireless->socket(Socket::SOCK_DGRAM, 1024, max_buf); args.push_back("udp"); - } else if(config->vision.rawcam_transport==1) { + } else if(config->vision.rawcam.transport==1) { max_buf=TCP_WIRELESS_BUFFER_SIZE; - visRaw=wireless->socket(SocketNS::SOCK_STREAM, 1024, max_buf); + visRaw=wireless->socket(Socket::SOCK_STREAM, 1024, max_buf); args.push_back("tcp"); } else { - serr->printf("ERROR: Invalid Config::vision.rawcam_transport: %d\n",config->vision.rawcam_transport); + serr->printf("ERROR: Invalid Config::vision.rawcam.transport: %d\n",*config->vision.rawcam.transport); return; } wireless->setDaemon(visRaw,true); wireless->setReceiver(visRaw,networkCallback); - wireless->listen(visRaw,config->vision.rawcam_port); + wireless->listen(visRaw,config->vision.rawcam.port); - Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RawVisionGUI",config->vision.rawcam_port,args); + Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RawVisionGUI",*config->vision.rawcam.port,args); } bool @@ -172,11 +172,13 @@ ASSERT(cur==NULL,"cur non-NULL"); cur=NULL; char * buf=packet=(char*)visRaw->getWriteBuffer(avail); - ASSERTRETVAL(packet!=NULL,"dropped frame, network bandwidth is saturated (reduce frame rate or size)",false); + ASSERT(packet!=NULL,"dropped frame, network bandwidth is saturated (reduce frame rate or size)"); + if(packet==NULL) + return false; if(!LoadSave::encodeInc("TekkotsuImage",buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; - if(!LoadSave::encodeInc(config->vision.rawcam_encoding,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; - if(!LoadSave::encodeInc(config->vision.rawcam_compression,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; + if(!LoadSave::encodeInc(*config->vision.rawcam.encoding,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; + if(!LoadSave::encodeInc(*config->vision.rawcam.compression,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; if(!LoadSave::encodeInc(fbkgen.getWidth(layer),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; if(!LoadSave::encodeInc(fbkgen.getHeight(layer),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; @@ -191,12 +193,12 @@ RawCamBehavior::writeColor(const FilterBankEvent& e) { FilterBankGenerator& fbkgen=*e.getSource(); - unsigned int y_layer=fbkgen.getNumLayers()-1-config->vision.rawcam_y_skip; - unsigned int uv_layer=fbkgen.getNumLayers()-1-config->vision.rawcam_uv_skip; + unsigned int y_layer=fbkgen.getNumLayers()-1-config->vision.rawcam.y_skip; + unsigned int uv_layer=fbkgen.getNumLayers()-1-config->vision.rawcam.uv_skip; - if(config->vision.rawcam_channel==-1) { - if(e.getGeneratorID()==EventBase::visRawCameraEGID && config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE - || e.getGeneratorID()==EventBase::visJPEGEGID && config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG) { + if(config->vision.rawcam.channel==-1) { + if(e.getGeneratorID()==EventBase::visRawCameraEGID && config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE + || e.getGeneratorID()==EventBase::visJPEGEGID && config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_JPEG) { if(const JPEGGenerator* jgen=dynamic_cast(&fbkgen)) if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_COLOR) return true; @@ -212,10 +214,10 @@ if(!LoadSave::encodeInc("blank",cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U); - if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam_transport to TCP and reopen raw cam")) return false; + if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false; fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V); - if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam_transport to TCP and reopen raw cam")) return false; + if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false; closePacket(); } @@ -229,7 +231,7 @@ small_layer=y_layer; } if(const JPEGGenerator* jgen=dynamic_cast(&fbkgen)) { - if(config->vision.rawcam_compression!=Config::vision_config::COMPRESS_JPEG) + if(config->vision.rawcam.compression!=Config::vision_config::RawCamConfig::COMPRESS_JPEG) return true; if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_COLOR && big_layer-small_layer<2) { openPacket(fbkgen,e.getTimeStamp(),big_layer); @@ -237,7 +239,7 @@ return false; fbkgen.selectSaveImage(big_layer,0); - if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam_transport to TCP and reopen raw cam")) return false; + if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false; closePacket(); } else if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_GRAYSCALE && big_layer-small_layer>=2) { @@ -247,12 +249,12 @@ if(big_layer==y_layer) { fbkgen.selectSaveImage(y_layer,RawCameraGenerator::CHAN_Y); - if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam_transport to TCP and reopen raw cam")) return false; + if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false; } else { fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U); - if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam_transport to TCP and reopen raw cam")) return false; + if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false; fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V); - if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam_transport to TCP and reopen raw cam")) return false; + if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false; } if(!opened) @@ -261,25 +263,25 @@ } else { bool opened=false; - if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==uv_layer) { + if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==uv_layer) { opened=openPacket(fbkgen,e.getTimeStamp(),big_layer); if(cur==NULL) //error should have been displayed by openPacket return false; fbkgen.selectSaveImage(y_layer,RawCameraGenerator::CHAN_Y); - if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam_transport to TCP and reopen raw cam")) return false; + if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false; } - if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==y_layer) { + if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==y_layer) { opened=openPacket(fbkgen,e.getTimeStamp(),big_layer); if(cur==NULL) //error should have been displayed by openPacket return false; fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U); - if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam_transport to TCP and reopen raw cam")) return false; + if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false; fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_V); - if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam_transport to TCP and reopen raw cam")) return false; + if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false; } - if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE || !opened) + if(config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE || !opened) closePacket(); } @@ -289,20 +291,20 @@ bool RawCamBehavior::writeSingleChannel(const FilterBankEvent& e) { FilterBankGenerator& fbkgen=*e.getSource(); - if( config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE && e.getGeneratorID()==EventBase::visRawCameraEGID - || config->vision.rawcam_compression==Config::vision_config::COMPRESS_JPEG && e.getGeneratorID()==EventBase::visJPEGEGID ) + if( config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_NONE && e.getGeneratorID()==EventBase::visRawCameraEGID + || config->vision.rawcam.compression==Config::vision_config::RawCamConfig::COMPRESS_JPEG && e.getGeneratorID()==EventBase::visJPEGEGID ) { if(const JPEGGenerator * jgen=dynamic_cast(&fbkgen)) if(jgen->getCurrentSourceFormat()!=JPEGGenerator::SRC_GRAYSCALE) return true; - unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rawcam_y_skip; + unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rawcam.y_skip; openPacket(fbkgen,e.getTimeStamp(),layer); if(cur==NULL) //error should have been displayed by openPacket return false; - fbkgen.selectSaveImage(layer,config->vision.rawcam_channel); - if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam_transport to TCP and reopen raw cam")) return false; + fbkgen.selectSaveImage(layer,config->vision.rawcam.channel); + if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.rawcam.transport to TCP and reopen raw cam")) return false; closePacket(); } @@ -317,7 +319,7 @@ return; visRaw->write(cur-packet); /* cout << "used=" << (cur-packet) << "\tavail==" << avail; - if(state->robotDesign & WorldState::ERS7Mask) + if(RobotName == ERS7Info::TargetName) cout << "\tmax bandwidth=" << (cur-packet)/1024.f*30 << "KB/sec" << endl; else cout << "\tmax bandwidth=" << (cur-packet)/1024.f*24 << "KB/sec" << endl; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/RawCamBehavior.h ./Behaviors/Mon/RawCamBehavior.h --- ../Tekkotsu_3.0/Behaviors/Mon/RawCamBehavior.h 2006-09-16 13:32:38.000000000 -0400 +++ ./Behaviors/Mon/RawCamBehavior.h 2007-06-07 14:24:42.000000000 -0400 @@ -20,8 +20,8 @@ * * I emphasize: beginning of each Vision packet, before the FilterBankGenerator header. * - <@c string:"TekkotsuImage"> - * - <Config::vision_config::encoding_t: rawcam_encoding> (expect single or multiple channels, 0 means color (3 channels), 1 means intensity (1 channel)) - * - <Config::vision_config::compression_t: rawcam_compression> (0==none, 1==jpeg, 2==rle) + * - <Config::vision_config::encoding_t: rawcam.encoding> (expect single or multiple channels, 0 means color (3 channels), 1 means intensity (1 channel)) + * - <Config::vision_config::compression_t: rawcam.compression> (0==none, 1==jpeg, 2==rle) * - <@c unsigned @c int: width> (this is the width of the largest channel - note different channels can be sent at different resolutions! Provides cheap "compression" of chromaticity channels) * - <@c unsigned @c int: height> (similarly, height of largest channel) * - <@c unsigned @c int: timestamp> (time image was taken, milliseconds since boot) @@ -46,7 +46,11 @@ //! destructor ~RawCamBehavior() { theOne=NULL; } +#ifdef PLATFORM_APERIOS static const unsigned int TCP_WIRELESS_BUFFER_SIZE=200000; //!< 200000 bytes for use up to 416x320 + 2*208x160 (double res Y, full res UV on ERS-7) +#else + static const unsigned int TCP_WIRELESS_BUFFER_SIZE=901*1024; //!< 900KB for max of full-color 640x480 + 1KB for header +#endif static const unsigned int UDP_WIRELESS_BUFFER_SIZE=64*1024; //!< 64KB is the max udp packet size virtual void DoStart(); @@ -57,7 +61,7 @@ static std::string getClassDescription() { char tmp[20]; - snprintf(tmp,20,"%d",config->vision.rawcam_port); tmp[19]='\0'; + snprintf(tmp,20,"%d",*config->vision.rawcam.port); tmp[19]='\0'; return std::string("Forwards images from camera over port ")+tmp; } virtual std::string getDescription() const { return getClassDescription(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/RegionCamBehavior.cc ./Behaviors/Mon/RegionCamBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Mon/RegionCamBehavior.cc 2006-09-09 00:32:29.000000000 -0400 +++ ./Behaviors/Mon/RegionCamBehavior.cc 2007-05-21 16:51:20.000000000 -0400 @@ -30,8 +30,8 @@ RegionCamBehavior::processEvent(const EventBase& e) { if(!wireless->isConnected(visRegion->sock)) return; - if(config->vision.region_transport==0 && visRegion->getTransport()==SocketNS::SOCK_STREAM - || config->vision.region_transport==1 && visRegion->getTransport()==SocketNS::SOCK_DGRAM) { + if(config->vision.regioncam.transport==0 && visRegion->getTransport()==Socket::SOCK_STREAM + || config->vision.regioncam.transport==1 && visRegion->getTransport()==Socket::SOCK_DGRAM) { closeServer(); setupServer(); return; @@ -61,23 +61,23 @@ std::vector args; args.push_back("reg"); char port[50]; - snprintf(port,50,"%d",config->vision.region_port); + snprintf(port,50,"%d",*config->vision.regioncam.port); args.push_back(port); - if(config->vision.region_transport==0) { + if(config->vision.regioncam.transport==0) { max_buf=UDP_WIRELESS_BUFFER_SIZE; - visRegion=wireless->socket(SocketNS::SOCK_DGRAM, 1024, max_buf); + visRegion=wireless->socket(Socket::SOCK_DGRAM, 1024, max_buf); args.push_back("udp"); - } else if(config->vision.region_transport==1) { + } else if(config->vision.regioncam.transport==1) { max_buf=TCP_WIRELESS_BUFFER_SIZE; - visRegion=wireless->socket(SocketNS::SOCK_STREAM, 1024, max_buf); + visRegion=wireless->socket(Socket::SOCK_STREAM, 1024, max_buf); wireless->setDaemon(visRegion,true); args.push_back("tcp"); } else { - serr->printf("ERROR: Invalid Config::vision.region_transport: %d\n",config->vision.region_transport); + serr->printf("ERROR: Invalid Config::vision.region_transport: %d\n",*config->vision.regioncam.transport); return; } - wireless->listen(visRegion,config->vision.region_port); - Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RegionVisionGUI",config->vision.region_port,args); + wireless->listen(visRegion,config->vision.regioncam.port); + Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RegionVisionGUI",config->vision.regioncam.port,args); } bool @@ -89,11 +89,13 @@ ASSERT(cur==NULL,"cur non-NULL"); cur=NULL; char * buf=packet=(char*)visRegion->getWriteBuffer(avail); - ASSERTRETVAL(packet!=NULL,"could not get buffer",false); + ASSERT(packet!=NULL,"could not get buffer"); + if(packet==NULL) + return false; if(!LoadSave::encodeInc("TekkotsuImage",buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; - if(!LoadSave::encodeInc(Config::vision_config::ENCODE_SINGLE_CHANNEL,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; - if(!LoadSave::encodeInc(Config::vision_config::COMPRESS_RLE,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; + if(!LoadSave::encodeInc(Config::vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; + if(!LoadSave::encodeInc(Config::vision_config::SegCamConfig::COMPRESS_RLE,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; if(!LoadSave::encodeInc(fbkgen.getWidth(layer),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; if(!LoadSave::encodeInc(fbkgen.getHeight(layer),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; @@ -108,14 +110,14 @@ RegionCamBehavior::writeRegions(const FilterBankEvent& e) { FilterBankGenerator& fbkgen=*e.getSource(); - unsigned int layer=fbkgen.getNumLayers()-1-config->vision.regioncam_skip; + unsigned int layer=fbkgen.getNumLayers()-1-config->vision.regioncam.skip; openPacket(fbkgen,e.getTimeStamp(),layer); ASSERTRETVAL(cur!=NULL,"header failed",false); RegionGenerator * regGen = dynamic_cast(&fbkgen); ASSERTRETVAL(regGen!=NULL,"fbkgen isn't an RegionGenerator",false); - regGen->selectSaveImage(layer,config->vision.segcam_channel); + regGen->selectSaveImage(layer,config->vision.segcam.channel); if(!LoadSave::checkInc(regGen->saveBuffer(cur,avail),cur,avail,"save region image failed")) return false; const SegmentedColorGenerator * seg = dynamic_cast((regGen->getSourceGenerator())->getSourceGenerator()); //Get the source of his source (the SegmentedColorGenerator) diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/RegionCamBehavior.h ./Behaviors/Mon/RegionCamBehavior.h --- ../Tekkotsu_3.0/Behaviors/Mon/RegionCamBehavior.h 2006-09-16 13:32:38.000000000 -0400 +++ ./Behaviors/Mon/RegionCamBehavior.h 2007-01-30 17:56:19.000000000 -0500 @@ -27,7 +27,7 @@ static std::string getClassDescription() { char tmp[20]; - sprintf(tmp,"%d",config->vision.segcam_port); + sprintf(tmp,"%d",*config->vision.segcam.port); return std::string("Forwards regions calculated out of images from camera over port ")+tmp; } virtual std::string getDescription() const { return getClassDescription(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/RoverControllerBehavior.cc ./Behaviors/Mon/RoverControllerBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Mon/RoverControllerBehavior.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Mon/RoverControllerBehavior.cc 2007-11-18 01:47:01.000000000 -0500 @@ -0,0 +1,208 @@ +#include "Shared/RobotInfo.h" +#if ( defined(TGT_HAS_ARMS) || defined(TGT_HAS_LEGS) ) && defined(TGT_HAS_HEAD) + +#include "RoverControllerBehavior.h" + +#include "Motion/MMAccessor.h" +#include + +using namespace std; + +void RoverControllerBehavior::DoStart() { + BehaviorBase::DoStart(); // do this first (required) + // open server socket + cmdsock=wireless->socket(Socket::SOCK_STREAM, 2048, 2048); + wireless->setReceiver(cmdsock, callback); + wireless->setDaemon(cmdsock,true); + wireless->listen(cmdsock, PORT); + // Open the gui on the desktop + Controller::loadGUI("org.tekkotsu.mon.RoverGUI","RoverGUI",PORT); + // set up motion commands + head = motman->addPersistentMotion(SharedObject()); + arm = motman->addPersistentMotion(SharedObject()); +} + +void RoverControllerBehavior::DoStop() { + // remove motion commands + motman->removeMotion(head); + head=MotionManager::invalid_MC_ID; + motman->removeMotion(arm); + arm=MotionManager::invalid_MC_ID; + // Close the GUI + Controller::closeGUI("RoverGUI"); + // close server socket + wireless->setDaemon(cmdsock,false); + wireless->close(cmdsock); + BehaviorBase::DoStop(); // do this last (required) +} + +void RoverControllerBehavior::initScale() { + PostureEngine e; + unsigned int gripperFrame=-1U, armBaseFrame=-1U; +#ifdef TGT_HAS_ARMS + gripperFrame=ArmOffset+NumArmJoints-1; + armBaseFrame=ArmOffset; +#elif defined(TGT_HAS_LEGS) + gripperFrame=PawFrameOffset; + armBaseFrame=LFrLegOffset; +#else + // RoverControllerBehavior can't find manipulator, won't run + // (user should've seen the compiler warning when including the header...) + return; +#endif + + // find something resembling range of motion + const float BIGNUM=1000000; + e.solveLinkPosition(BIGNUM,0,0, gripperFrame, 0,0,0); + NEWMAT::ColumnVector x = e.jointToBase(gripperFrame).SubMatrix(1,4,4,4); + float max=ceil(x(1)/x(4)); + x = e.jointToBase(armBaseFrame).SubMatrix(1,4,4,4); + tgtXOff=x(1)/x(4); + tgtScale=(max-tgtXOff)/2; + tgtXOff+=tgtScale; + x = e.jointToBase(armBaseFrame+1).SubMatrix(1,4,4,4); + tgtYOff=x(2)/x(4); + std::cout << "Gripper XY reach (-1 to 1): " << tgtXOff-tgtScale << " to " << tgtXOff+tgtScale << std::endl; + std::cout << "Gripper XY center: (" << tgtXOff << ',' << tgtYOff <<')' << std::endl; + +#ifdef TGT_HAS_ARMS + // arm just goes up + e.solveLinkPosition(0,0,BIGNUM, gripperFrame, 0,0,0); + x = e.jointToBase(gripperFrame).SubMatrix(1,4,4,4); + max=ceil(x(3)/x(4)); + x = e.jointToBase(armBaseFrame).SubMatrix(1,4,4,4); + heightOff=x(3)/x(4); + heightScale=max-heightOff; +#elif defined(TGT_HAS_LEGS) + // legs start at bottom + e.solveLinkPosition(0,0,-BIGNUM, gripperFrame, 0,0,0); + x = e.jointToBase(gripperFrame).SubMatrix(1,4,4,4); + heightOff=floor(x(3)/x(4)); + x = e.jointToBase(armBaseFrame).SubMatrix(1,4,4,4); + heightScale=(x(3)/x(4)-heightOff)*2; +#endif + std::cout << "Gripper Z reach (0 to 1):" << heightOff << " to " << heightOff+heightScale << std::endl; + + e.solveLinkPosition(0,0,-BIGNUM, gripperFrame, 0,0,0); + e.solveLinkPosition(0,0,BIGNUM, CameraFrameOffset, 0,0,0); + float ydist=e.jointToJoint(gripperFrame,CameraFrameOffset).SubMatrix(1,3,4,4).NormFrobenius(); + e.solveLinkPosition(BIGNUM,0,0, gripperFrame, 0,0,0); + e.solveLinkPosition(-BIGNUM,0,0, CameraFrameOffset, 0,0,0); + float xdist=e.jointToJoint(gripperFrame,CameraFrameOffset).SubMatrix(1,3,4,4).NormFrobenius(); + distanceScale = ceil(xdist>ydist ? xdist : ydist); + std::cout << "Max camera distance is " << distanceScale << std::endl; +} + +int RoverControllerBehavior::update(char *buf, int bytes) { + stringstream cmd(string(buf,bytes)); + + bool armDirty=false, headDirty=false; + + while(cmd) { + string t; + cmd >> t; + bool gotArm=false, gotHead=false; + if(gotHead=(t=="autoPerspective")) + cmd >> autoPerspective; + else if(gotArm=(t=="autoAngle")) + cmd >> autoGripperAngle; + else if(gotHead=(t=="autoTrack")) + cmd >> autoTrackGripper; + else if(gotHead=(t=="maxDist")) + cmd >> maxDist; + + else if(t=="gripper") { + float v; + cmd >> v; + unsigned int idx=-1U; +#ifdef TGT_HAS_ARMS + idx=ArmOffset+JointsPerArm-1; +#elif defined(TGT_HAS_MOUTH) + if(NumMouthJoints>0) + idx=MouthOffset; +#endif + if(idx!=-1U) { + float range = outputRanges[idx][MaxRange] - outputRanges[idx][MinRange]; + v = v*range + outputRanges[idx][MinRange]; + MMAccessor(arm)->setOutputCmd(idx,v); + } + } + else if(gotArm=(t=="gripperAngle")) + cmd >> gripperAngle; + else if(gotHead=(t=="perspective")) + cmd >> perspective; + else if(gotArm=(t=="gripperHeight")) { + cmd >> tgtZ; + tgtZ*=heightScale; + tgtZ+=heightOff; + } + else if(gotHead=(t=="cameraDistance")) { + cmd >> lookD; + lookD*=distanceScale; + } + + else if(gotArm=(t=="tgt")) { + cmd >> tgtY >> tgtX; + tgtY=-tgtY; + tgtX*=tgtScale; + tgtX+=tgtXOff; + tgtY*=tgtScale; + tgtY+=tgtYOff; + } + else if(gotHead=(t=="look")) { + cmd >> lookY >> lookX; + lookY=-lookY; + lookX*=tgtScale; + lookX+=tgtXOff; + lookY*=tgtScale; + lookY+=tgtYOff; + } + + armDirty = armDirty || gotArm; + headDirty = headDirty || gotHead; + } + if(autoTrackGripper) { + lookX=tgtX; + lookY=tgtY; + headDirty = headDirty || armDirty; + } + + if(armDirty) + updateArm(); + + if(headDirty) + updateHead(); + + return 0; +} + +RoverControllerBehavior * RoverControllerBehavior::theOne=NULL; + +void RoverControllerBehavior::updateArm() { +#ifdef TGT_HAS_ARMS + MMAccessor(arm)->solveLinkPosition(tgtX,tgtY,tgtZ,ArmOffset+NumArmJoints-1,0,0,0); +#elif defined(TGT_HAS_LEGS) + MMAccessor(arm)->solveLinkPosition(tgtX,tgtY,tgtZ,PawFrameOffset,0,0,0); +#endif +} + +void RoverControllerBehavior::updateHead() { + if(maxDist) + MMAccessor(head)->lookAtPoint(lookX,lookY,tgtZ); + else + MMAccessor(head)->lookAtPoint(lookX,lookY,tgtZ,lookD); +} + + +/*! @file + * @brief + * @author Ethan Tira-Thompson (ejt) (Creator) + * + * $Author: ejt $ + * $Name: tekkotsu-4_0 $ + * $Revision: 1.5 $ + * $State: Exp $ + * $Date: 2007/11/18 06:47:01 $ + */ + +#endif // check for (arms or hands) and legs diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/RoverControllerBehavior.h ./Behaviors/Mon/RoverControllerBehavior.h --- ../Tekkotsu_3.0/Behaviors/Mon/RoverControllerBehavior.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Mon/RoverControllerBehavior.h 2007-11-13 11:41:14.000000000 -0500 @@ -0,0 +1,116 @@ +//-*-c++-*- +#ifndef INCLUDED_RoverControllerBehavior_h_ +#define INCLUDED_RoverControllerBehavior_h_ + +#include "Shared/RobotInfo.h" +#if !defined(TGT_HAS_ARMS) && !defined(TGT_HAS_LEGS) || !defined(TGT_HAS_HEAD) +// no manipulator, but we can still compile, so it's only a warning +# warning RoverControllerBehavior can't find a manipulator, won't run +#endif + +#include "Behaviors/BehaviorBase.h" +#include "Behaviors/Controller.h" +#include "Events/EventRouter.h" +#include "Motion/PostureMC.h" +#include "Motion/HeadPointerMC.h" + +//! DESCRIPTION +class RoverControllerBehavior : public BehaviorBase { +public: + static const int PORT=10056; //!< port number to open up + + //! returns #theOne, initializing it if necessary + static RoverControllerBehavior& getInstance() { + if(theOne==NULL) + theOne = new RoverControllerBehavior; + return *theOne; + } + + //! destructor + virtual ~RoverControllerBehavior() { + if(isActive()) + DoStop(); + theOne=NULL; + } + + virtual void DoStart(); + virtual void DoStop(); + + static std::string getClassDescription() { return "DESCRIPTION"; } + virtual std::string getDescription() const { return getClassDescription(); } + + +protected: + //! constructor + RoverControllerBehavior() + : BehaviorBase("Rover Remote Control"), + tgtScale(1), tgtXOff(0), tgtYOff(0), heightScale(1), heightOff(0), distanceScale(1), + tgtX(0),tgtY(0),tgtZ(0), lookX(0),lookY(0),lookD(0), + gripperAngle(0),perspective(0), + autoGripperAngle(true),autoPerspective(true),autoTrackGripper(true),maxDist(true), + cmdsock(NULL), head(MotionManager::invalid_MC_ID), arm(MotionManager::invalid_MC_ID) + { + initScale(); + } + + void initScale(); + + //! called by wireless when there's new data, forwards call to theOne->update + static int callback(char *buf, int bytes) { return theOne->update(buf,bytes); } + //! called by wireless (via callback()) when there's new data + int update(char *buf, int bytes); + + void updateArm(); + void updateHead(); + + static RoverControllerBehavior * theOne; + + float tgtScale; + float tgtXOff; + float tgtYOff; + float heightScale; + float heightOff; + float distanceScale; + + float tgtX; + float tgtY; + float tgtZ; + float lookX; + float lookY; + float lookD; + + float gripperAngle; + float perspective; + + bool autoGripperAngle; + bool autoPerspective; + bool autoTrackGripper; + bool maxDist; + + Socket *cmdsock; //!< The input command stream socket + + MotionManager::MC_ID head; + MotionManager::MC_ID arm; + +private: + // Providing declarations for these functions will avoid a compiler warning if + // you have any class members which are pointers. However, as it is, an error + // will result if you inadvertantly cause a call to either (which is probably + // a good thing, unless you really intended to copy/assign a behavior, in + // which case simply provide implementations for the functions) + RoverControllerBehavior(const RoverControllerBehavior&); //!< don't call (copy constructor) + RoverControllerBehavior& operator=(const RoverControllerBehavior&); //!< don't call (assignment operator) +}; + +/*! @file + * @brief Defines RoverControllerBehavior, which DESCRIPTION + * @author Ethan Tira-Thompson (ejt) (Creator) + * + * $Author: ejt $ + * $Name: tekkotsu-4_0 $ + * $Revision: 1.4 $ + * $State: Exp $ + * $Date: 2007/11/13 16:41:14 $ + */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/SegCamBehavior.cc ./Behaviors/Mon/SegCamBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Mon/SegCamBehavior.cc 2006-09-14 14:57:03.000000000 -0400 +++ ./Behaviors/Mon/SegCamBehavior.cc 2007-05-21 16:51:20.000000000 -0400 @@ -35,8 +35,8 @@ SegCamBehavior::processEvent(const EventBase& e) { if(!wireless->isConnected(visRLE->sock)) return; - if(config->vision.segcam_transport==0 && visRLE->getTransport()==SocketNS::SOCK_STREAM - || config->vision.segcam_transport==1 && visRLE->getTransport()==SocketNS::SOCK_DGRAM) { + if(config->vision.segcam.transport==0 && visRLE->getTransport()==Socket::SOCK_STREAM + || config->vision.segcam.transport==1 && visRLE->getTransport()==Socket::SOCK_DGRAM) { closeServer(); setupServer(); return; @@ -47,9 +47,9 @@ CameraStreamBehavior::processEvent(e); return; } - if ((get_time() - lastProcessedTime) < config->vision.segcam_interval) // not enough time has gone by + if ((get_time() - lastProcessedTime) < config->vision.segcam.interval) // not enough time has gone by return; - if(config->vision.segcam_compression==Config::vision_config::COMPRESS_NONE && e.getGeneratorID()==EventBase::visSegmentEGID) { + if(config->vision.segcam.compression==Config::vision_config::SegCamConfig::COMPRESS_NONE && e.getGeneratorID()==EventBase::visSegmentEGID) { if(!writeSeg(*fbke)) { if(packet!=NULL) { cur=packet; @@ -59,7 +59,7 @@ //ASSERTRET(succ,"serialization failed"); } } - if(config->vision.segcam_compression==Config::vision_config::COMPRESS_RLE && e.getGeneratorID()==EventBase::visRLEEGID) { + if(config->vision.segcam.compression==Config::vision_config::SegCamConfig::COMPRESS_RLE && e.getGeneratorID()==EventBase::visRLEEGID) { if(!writeRLE(*fbke)) { if(packet!=NULL) { cur=packet; @@ -98,25 +98,25 @@ std::vector args; args.push_back("rle"); char port[50]; - snprintf(port,50,"%d",config->vision.segcam_port); + snprintf(port,50,"%d",*config->vision.segcam.port); args.push_back(port); - if(config->vision.segcam_transport==0) { + if(config->vision.segcam.transport==0) { max_buf=UDP_WIRELESS_BUFFER_SIZE; - visRLE=wireless->socket(SocketNS::SOCK_DGRAM, 1024, max_buf); + visRLE=wireless->socket(Socket::SOCK_DGRAM, 1024, max_buf); args.push_back("udp"); - } else if(config->vision.segcam_transport==1) { + } else if(config->vision.segcam.transport==1) { max_buf=TCP_WIRELESS_BUFFER_SIZE; - visRLE=wireless->socket(SocketNS::SOCK_STREAM, 1024, max_buf); + visRLE=wireless->socket(Socket::SOCK_STREAM, 1024, max_buf); args.push_back("tcp"); } else { - serr->printf("ERROR: Invalid Config::vision.segcam_transport: %d\n",config->vision.segcam_transport); + serr->printf("ERROR: Invalid Config::vision.segcam.transport: %d\n",*config->vision.segcam.transport); return; } wireless->setDaemon(visRLE,true); wireless->setReceiver(visRLE,networkCallback); - wireless->listen(visRLE,config->vision.segcam_port); + wireless->listen(visRLE,config->vision.segcam.port); - Controller::loadGUI("org.tekkotsu.mon.VisionGUI","SegVisionGUI",config->vision.segcam_port,args); + Controller::loadGUI("org.tekkotsu.mon.VisionGUI","SegVisionGUI",config->vision.segcam.port,args); } bool @@ -128,11 +128,13 @@ ASSERT(cur==NULL,"cur non-NULL"); cur=NULL; char * buf=packet=(char*)visRLE->getWriteBuffer(avail); - ASSERTRETVAL(packet!=NULL,"dropped frame, network bandwidth is saturated (reduce frame rate or size)",false); + ASSERT(packet!=NULL,"dropped frame, network bandwidth is saturated (reduce frame rate or size)"); + if(packet==NULL) + return false; if(!LoadSave::encodeInc("TekkotsuImage",buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;; - if(!LoadSave::encodeInc(Config::vision_config::ENCODE_SINGLE_CHANNEL,buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;; - if(!LoadSave::encodeInc(Config::vision_config::COMPRESS_RLE,buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;; + if(!LoadSave::encodeInc(Config::vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL,buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;; + if(!LoadSave::encodeInc(Config::vision_config::SegCamConfig::COMPRESS_RLE,buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;; if(!LoadSave::encodeInc(fbkgen.getWidth(layer),buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;; if(!LoadSave::encodeInc(fbkgen.getHeight(layer),buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;; @@ -147,7 +149,7 @@ SegCamBehavior::writeRLE(const FilterBankEvent& e) { FilterBankGenerator& fbkgen=*e.getSource(); - unsigned int layer=fbkgen.getNumLayers()-1-config->vision.segcam_skip; + unsigned int layer=fbkgen.getNumLayers()-1-config->vision.segcam.skip; openPacket(fbkgen,e.getTimeStamp(),layer); if(cur==NULL) //error should have been displayed by openPacket return false; @@ -155,8 +157,8 @@ RLEGenerator * rle = dynamic_cast(&fbkgen); ASSERTRETVAL(rle!=NULL,"fbkgen isn't an RLEGenerator",false); - rle->selectSaveImage(layer,config->vision.segcam_channel); - if(!LoadSave::checkInc(rle->saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.segcam_transport to TCP and reopen seg cam")) return false; + rle->selectSaveImage(layer,config->vision.segcam.channel); + if(!LoadSave::checkInc(rle->saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.segcam.transport to TCP and reopen seg cam")) return false; // send out the color map ourselves (since RLE compression doesn't have a concept of color) const SegmentedColorGenerator * seg = dynamic_cast(rle->getSourceGenerator()); @@ -172,13 +174,13 @@ SegCamBehavior::writeSeg(const FilterBankEvent& e) { FilterBankGenerator& fbkgen=*e.getSource(); - unsigned int layer=fbkgen.getNumLayers()-1-config->vision.segcam_skip; + unsigned int layer=fbkgen.getNumLayers()-1-config->vision.segcam.skip; openPacket(fbkgen,e.getTimeStamp(),layer); if(cur==NULL) //error should have been displayed by openPacket return false; - fbkgen.selectSaveImage(layer,config->vision.segcam_channel); - if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.segcam_transport to TCP and reopen seg cam")) return false; + fbkgen.selectSaveImage(layer,config->vision.segcam.channel); + if(!LoadSave::checkInc(fbkgen.saveBuffer(cur,avail),cur,avail,"image size too large -- may need to set Config::vision.segcam.transport to TCP and reopen seg cam")) return false; closePacket(); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/SegCamBehavior.h ./Behaviors/Mon/SegCamBehavior.h --- ../Tekkotsu_3.0/Behaviors/Mon/SegCamBehavior.h 2006-09-16 16:11:50.000000000 -0400 +++ ./Behaviors/Mon/SegCamBehavior.h 2007-05-01 17:21:07.000000000 -0400 @@ -63,7 +63,11 @@ //! destructor ~SegCamBehavior() { theOne=NULL; } +#if defined(TGT_ERS210) || defined(TGT_ERS220) || defined(TGT_ERS2xx) || defined(TGT_ERS7) static const unsigned int TCP_WIRELESS_BUFFER_SIZE=85000; //!< 85000 bytes for use up to 416x320 pixels / 8 min expected runs * 5 bytes per run + some padding +#else + static const unsigned int TCP_WIRELESS_BUFFER_SIZE=301*1024; //!< 300KB for use up to 640x480 pixels / 8 min expected runs * 5 bytes per run + some padding +#endif static const unsigned int UDP_WIRELESS_BUFFER_SIZE=64*1024; //!< 64KB is the max udp packet size virtual void DoStart(); @@ -74,7 +78,7 @@ static std::string getClassDescription() { char tmp[20]; - snprintf(tmp,20,"%d",config->vision.segcam_port); tmp[19]='\0'; + snprintf(tmp,20,"%d",*config->vision.segcam.port); tmp[19]='\0'; return std::string("Forwards segmented images from camera over port ")+tmp; } virtual std::string getDescription() const { return getClassDescription(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/SpeakerServer.cc ./Behaviors/Mon/SpeakerServer.cc --- ../Tekkotsu_3.0/Behaviors/Mon/SpeakerServer.cc 2006-09-18 14:08:07.000000000 -0400 +++ ./Behaviors/Mon/SpeakerServer.cc 2007-11-21 15:57:13.000000000 -0500 @@ -7,11 +7,11 @@ SpeakerServer* SpeakerServer::instance = 0; -SpeakerServer* SpeakerServer::GetInstance() { +SpeakerServer& SpeakerServer::getInstance() { if (instance == 0) { instance = new SpeakerServer(); } - return instance; + return *instance; } SpeakerServer::SpeakerServer() @@ -45,7 +45,7 @@ channel = SoundManager::invalid_Play_ID; socket = - wireless->socket(SocketNS::SOCK_STREAM, RECEIVE_BUFFER_SIZE + 8, 512); + wireless->socket(Socket::SOCK_STREAM, RECEIVE_BUFFER_SIZE + 8, 512); wireless->setReceiver(socket->sock, socket_callback); wireless->setDaemon(socket, true); wireless->listen(socket->sock, config->sound.streaming.speaker_port); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/SpeakerServer.h ./Behaviors/Mon/SpeakerServer.h --- ../Tekkotsu_3.0/Behaviors/Mon/SpeakerServer.h 2006-09-16 16:11:50.000000000 -0400 +++ ./Behaviors/Mon/SpeakerServer.h 2007-11-21 15:57:13.000000000 -0500 @@ -8,7 +8,7 @@ //! Plays streamed audio via the speaker class SpeakerServer : public BehaviorBase { public: - static SpeakerServer* GetInstance(); //!< returns global #instance + static SpeakerServer& getInstance(); //!< returns global #instance virtual ~SpeakerServer(); //!< destructor virtual void DoStart(); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/SpiderMachineBehavior.cc ./Behaviors/Mon/SpiderMachineBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Mon/SpiderMachineBehavior.cc 2006-02-21 00:27:13.000000000 -0500 +++ ./Behaviors/Mon/SpiderMachineBehavior.cc 1969-12-31 19:00:00.000000000 -0500 @@ -1,239 +0,0 @@ -#include "SpiderMachineBehavior.h" -#include "Events/EventRouter.h" -#include "Behaviors/StateNode.h" -#include "Wireless/Wireless.h" - -SpiderMachineBehavior * SpiderMachineBehavior::theOne=NULL; -unsigned int SpiderMachineBehavior::port=10081; - -void SpiderMachineBehavior::DoStart() { - BehaviorBase::DoStart(); // do this first - theOne=this; - // Turn on wireless - cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 1024, 1024*10); - wireless->setDaemon(cmdsock,true); - wireless->setReceiver(cmdsock->sock, callback); - wireless->listen(cmdsock->sock, port); - erouter->addListener(this,EventBase::stateMachineEGID); - erouter->addListener(this,EventBase::stateTransitionEGID); -} - -void SpiderMachineBehavior::DoStop() { - erouter->removeListener(this); - expected.clear(); - while(!queuedEvents.empty()) - queuedEvents.pop(); - // Close socket; turn wireless off - wireless->setDaemon(cmdsock,false); - wireless->close(cmdsock); - if(theOne==this) - theOne=NULL; - BehaviorBase::DoStop(); // do this last -} - -void SpiderMachineBehavior::processEvent(const EventBase& e) { - if(!wireless->isConnected(cmdsock->sock) || listen.size()==0) - return; - - if(e.getGeneratorID()==EventBase::stateTransitionEGID) { - bool care=false; - 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++) - care=isListening(*it); - for(std::vector::const_iterator it=outgoing.begin(); it!=outgoing.end() && !care; it++) - care=isListening(*it); - if(!care) - return; - - if(expected.size()!=0) { - queuedEvents.push(e); - } else { - cmdsock->printf("\n"); - indent(1); - cmdsock->printf("\n",trans->getName().c_str(),e.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); - } - } - - } else if(e.getGeneratorID()==EventBase::stateMachineEGID) { - if(e.getTypeID()==EventBase::statusETID) - return; - const StateNode * beh=reinterpret_cast(e.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(e); - return; - } - } else - format=" \n"; // expected as part of transition - if(e.getTypeID()==EventBase::activateETID) - cmdsock->printf(format,"start",beh->getName().c_str(),e.getTimeStamp()); - else if(e.getTypeID()==EventBase::deactivateETID) - cmdsock->printf(format,"stop",beh->getName().c_str(),e.getTimeStamp()); - else - serr->printf("WARNING: Unrecognized TypeID %d\n",e.getTypeID()); - } - if(it!=expected.end()) { //was found - expected.erase(it); - if(expected.size()==0) { - cmdsock->printf("\n"); - while(queuedEvents.size()>0) { - EventBase qe=queuedEvents.front(); - queuedEvents.pop(); - processEvent(qe); - } - } - } - - } else { - serr->printf("WARNING: Unknown event %s (%s)\n",e.getName().c_str(),e.getDescription().c_str()); - } -} - -void SpiderMachineBehavior::spider(const StateNode* n, unsigned int depth/*=0*/) { - if(n==NULL) - return; - - const std::vector& subnodes=n->getNodes(); - if(subnodes.size()==0) { - // it's a leaf node, no subnodes or transitions between them - indent(depth); - cmdsock->printf("\n", n->getClassName().c_str(), n->getName().c_str()); - } else { - - // first output current node's info - indent(depth); - cmdsock->printf("\n", n->getClassName().c_str(), n->getName().c_str()); - - 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); - cmdsock->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()); - } - const std::vector& outgoing=(*it)->getDestinations(); - for(unsigned int i=0; iprintf("%s\n",outgoing[i]->getName().c_str()); - } - indent(depth+1); - cmdsock->printf("\n"); - } - - indent(depth); - cmdsock->printf("\n"); - } -} - -bool SpiderMachineBehavior::isListening(const StateNode* n) { - while(n!=NULL) { - if(listen.find(n->getName())!=listen.end()) - return true; - n=n->getParent(); - } - return false; -} - -void SpiderMachineBehavior::indent(unsigned int level) { - for(unsigned int i=0; iprintf(" "); -} - -const StateNode * SpiderMachineBehavior::find(const std::string& name) { - for(registry_t::const_iterator it=getRegistry().begin(); it!=getRegistry().end(); it++) { - const StateNode * cur=dynamic_cast(*it); - if(cur!=NULL && cur->getName()==name) - return cur; - } - //serr->printf("WARNING: SpiderMachineBehavior Could not find StateNode named `%s'\n",name.c_str()); - return NULL; -} - -void SpiderMachineBehavior::runCommand(const std::string& s) { - if(s==std::string("list")) { - unsigned int numstate=0; - for(registry_t::const_iterator it=getRegistry().begin(); it!=getRegistry().end(); it++) { - const StateNode * cur=dynamic_cast(*it); - if(cur!=NULL) - numstate++; - } - cmdsock->printf("%d\n",numstate); - for(registry_t::const_iterator it=getRegistry().begin(); it!=getRegistry().end(); it++) { - const StateNode * cur=dynamic_cast(*it); - if(cur!=NULL) - cmdsock->printf("%s\n",cur->getName().c_str()); - } - - } else if(s.find("spider ")==0) { - const StateNode * n=find(s.substr(7)); - if(n==NULL) { - serr->printf("WARNING: SpiderMachineBehavior could not find \"%s\" for spidering\n",s.substr(7).c_str()); - cmdsock->printf("\n"); - } else { - cmdsock->printf("\n"); - spider(n); - cmdsock->printf("\n"); - } - - } else if(s.find("listen ")==0) { - listen.insert(s.substr(7)); - - } else if(s.find("ignore ")==0) { - listen.erase(s.substr(7)); - - } else if(s=="clear") { - listen.clear(); - - } else { - serr->printf("SpiderMachineBehavior::runCommand() - bad message: '%s'\n",s.c_str()); - } -} - -// The command packet reassembly mechanism -int SpiderMachineBehavior::callback(char *buf, int bytes) { - if(SpiderMachineBehavior::theOne==NULL) - return 0; - static std::string cmd; - for(int i=0; irunCommand(cmd); - cmd.clear(); - } else if(buf[i]!='\r') - cmd+=buf[i]; - } - return 0; -} - -/*! @file - * @brief Implements SpiderMachineBehavior, which when active, active and connected over network socket, outputs structure of requested state machine(s) - * @author ejt (Creator) - * - * $Author: ejt $ - * $Name: tekkotsu-3_0 $ - * $Revision: 1.5 $ - * $State: Exp $ - * $Date: 2006/02/21 05:27:13 $ - */ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/SpiderMachineBehavior.h ./Behaviors/Mon/SpiderMachineBehavior.h --- ../Tekkotsu_3.0/Behaviors/Mon/SpiderMachineBehavior.h 2005-08-07 00:11:03.000000000 -0400 +++ ./Behaviors/Mon/SpiderMachineBehavior.h 1969-12-31 19:00:00.000000000 -0500 @@ -1,136 +0,0 @@ -//-*-c++-*- -#ifndef INCLUDED_SpiderMachineBehavior_h_ -#define INCLUDED_SpiderMachineBehavior_h_ - -#include "Behaviors/BehaviorBase.h" -#include - -class StateNode; - -//! When active and connected over network socket, outputs structure of requested state machine(s) -/*! The protocol 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 -*/ -class SpiderMachineBehavior : public BehaviorBase { -public: - //! Points to the one SpiderMachineBehavior object that the input command stream is talking to. - /*! A kludge. Dunno how you're gonna make sure you're not using this uninitialized. */ - static SpiderMachineBehavior * theOne; - static unsigned int port; //!< the port to listen on (10081 by default) - static int callback(char *buf, int bytes); //!< called by wireless when there's new data - -public: - //! constructor - SpiderMachineBehavior() : BehaviorBase("SpiderMachineBehavior"), cmdsock(NULL), expected(), listen(), queuedEvents() {} - - virtual void DoStart(); - virtual void DoStop(); - virtual void processEvent(const EventBase& e); - - //! dumps all of the transitions and subnodes of a given statenode - void spider(const StateNode* n, unsigned int depth=0); - - //! returns true iff @a n or one of its parents is found in #listen - bool isListening(const StateNode* n); - - static std::string getClassDescription() { - char tmp[20]; - sprintf(tmp,"%d",port); - return std::string("When active and connected over network socket on port ")+tmp - +std::string(", outputs structure of requested state machine(s)"); - } - virtual std::string getDescription() const { return getClassDescription(); } - - //! parses commands sent from callback() - void runCommand(const std::string& s); - -protected: - //!just to prettify the data sent out - probably should make this a null-op to save bandwidth after debugging is done - void indent(unsigned int level); - - //!searches currently instantiated StateNodes to find the one named @a name - const StateNode * find(const std::string& name); - - class Socket *cmdsock; //!< the socket for communication - - typedef std::set registry_t; //!< the type of the behavior registry (BehaviorBase::registry) - typedef std::multiset expected_t; //!< the type of #expected - typedef std::set listen_t; //!< the type of #listen - typedef std::queue queuedEvents_t; //!< the type of #queuedEvents - - expected_t expected; //!< a set of behaviors which are involved with an impending transition - their next stateMachineEGID event should be ignored - - listen_t listen; //!< a set of state machine names which should have their subnodes monitored - - queuedEvents_t queuedEvents; //!< used if a transition causes other transitions, those transitions need to be remembered - -private: - SpiderMachineBehavior(const SpiderMachineBehavior&); //!< don't call - SpiderMachineBehavior operator=(const SpiderMachineBehavior&); //!< don't call -}; - -/*! @file - * @brief Defines SpiderMachineBehavior, which active and connected over network socket, outputs structure of requested state machine(s) - * @author ejt (Creator) - * - * $Author: ejt $ - * $Name: tekkotsu-3_0 $ - * $Revision: 1.9 $ - * $State: Exp $ - * $Date: 2005/08/07 04:11:03 $ - */ - -#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/StewartPlatformBehavior.cc ./Behaviors/Mon/StewartPlatformBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Mon/StewartPlatformBehavior.cc 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Mon/StewartPlatformBehavior.cc 2007-05-21 16:51:20.000000000 -0400 @@ -13,7 +13,7 @@ // Behavior startup BehaviorBase::DoStart(); // Turn on wireless - cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 300, 300); + cmdsock=wireless->socket(Socket::SOCK_STREAM, 300, 300); sock=cmdsock->sock; wireless->setDaemon(cmdsock,true); wireless->setReceiver(cmdsock->sock, callback); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/StewartPlatformBehavior.h ./Behaviors/Mon/StewartPlatformBehavior.h --- ../Tekkotsu_3.0/Behaviors/Mon/StewartPlatformBehavior.h 2005-08-07 00:11:03.000000000 -0400 +++ ./Behaviors/Mon/StewartPlatformBehavior.h 2007-01-30 17:56:19.000000000 -0500 @@ -41,7 +41,7 @@ static std::string getClassDescription() { char tmp[20]; - sprintf(tmp,"%d",config->main.stewart_port); + sprintf(tmp,"%d",*config->main.stewart_port); return std::string("moves the legs in synchrony to emulate the capabilities of a stewart platform, base on commands from port ")+tmp; } virtual std::string getDescription() const { return getClassDescription(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/UPennWalkControllerBehavior.cc ./Behaviors/Mon/UPennWalkControllerBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Mon/UPennWalkControllerBehavior.cc 2006-09-18 14:08:07.000000000 -0400 +++ ./Behaviors/Mon/UPennWalkControllerBehavior.cc 2007-05-21 16:51:20.000000000 -0400 @@ -94,7 +94,7 @@ // Turn on wireless theLastOne=theOne; theOne=this; - cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 2048, 2048); + cmdsock=wireless->socket(Socket::SOCK_STREAM, 2048, 2048); wireless->setReceiver(cmdsock->sock, mechacmd_callback); wireless->setDaemon(cmdsock,true); wireless->listen(cmdsock->sock, config->main.walkControl_port); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/UPennWalkControllerBehavior.h ./Behaviors/Mon/UPennWalkControllerBehavior.h --- ../Tekkotsu_3.0/Behaviors/Mon/UPennWalkControllerBehavior.h 2005-04-23 14:33:26.000000000 -0400 +++ ./Behaviors/Mon/UPennWalkControllerBehavior.h 2007-01-30 17:56:19.000000000 -0500 @@ -90,7 +90,7 @@ static std::string getClassDescription() { char tmp[20]; - sprintf(tmp,"%d",config->main.walkControl_port); + sprintf(tmp,"%d",*config->main.walkControl_port); return std::string("Listens to walk control commands coming in from port ")+tmp; } virtual std::string getDescription() const { return getClassDescription(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/ViewWMVarsBehavior.h ./Behaviors/Mon/ViewWMVarsBehavior.h --- ../Tekkotsu_3.0/Behaviors/Mon/ViewWMVarsBehavior.h 2004-11-10 20:45:36.000000000 -0500 +++ ./Behaviors/Mon/ViewWMVarsBehavior.h 2007-01-30 17:56:19.000000000 -0500 @@ -27,7 +27,7 @@ static std::string getClassDescription() { char tmp[20]; - sprintf(tmp,"%d",config->main.wmmonitor_port); + sprintf(tmp,"%d",*config->main.wmmonitor_port); return std::string("Brings up the WatchableMemory GUI on port ")+tmp+std::string(" (connects to WMMonitorBehavior, this just launches the GUI)"); } virtual std::string getDescription() const { return getClassDescription(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/WMMonitorBehavior.h ./Behaviors/Mon/WMMonitorBehavior.h --- ../Tekkotsu_3.0/Behaviors/Mon/WMMonitorBehavior.h 2004-11-10 20:45:36.000000000 -0500 +++ ./Behaviors/Mon/WMMonitorBehavior.h 2007-05-21 16:51:20.000000000 -0400 @@ -65,7 +65,7 @@ // Behavior startup BehaviorBase::DoStart(); // Turn on wireless - cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 2048, 8192); + cmdsock=wireless->socket(Socket::SOCK_STREAM, 2048, 8192); wireless->setReceiver(cmdsock->sock, wmmonitorcmd_callback); wireless->setDaemon(cmdsock,true); wireless->listen(cmdsock->sock, config->main.wmmonitor_port); @@ -100,7 +100,7 @@ static std::string getClassDescription() { char tmp[20]; - sprintf(tmp,"%d",config->main.wmmonitor_port); + sprintf(tmp,"%d",*config->main.wmmonitor_port); return std::string("Bidirectional control communication with WMMonitor on port ")+tmp; } virtual std::string getDescription() const { return getClassDescription(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/WalkControllerBehavior.cc ./Behaviors/Mon/WalkControllerBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Mon/WalkControllerBehavior.cc 2006-09-18 14:08:07.000000000 -0400 +++ ./Behaviors/Mon/WalkControllerBehavior.cc 2007-05-21 16:51:20.000000000 -0400 @@ -101,7 +101,7 @@ // Turn on wireless theLastOne=theOne; theOne=this; - cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 2048, 2048); + cmdsock=wireless->socket(Socket::SOCK_STREAM, 2048, 2048); wireless->setReceiver(cmdsock->sock, mechacmd_callback); wireless->setDaemon(cmdsock,true); wireless->listen(cmdsock->sock, config->main.walkControl_port); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/WalkControllerBehavior.h ./Behaviors/Mon/WalkControllerBehavior.h --- ../Tekkotsu_3.0/Behaviors/Mon/WalkControllerBehavior.h 2005-09-13 18:10:36.000000000 -0400 +++ ./Behaviors/Mon/WalkControllerBehavior.h 2007-08-28 14:31:15.000000000 -0400 @@ -2,8 +2,6 @@ #ifndef INCLUDED_WalkControllerBehavior_h_ #define INCLUDED_WalkControllerBehavior_h_ -#include -#include "Wireless/Wireless.h" #include "Behaviors/BehaviorBase.h" #include "Motion/MotionManager.h" #include "Motion/WalkMC.h" @@ -11,6 +9,9 @@ #include "Events/EventRouter.h" #include "Events/EventBase.h" #include "Shared/Config.h" +#include "Wireless/Wireless.h" + +#include //! Listens to control commands coming in from the command port for remotely controlling the walk /*! The communication protocol is a very simple binary format, shared @@ -102,7 +103,7 @@ static std::string getClassDescription() { char tmp[20]; - sprintf(tmp,"%d",config->main.walkControl_port); + sprintf(tmp,"%d",*config->main.walkControl_port); return std::string("Listens to walk control commands coming in from port ")+tmp; } virtual std::string getDescription() const { return getClassDescription(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/WorldStateSerializerBehavior.cc ./Behaviors/Mon/WorldStateSerializerBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Mon/WorldStateSerializerBehavior.cc 2006-09-19 18:51:38.000000000 -0400 +++ ./Behaviors/Mon/WorldStateSerializerBehavior.cc 2007-06-26 00:27:44.000000000 -0400 @@ -6,6 +6,9 @@ #include "Events/EventRouter.h" #include "Shared/LoadSave.h" +//! a little trick to get RobotName length under aperios, which has to use a string class instead of char* +inline size_t strlen(const std::string& st) { return st.size(); } + WorldStateSerializerBehavior::WorldStateSerializerBehavior() : BehaviorBase("WorldStateSerializerBehavior"), wsJoints(NULL), wsPIDs(NULL), lastProcessedTime(0) { @@ -15,13 +18,13 @@ BehaviorBase::DoStart(); // do this first const unsigned int transmitPIDSize=(NumPIDJoints*3)*LoadSave::getSerializedSize()+2*LoadSave::getSerializedSize(); - wsPIDs=wireless->socket(SocketNS::SOCK_STREAM, 1024, transmitPIDSize*4); // enough for 4 packets queued up + wsPIDs=wireless->socket(Socket::SOCK_STREAM, 1024, transmitPIDSize*4); // enough for 4 packets queued up wireless->setDaemon(wsPIDs); wireless->listen(wsPIDs, config->main.wspids_port); const size_t robotNameLen=strlen(RobotName)+1; //note the +1 to include '\0' at the end const size_t transmitJointsSize=(NumOutputs+NumSensors+NumButtons+NumPIDJoints)*LoadSave::getSerializedSize()+6*LoadSave::getSerializedSize()+robotNameLen; - wsJoints=wireless->socket(SocketNS::SOCK_STREAM, 1024, transmitJointsSize*4); // enough for 4 packets queued up + wsJoints=wireless->socket(Socket::SOCK_STREAM, 1024, transmitJointsSize*4); // enough for 4 packets queued up wireless->setDaemon(wsJoints); wireless->listen(wsJoints, config->main.wsjoints_port); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Mon/WorldStateSerializerBehavior.h ./Behaviors/Mon/WorldStateSerializerBehavior.h --- ../Tekkotsu_3.0/Behaviors/Mon/WorldStateSerializerBehavior.h 2006-09-09 19:28:59.000000000 -0400 +++ ./Behaviors/Mon/WorldStateSerializerBehavior.h 2007-06-26 00:27:44.000000000 -0400 @@ -4,6 +4,7 @@ #include "Behaviors/BehaviorBase.h" #include "Shared/Config.h" +#include class Socket; @@ -38,7 +39,7 @@ virtual void processEvent(const EventBase& e); //!< core functionality - performs serialization, sends to sockets static std::string getClassDescription() { char tmp[80]; - sprintf(tmp,"Sends sensor information to port %d and current pid values to port %d",config->main.wsjoints_port,config->main.wspids_port); + sprintf(tmp,"Sends sensor information to port %d and current pid values to port %d",*config->main.wsjoints_port,*config->main.wspids_port); return tmp; } virtual std::string getDescription() const { return getClassDescription(); } @@ -60,10 +61,16 @@ //! writes @a num copies of T from @a src to @a dst and advances @a dst by @a num * sizeof(T) /*! doesn't do any byte swapping, so this is only used if LoadSave indicates no byte swapping is needed */ template - inline static void copy(char **dst, const T * src, int num) { + inline static void copy(char **dst, const T * src, size_t num) { memcpy(*dst, src, num*sizeof(T)); (*dst) += num*sizeof(T); } + + //! writes @a num characters from @a src to @a dst and advances @a dst by @a num * sizeof(T) + inline static void copy(char **dst, const std::string& src, size_t num) { + memcpy(*dst, src.c_str(), num*sizeof(std::string::value_type)); + (*dst) += num*sizeof(std::string::value_type); + } Socket *wsJoints; //!< socket for sending current joint data Socket *wsPIDs; //!< socket for sending current PID info diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Nodes/MCNode.cc ./Behaviors/Nodes/MCNode.cc --- ../Tekkotsu_3.0/Behaviors/Nodes/MCNode.cc 2006-09-27 16:10:27.000000000 -0400 +++ ./Behaviors/Nodes/MCNode.cc 2007-03-13 04:31:26.000000000 -0400 @@ -8,18 +8,26 @@ // defined here instead of corresponding .cc files to avoid file bloat // (there would be nothing else in their .cc files) -//! name for TailWagNode to pass as template argument -extern const char defTailWagNodeName[]="TailWagNode"; -//! description for TailWagNode to pass as template argument -extern const char defTailWagNodeDesc[]="Wags the tail for as long as the state is active"; //! name for HeadPointerNode to pass as template argument extern const char defHeadPointerNodeName[]="HeadPointerNode"; //! description for HeadPointerNode to pass as template argument extern const char defHeadPointerNodeDesc[]="Manages a HeadPointerMC to look in a given direction each time the node is activated"; +//! name for TailWagNode to pass as template argument +extern const char defTailWagNodeName[]="TailWagNode"; +//! description for TailWagNode to pass as template argument +extern const char defTailWagNodeDesc[]="Wags the tail for as long as the state is active"; +//! name for PostureNode to pass as template argument +extern const char defPostureNodeName[]="PostureNode"; +//! description for PostureNode to pass as template argument +extern const char defPostureNodeDesc[]="Moves the body to the specified posture"; //! name for WalkNode to pass as template argument extern const char defWalkNodeName[]="WalkNode"; //! description for WalkNode to pass as template argument extern const char defWalkNodeDesc[]="Manages a WalkMC node to walk in a direction each time the node is activated."; +//! name for WaypointWalkNode to pass as template argument +extern const char defWaypointWalkNodeName[]="WaypointWalkNode"; +//! description for WaypointWalkNode to pass as template argument +extern const char defWaypointWalkNodeDesc[]="Manages a WaypointWalkMC to perform a waypoint walk each time the node is activated."; void MCNodeBase::DoStart() { diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Nodes/MotionSequenceNode.h ./Behaviors/Nodes/MotionSequenceNode.h --- ../Tekkotsu_3.0/Behaviors/Nodes/MotionSequenceNode.h 2006-09-09 00:32:33.000000000 -0400 +++ ./Behaviors/Nodes/MotionSequenceNode.h 2007-04-06 22:03:43.000000000 -0400 @@ -6,6 +6,7 @@ #include "Motion/MotionManager.h" #include "Motion/MotionSequenceMC.h" #include "Motion/MMAccessor.h" +#include "Shared/debuget.h" //! A StateNode for playing a MotionSequence (and looping it if desired) /*! Eventually, i'd like to just build the looping functionality into diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Nodes/PostureNode.h ./Behaviors/Nodes/PostureNode.h --- ../Tekkotsu_3.0/Behaviors/Nodes/PostureNode.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Nodes/PostureNode.h 2007-11-11 18:57:19.000000000 -0500 @@ -0,0 +1,68 @@ +//-*-c++-*- +#ifndef INCLUDED_TailWagNode_h_ +#define INCLUDED_TailWagNode_h_ + +#include "MCNode.h" +#include "Events/EventRouter.h" +#include "Motion/PostureMC.h" + +//!default name for PostureNode's (have to instantiate a variable in order to use as a template argument) +/*! instantiation will be placed in MCNode.cc to avoid file bloat */ +extern const char defPostureNodeName[]; +//!default description for PostureNode's (have to instantiate a variable in order to use as a template argument) +/*! instantiation will be placed in MCNode.cc to avoid file bloat */ +extern const char defPostureNodeDesc[]; + +//! A simple StateNode that executes a PostureMC motion command +/*! Caches the posture file in a private PostureEngine because the + * motion command might be shared with other functions that are + * using it for other purposes */ +class PostureNode : public MCNode { +public: + + //! Constructor: takes optional instance name and filename. + /*! Caches the posture file in a private PostureEngine because the + motion command might be shared with other functions that are + using it for other purposes */ + PostureNode(const std::string &nodename=defPostureNodeName, const std::string &filename=std::string()) + : MCNode(nodename), posture() + { + if ( filename.size() > 0 ) + posture.loadFile(filename.c_str()); + } + + virtual void DoStart() { + getMC()->setAverage(posture,1); // copy cached posture into the motion command + MCNode::DoStart(); + } + + //! loads the specified file into #posture, note this @em doesn't affect the current PostureMC, just the cached one which will be loaded into it on next activation. See getPosture(), getMC_ID() + virtual void loadFile(const std::string &filename) { + posture.loadFile(filename.c_str()); + getMC()->setAverage(posture,1); + } + + //! accessor for #posture, note this @em doesn't affect the current PostureMC, just the cached one which will be loaded into it on next activation. See getMC_ID() + virtual PostureEngine& getPosture() { return posture; } + //! accessor for #posture, note this @em doesn't return the current PostureMC, just the cached one which will be loaded into it on next activation. See getMC_ID() + virtual const PostureEngine& getPosture() const { return posture; } + +protected: + //! The internal cache of joint positions, copied to the motion command when activated. + /*! This allows the motion command to be shared by other nodes/behaviors, which might modify + * the posture on an ongoing basis. */ + PostureEngine posture; +}; + +/*! @file + * @brief Defines PostureNode, a simple StateNode that runs a PostureMC motion command + * @author dst + * + * $Author: ejt $ + * $Name: tekkotsu-4_0 $ + * $Revision: 1.5 $ + * $State: Exp $ + * $Date: 2007/11/11 23:57:19 $ + */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Nodes/WalkToTargetNode.cc ./Behaviors/Nodes/WalkToTargetNode.cc --- ../Tekkotsu_3.0/Behaviors/Nodes/WalkToTargetNode.cc 2004-12-04 23:47:53.000000000 -0500 +++ ./Behaviors/Nodes/WalkToTargetNode.cc 2007-08-28 14:05:19.000000000 -0400 @@ -5,7 +5,11 @@ #include "Events/VisionObjectEvent.h" #include "Shared/WorldState.h" #include "Behaviors/Transitions/TimeOutTrans.h" -#include "Behaviors/Transitions/VisualTargetCloseTrans.h" +#include "Shared/RobotInfo.h" +#include "Shared/ERS7Info.h" +#ifdef TGT_HAS_IR_DISTANCE +# include "Behaviors/Transitions/VisualTargetCloseTrans.h" +#endif void WalkToTargetNode::DoStart() { StateNode::DoStart(); @@ -37,19 +41,25 @@ } else return; + // for portability, look to see if the host hardware has a head pan & tilt joints + const unsigned int panIdx = capabilities.findOutputOffset(ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::PanOffset]); + const unsigned int tiltIdx = capabilities.findOutputOffset(ERS7Info::outputNames[ERS7Info::HeadOffset+ERS7Info::TiltOffset]); + if(panIdx==-1U || tiltIdx==-1U) + return; // guess not... + //cout << "Pos: " << horiz << ' ' << vert << endl; - double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/6; - double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/7.5; - if(tilt>outputRanges[HeadOffset+TiltOffset][MaxRange]) - tilt=outputRanges[HeadOffset+TiltOffset][MaxRange]; - if(tiltoutputRanges[HeadOffset+PanOffset][MaxRange]*2/3) - pan=outputRanges[HeadOffset+PanOffset][MaxRange]*2/3; - if(pan(headpointer_id)->setJoints(tilt,pan,0);} //note use of {}'s to limit scope + double tilt=state->outputs[tiltIdx]-vert*M_PI/6; + double pan=state->outputs[panIdx]-horiz*M_PI/7.5; + if(tilt>outputRanges[tiltIdx][MaxRange]) + tilt=outputRanges[tiltIdx][MaxRange]; + if(tiltoutputRanges[panIdx][MaxRange]*2/3) + pan=outputRanges[panIdx][MaxRange]*2/3; + if(pan(headpointer_id)->setJoints(tilt,pan,0); // note no variable name, one-line scope { MMAccessor walker(walker_id); @@ -65,7 +75,12 @@ } Transition* WalkToTargetNode::newDefaultCloseTrans(StateNode* dest) { +#ifdef TGT_HAS_IR_DISTANCE return new VisualTargetCloseTrans(dest,tracking); +#else + (void)dest; + return NULL; +#endif } @@ -74,9 +89,8 @@ * @author ejt (Creator) * * $Author: ejt $ - * $Name: tekkotsu-3_0 $ - * $Revision: 1.1 $ + * $Name: tekkotsu-4_0 $ + * $Revision: 1.4 $ * $State: Exp $ - * $Date: 2004/12/05 04:47:53 $ + * $Date: 2007/08/28 18:05:19 $ */ - diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Nodes/WaypointWalkNode.h ./Behaviors/Nodes/WaypointWalkNode.h --- ../Tekkotsu_3.0/Behaviors/Nodes/WaypointWalkNode.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Nodes/WaypointWalkNode.h 2007-02-26 03:42:51.000000000 -0500 @@ -0,0 +1,64 @@ +//-*-c++-*- +#ifndef INCLUDED_WaypointWalkNode_h_ +#define INCLUDED_WaypointWalkNode_h_ + +#include "MCNode.h" +#include "Motion/MotionManager.h" +#include "Motion/WaypointWalkMC.h" +#include "Events/EventRouter.h" +#include "Events/LocomotionEvent.h" + +//!default name for WaypointEngineNode's (have to instantiate a variable in order to use as a template argument) +/*! instantiation will be placed in MCNode.cc to avoid file bloat */ +extern const char defWaypointWalkNodeName[]; +//!default description for WaypointWalkNode's (have to instantiate a variable in order to use as a template argument) +/*! instantiation will be placed in MCNode.cc to avoid file bloat */ +extern const char defWaypointWalkNodeDesc[]; + +//! A StateNode for doing a waypoint walk, use the template parameter to specify a custom walk MC, or use the ::WaypointWalkNode typedef to accept the "default" walk +template +class WaypointEngineNode : public MCNode { +public: + + //! constructor + WaypointEngineNode() : MCNode() {} + + //! constructor + WaypointEngineNode(const std::string& name) : MCNode(name) {} + + //!destructor + ~WaypointEngineNode() {} + + virtual void DoStart() { + MCNode::DoStart(); + erouter->addListener(this,EventBase::locomotionEGID,MCNode::getMC_ID(),EventBase::statusETID); + } + +protected: + + //! constructor + WaypointEngineNode(const std::string& className, const std::string& instanceName) : + MCNode(className,instanceName) {} + + void processEvent(const EventBase &event) { + if ( static_cast(event).isStop() ) + MCNode::postCompletionEvent(); + } + +}; + +//! the prototypical WaypointWalkNode, using a WaypointWalkMC +typedef WaypointEngineNode WaypointWalkNode; + +/*! @file + * @brief Describes WaypointEngineNode, a StateNode for doing a waypoint walk; use the template parameter to specify a custom waypoint walk MC, or use the WaypointWalkNode typedef to accept the "default" walk + * @author dst (Creator) + * + * $Author: dst $ + * $Name: tekkotsu-4_0 $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2007/02/26 08:42:51 $ + */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Services/AutoGetupBehavior.h ./Behaviors/Services/AutoGetupBehavior.h --- ../Tekkotsu_3.0/Behaviors/Services/AutoGetupBehavior.h 2006-09-18 14:07:56.000000000 -0400 +++ ./Behaviors/Services/AutoGetupBehavior.h 2007-11-12 13:00:40.000000000 -0500 @@ -19,10 +19,10 @@ //! destructor virtual ~AutoGetupBehavior() {} - //! Listens for the SensorSourceID::UpdatedSID + //! Listens for the SensorSrcID::UpdatedSID virtual void DoStart() { BehaviorBase::DoStart(); - erouter->addListener(this,EventBase::sensorEGID,SensorSourceID::UpdatedSID); + erouter->addListener(this,EventBase::sensorEGID,SensorSrcID::UpdatedSID); } //! Stops listening for events virtual void DoStop() { @@ -33,7 +33,7 @@ virtual void processEvent(const EventBase &event) { if(event.getGeneratorID()==EventBase::motmanEGID) { //previous attempt at getting up has completed - cout << "Getup complete" << endl; + std::cout << "Getup complete" << std::endl; erouter->removeListener(this,EventBase::motmanEGID); waiting=false; return; @@ -42,7 +42,7 @@ side=side*gamma+(1-gamma)*state->sensors[LAccelOffset]; if(!waiting && back*back+side*side>sensitivity*WorldState::g*WorldState::g) { //fallen down - cout << "I've fallen!" << endl; + std::cout << "I've fallen!" << std::endl; sndman->playFile("yipper.wav"); std::string gu; //config->motion.makePath will return a path relative to config->motion.root (from config file read at boot) diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Services/BatteryMonitorBehavior.h ./Behaviors/Services/BatteryMonitorBehavior.h --- ../Tekkotsu_3.0/Behaviors/Services/BatteryMonitorBehavior.h 2006-09-14 15:14:03.000000000 -0400 +++ ./Behaviors/Services/BatteryMonitorBehavior.h 2007-06-28 00:36:20.000000000 -0400 @@ -29,16 +29,16 @@ //! destructor virtual ~BatteryMonitorBehavior() {} - //! Listens for the PowerSourceID::LowPowerWarnSID + //! Listens for the PowerSrcID::LowPowerWarnSID virtual void DoStart() { BehaviorBase::DoStart(); - erouter->addListener(this,EventBase::powerEGID,PowerSourceID::LowPowerWarnSID); - erouter->addListener(this,EventBase::powerEGID,PowerSourceID::ExternalPowerSID); - erouter->addListener(this,EventBase::powerEGID,PowerSourceID::BatteryConnectSID); - erouter->addListener(this,EventBase::powerEGID,PowerSourceID::UpdatedSID); + erouter->addListener(this,EventBase::powerEGID,PowerSrcID::LowPowerWarnSID); + erouter->addListener(this,EventBase::powerEGID,PowerSrcID::ExternalPowerSID); + erouter->addListener(this,EventBase::powerEGID,PowerSrcID::BatteryConnectSID); + erouter->addListener(this,EventBase::powerEGID,PowerSrcID::UpdatedSID); //if the low power warning is *already* on, better forge an event and send it to myself if(shouldWarn()) - processEvent(EventBase(EventBase::powerEGID,PowerSourceID::UpdatedSID,EventBase::statusETID)); + processEvent(EventBase(EventBase::powerEGID,PowerSrcID::UpdatedSID,EventBase::statusETID)); } //! Stops listening for events virtual void DoStop() { @@ -98,7 +98,7 @@ virtual std::string getDescription() const { return getClassDescription(); } //! returns true if the warning should be active (power remaining less than high_power_p, no external power, but also checks that a power update has been received) - static bool shouldWarn() { return state!=NULL && state->powerFlags[PowerSourceID::BatteryConnectSID] && (state->sensors[PowerRemainOffset]*100<=high_power_p || state->powerFlags[PowerSourceID::LowPowerWarnSID]) && !state->powerFlags[PowerSourceID::ExternalPowerSID]; } + static bool shouldWarn() { return state!=NULL && state->powerFlags[PowerSrcID::BatteryConnectSID] && (state->sensors[PowerRemainOffset]*100<=high_power_p || state->powerFlags[PowerSrcID::LowPowerWarnSID]) && !state->powerFlags[PowerSrcID::ExternalPowerSID]; } protected: //! adds a pose and a timer to get the ears flipping @@ -131,16 +131,20 @@ return 0; return (unsigned int)(max_t*(cur_power-no_power)/(high_power-no_power)); } - //!sets the ears on a 210 or the headlight on a 220 - true toggles current, false clears + //!sets the ears on a 210 & 7 or the headlight on a 220 - true toggles current, false clears void setFlipper(bool set) { - if(state->robotDesign & WorldState::ERS210Mask) - for(unsigned int i=ERS210Info::EarOffset; isetOutputCmd(i,set?!state->outputs[i]:OutputCmd()); - if(state->robotDesign & WorldState::ERS220Mask) - pose->setOutputCmd(ERS220Info::RetractableHeadLEDOffset,set?(state->outputs[ERS220Info::RetractableHeadLEDOffset]>.5?0:1):OutputCmd()); - if(state->robotDesign & WorldState::ERS7Mask) - for(unsigned int i=ERS7Info::EarOffset; isetOutputCmd(light,set?(state->outputs[light]>.5?0:1):OutputCmd()); + } else if(RobotName == ERS7Info::TargetName) { + int ear=capabilities.getOutputOffset(ERS7Info::outputNames[ERS7Info::EarOffset]); + for(unsigned int i=ear; isetOutputCmd(i,set?!state->outputs[i]:OutputCmd()); + } } PostureMC* pose; //!< if we are currently warning of low battery, holds a pose, NULL otherwise MotionManager::MC_ID pose_id; //!< id of pose if we are currently warning, MotionManager::invalid_MC_ID otherwise diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Services/DeadReckoningBehavior.h ./Behaviors/Services/DeadReckoningBehavior.h --- ../Tekkotsu_3.0/Behaviors/Services/DeadReckoningBehavior.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Services/DeadReckoningBehavior.h 2007-11-11 18:57:20.000000000 -0500 @@ -0,0 +1,64 @@ +//-*-c++-*- +#ifndef INCLUDED_DeadReckoningBehavior_h_ +#define INCLUDED_DeadReckoningBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Events/EventRouter.h" +#include "Events/LocomotionEvent.h" +#include "Motion/HolonomicMotionModel.h" +#include "Shared/WorldState.h" + +//! Subscribes to LocomotionEvents and attempts to track robot movement over time using a fairly generic HolonomicMotionModel +/*! Can be used as a ParticleFilter::MotionModel, or as a component of other behaviors. + * + * If you want a regular report on position, the behavior will output current position on timer events */ +template +class DeadReckoningBehavior : public BehaviorBase, public HolonomicMotionModel { +public: + //! constructor + explicit DeadReckoningBehavior(const std::string& name="DeadReckoningBehavior") : BehaviorBase(name), HolonomicMotionModel() {} + + //! constructor + DeadReckoningBehavior(float xVariance, float yVariance, float aVariance) + : BehaviorBase("DeadReckoningBehavior"), HolonomicMotionModel(xVariance,yVariance,aVariance) {} + + virtual void DoStart() { + BehaviorBase::DoStart(); // do this first (required) + HolonomicMotionModel::setVelocity(state->vel_x, state->vel_y, state->vel_a); + erouter->addListener(this, EventBase::locomotionEGID ); + //erouter->addTimer(this, 0, 500); + } + + virtual void processEvent(const EventBase& event) { + if (event.getGeneratorID() == EventBase::locomotionEGID) { + const LocomotionEvent &locoevt = dynamic_cast(event); + HolonomicMotionModel::setVelocity(locoevt.x,locoevt.y,locoevt.a,locoevt.getTimeStamp()); + } else if (event.getGeneratorID() == EventBase::timerEGID) { + float tempx; + float tempy; + float tempa; + HolonomicMotionModel::getPosition(tempx, tempy, tempa); + std::cout << "DEADPOS " << tempx << " " << tempy << " " << tempa << std::endl; + } + } + + static std::string getClassDescription() { return "Subscribes to LocomotionEvents and attempts to track robot movement over time"; } + virtual std::string getDescription() const { return getClassDescription(); } + +protected: + //! constructor, accepts separate classname and instance name parameters from subclass + DeadReckoningBehavior(const std::string& classname, const std::string& instancename) : BehaviorBase(classname, instancename), HolonomicMotionModel() {} +}; + +/*! @file +* @brief Defines DeadReckoningBehavior, which subscribes to LocomotionEvents and attempts to track robot movement over time using a fairly generic HolonomicMotionModel +* @author Ethan Tira-Thompson (ejt) (Creator) +* +* $Author: ejt $ +* $Name: tekkotsu-4_0 $ +* $Revision: 1.2 $ +* $State: Exp $ +* $Date: 2007/11/11 23:57:20 $ +*/ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Services/FlashIPAddrBehavior.cc ./Behaviors/Services/FlashIPAddrBehavior.cc --- ../Tekkotsu_3.0/Behaviors/Services/FlashIPAddrBehavior.cc 2006-09-18 14:07:56.000000000 -0400 +++ ./Behaviors/Services/FlashIPAddrBehavior.cc 2007-08-05 12:16:04.000000000 -0400 @@ -1,5 +1,11 @@ -#include "FlashIPAddrBehavior.h" +#include "Shared/RobotInfo.h" +#ifdef TGT_HAS_LEDS + +#include "Shared/ERS210Info.h" +#include "Shared/ERS220Info.h" +#include "Shared/ERS7Info.h" +#include "FlashIPAddrBehavior.h" #include "Events/EventRouter.h" #include "Motion/MMAccessor.h" #include "Motion/LedEngine.h" @@ -16,8 +22,10 @@ ms_id = motman->addPrunableMotion(ms,MotionManager::kEmergencyPriority+1); erouter->addListener(this,EventBase::motmanEGID,ms_id,EventBase::deactivateETID); } +#ifdef TGT_HAS_BUTTONS erouter->addListener(this,EventBase::buttonEGID,button1); erouter->addListener(this,EventBase::buttonEGID,button2); +#endif } void FlashIPAddrBehavior::DoStop() { @@ -55,6 +63,7 @@ } +#ifdef TGT_HAS_BUTTONS } else if(e.getGeneratorID()==EventBase::buttonEGID) { //if it's an activate, start a timer to expire in a few seconds //if it's a deactivate, cancel that timer @@ -63,6 +72,7 @@ erouter->addTimer(this,ACTIVATE_TIMER,2000,false); } else if(e.getTypeID()==EventBase::deactivateETID) erouter->removeTimer(this,ACTIVATE_TIMER); +#endif } else if(e.getGeneratorID()==EventBase::motmanEGID) { // display has completed, mark it as such @@ -143,14 +153,16 @@ } //cout << endl; if(i!=0) { - LEDBitMask_t dot=1<robotDesign&WorldState::ERS210Mask) { - dot=LedEngine::ERS210numMasks[10]; - } else if(state->robotDesign&WorldState::ERS220Mask) { + LEDBitMask_t dot=LedEngine::defaultCountNumMasks[10]; //default in case we don't recognize model +#ifdef TGT_HAS_LED_PANEL + if(RobotName == ERS210Info::TargetName) { + dot=LedEngine::defaultMimicNumMasks[10]; + } else if(RobotName == ERS220Info::TargetName) { dot=LedEngine::ERS220numMasks[10]; - } else if(state->robotDesign&WorldState::ERS7Mask) { + } else if(RobotName == ERS7Info::TargetName) { dot=LedEngine::ERS7numMasks[10]; } +#endif erouter->addTimer(this,sounds.size(),ms->getTime()+delay,false); sounds.push_back("numbers/dot.wav"); for(unsigned int j=0; jplay(); } +#endif /*! @file * @brief Implements FlashIPAddrBehavior, which displays IP address by flashing a series of numbers on the LED face panel diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Services/FlashIPAddrBehavior.h ./Behaviors/Services/FlashIPAddrBehavior.h --- ../Tekkotsu_3.0/Behaviors/Services/FlashIPAddrBehavior.h 2006-09-14 15:14:04.000000000 -0400 +++ ./Behaviors/Services/FlashIPAddrBehavior.h 2007-08-05 12:16:04.000000000 -0400 @@ -2,6 +2,10 @@ #ifndef INCLUDED_FlashIPAddrBehavior_h_ #define INCLUDED_FlashIPAddrBehavior_h_ +#ifndef TGT_HAS_LEDS +# error FlashIPAddrBehavior requires a target with LEDs +#endif + #include "Behaviors/BehaviorBase.h" #include "Motion/MotionManager.h" #include "Motion/MotionSequenceMC.h" @@ -26,11 +30,16 @@ virtual void processEvent(const EventBase& e); static std::string getClassDescription() { - std::string pre="Displays IP address by flashing a series of numbers on the LED face panel; Hold down "; + std::string pre="Displays IP address by flashing a series of numbers on the LED face panel;"; +#ifdef TGT_HAS_BUTTONS + pre+=" Hold down "; pre+=buttonNames[button1]; pre+=" and "; pre+=buttonNames[button2]; pre+=" to trigger any time while running"; +#else + pre+=" Set configuration file to enable display on boot."; +#endif return pre; } virtual std::string getDescription() const { return getClassDescription(); } @@ -42,8 +51,10 @@ void releaseSounds(); //!< releases the numeric sounds void setupSequence(); //!< construct the motion sequence for flashing leds, request timers to play corresponding sound file +#ifdef TGT_HAS_BUTTONS static const unsigned int button1=ChinButOffset; //!< one of two buttons which must be pressed together to trigger the report without using the Controller static const unsigned int button2=HeadFrButOffset; //!< one of two buttons which must be pressed together to trigger the report without using the Controller +#endif static const unsigned int ACTIVATE_TIMER=-1U; //!< timer id to specify both trigger buttons have been down long enough std::vector sounds; //!< sound to play, corresponding to timers to coincide with corresponding digit on the LEDs (could be done with chained sounds, but this is cooler) diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/StateNode.h ./Behaviors/StateNode.h --- ../Tekkotsu_3.0/Behaviors/StateNode.h 2006-10-03 18:11:44.000000000 -0400 +++ ./Behaviors/StateNode.h 2007-08-14 14:23:27.000000000 -0400 @@ -9,14 +9,16 @@ //! Recursive data structure - both a state machine controller as well as a node within a state machine itself /*! - * Override DoStart() / DoStop() as you would a normal BehaviorBase subclass to - * have this node add some functionality of its own. - * * Override setup() to build your own Transition and StateNode network if you want * this node to contain a state machine. * + * Override DoStart() / DoStop() as you would a normal BehaviorBase subclass to + * have this node add some functionality of its own. + * * You can override setup to create a sub-network, as well as overriding DoStart and DoStop, in the same class. * + * See also the tutorial page on State Machines. + * * There are two StateNode templates in project/templates/: * - statenode.h * is intended for leaf nodes, which directly implement the execution of a task. diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Transition.cc ./Behaviors/Transition.cc --- ../Tekkotsu_3.0/Behaviors/Transition.cc 2006-09-18 14:08:05.000000000 -0400 +++ ./Behaviors/Transition.cc 2007-03-02 12:20:51.000000000 -0500 @@ -12,7 +12,7 @@ if(sound.size()!=0) sndman->playFile(sound); - erouter->postEvent(EventBase::stateTransitionEGID,reinterpret_cast(this),EventBase::statusETID,0,getName(),1); + erouter->postEvent(EventBase::stateTransitionEGID,reinterpret_cast(this),EventBase::activateETID,0,getName(),1); for(unsigned int i=0; iisActive()) //It's usually a bad idea to call DoStop/DoStart when it's already stopped/started... @@ -21,6 +21,8 @@ if(!dsts[i]->isActive()) dsts[i]->DoStart(); + erouter->postEvent(EventBase::stateTransitionEGID,reinterpret_cast(this),EventBase::deactivateETID,0,getName(),0); + //serr->printf("%s fire() - leave %d\n",getName().c_str(),get_time()); RemoveReference(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Transitions/CompareTrans.h ./Behaviors/Transitions/CompareTrans.h --- ../Tekkotsu_3.0/Behaviors/Transitions/CompareTrans.h 2005-04-15 17:31:15.000000000 -0400 +++ ./Behaviors/Transitions/CompareTrans.h 2007-06-05 14:04:30.000000000 -0400 @@ -12,7 +12,7 @@ * * For example, if you want to transition when the IR sensor goes below, say 200, * pass &state->sensors[IRDistOffset], CompareTrans::LT, 200, and - * EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID) + * EventBase(EventBase::sensorEGID,SensorSrcID::UpdatedSID,EventBase::statusETID) * as the polling event. Or a timer event to just check at a certain interval. * * If you pass a class as the templated type, only requires that < operator is diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Transitions/CompletionTrans.h ./Behaviors/Transitions/CompletionTrans.h --- ../Tekkotsu_3.0/Behaviors/Transitions/CompletionTrans.h 2004-12-22 20:47:07.000000000 -0500 +++ ./Behaviors/Transitions/CompletionTrans.h 2007-11-12 23:16:01.000000000 -0500 @@ -30,7 +30,7 @@ completions[i] = false; erouter->addListener(this, EventBase::stateMachineEGID, - (unsigned int)getSources()[i], + reinterpret_cast(getSources()[i]), EventBase::statusETID); }; } @@ -47,7 +47,7 @@ virtual void processEvent(const EventBase &event) { int numcomplete = 0; for ( unsigned int i=0; i(getSources()[i]) ) completions[i] = true; if ( completions[i] ) ++numcomplete; }; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Transitions/EventTrans.h ./Behaviors/Transitions/EventTrans.h --- ../Tekkotsu_3.0/Behaviors/Transitions/EventTrans.h 2004-12-22 20:47:07.000000000 -0500 +++ ./Behaviors/Transitions/EventTrans.h 2007-11-13 00:13:31.000000000 -0500 @@ -11,44 +11,63 @@ private: int argcount; //!< the number of arguments which were supplied to the constructor (granularity of filtering) EventBase::EventGeneratorID_t egid; //!< the requested generator - unsigned int esid; //!< the requested source + size_t esid; //!< the requested source + unsigned int *esidAddr; EventBase::EventTypeID_t etid; //!< the requested type public: //!@name Constructors //! follows general pattern of EventRouter::addListener() EventTrans(StateNode* destination, EventBase::EventGeneratorID_t gid) - : Transition("EventTrans",destination), argcount(1), egid(gid), esid(0), etid(EventBase::statusETID) + : Transition("EventTrans",destination), argcount(1), egid(gid), esid(0), esidAddr(NULL), etid(EventBase::statusETID) {} - EventTrans(StateNode* destination, EventBase::EventGeneratorID_t gid, unsigned int sid) - : Transition("EventTrans",destination), argcount(2), egid(gid), esid(sid), etid(EventBase::statusETID) + EventTrans(StateNode* destination, EventBase::EventGeneratorID_t gid, size_t sid) + : Transition("EventTrans",destination), argcount(2), egid(gid), esid(sid), esidAddr(NULL), etid(EventBase::statusETID) {} - EventTrans(StateNode* destination, EventBase::EventGeneratorID_t gid, unsigned int sid, EventBase::EventTypeID_t tid) - : Transition("EventTrans",destination), argcount(3), egid(gid), esid(sid), etid(tid) + EventTrans(StateNode* destination, EventBase::EventGeneratorID_t gid, unsigned int *sidAddr) + : Transition("EventTrans",destination), argcount(2), egid(gid), esid(0), esidAddr(sidAddr), etid(EventBase::statusETID) + {} + + EventTrans(StateNode* destination, EventBase::EventGeneratorID_t gid, size_t sid, EventBase::EventTypeID_t tid) + : Transition("EventTrans",destination), argcount(3), egid(gid), esid(sid), esidAddr(NULL), etid(tid) + {} + + EventTrans(StateNode* destination, EventBase::EventGeneratorID_t gid, unsigned int* sidAddr, EventBase::EventTypeID_t tid) + : Transition("EventTrans",destination), argcount(3), egid(gid), esid(0), esidAddr(sidAddr), etid(tid) {} EventTrans(const std::string& name, StateNode* destination, EventBase::EventGeneratorID_t gid) - : Transition("EventTrans",name,destination), argcount(1), egid(gid), esid(0), etid(EventBase::statusETID) + : Transition("EventTrans",name,destination), argcount(1), egid(gid), esid(0), esidAddr(NULL), etid(EventBase::statusETID) {} - EventTrans(const std::string& name, StateNode* destination, EventBase::EventGeneratorID_t gid, unsigned int sid) - : Transition("EventTrans",name,destination), argcount(2), egid(gid), esid(sid), etid(EventBase::statusETID) + EventTrans(const std::string& name, StateNode* destination, EventBase::EventGeneratorID_t gid, size_t sid) + : Transition("EventTrans",name,destination), argcount(2), egid(gid), esid(sid), esidAddr(NULL), etid(EventBase::statusETID) {} - EventTrans(const std::string& name, StateNode* destination, EventBase::EventGeneratorID_t gid, unsigned int sid, EventBase::EventTypeID_t tid) - : Transition("EventTrans",name,destination), argcount(3), egid(gid), esid(sid), etid(tid) + EventTrans(const std::string& name, StateNode* destination, EventBase::EventGeneratorID_t gid, unsigned int *sidAddr) + : Transition("EventTrans",name,destination), argcount(2), egid(gid), esid(0), esidAddr(sidAddr), etid(EventBase::statusETID) + {} + + EventTrans(const std::string& name, StateNode* destination, EventBase::EventGeneratorID_t gid, size_t sid, EventBase::EventTypeID_t tid) + : Transition("EventTrans",name,destination), argcount(3), egid(gid), esid(sid), esidAddr(NULL), etid(tid) + {} + EventTrans(const std::string& name, StateNode* destination, EventBase::EventGeneratorID_t gid, unsigned int *sidAddr, EventBase::EventTypeID_t tid) + : Transition("EventTrans",name,destination), argcount(3), egid(gid), esid(0), esidAddr(sidAddr), etid(tid) {} //@} //! starts listening virtual void DoStart() { Transition::DoStart(); - switch (argcount) { - case 1: erouter->addListener(this,egid); break; - case 2: erouter->addListener(this,egid,esid); break; - case 3: erouter->addListener(this,egid,esid,etid); + if ( esidAddr != 0 ) + erouter->addListener(this,egid); // must check sid dynamically as events come in + else + switch (argcount) { + case 1: erouter->addListener(this,egid); break; + case 2: erouter->addListener(this,egid,esid); break; + case 3: erouter->addListener(this,egid,esid,etid); }; } @@ -59,7 +78,17 @@ } //! fire the transition if an event is seen - virtual void processEvent(const EventBase&) { fire(); } + virtual void processEvent(const EventBase &event) { + if ( esidAddr == 0 ) + fire(); + else if ( event.getSourceID() == *esidAddr ) + if ( argcount == 2 || event.getTypeID() == etid ) + fire(); + } + +private: + EventTrans(const EventTrans&); //!< do not call + EventTrans& operator=(const EventTrans&); //!< do not call }; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Transitions/RandomTrans.cc ./Behaviors/Transitions/RandomTrans.cc --- ../Tekkotsu_3.0/Behaviors/Transitions/RandomTrans.cc 2006-09-18 14:08:04.000000000 -0400 +++ ./Behaviors/Transitions/RandomTrans.cc 2007-03-02 12:20:53.000000000 -0500 @@ -33,6 +33,9 @@ void RandomTrans::fire() { AddReference(); // for safety + + erouter->postEvent(EventBase::stateTransitionEGID,reinterpret_cast(this),EventBase::activateETID,0,getName(),1); + if ( sound.size()!=0 ) sndman->playFile(sound); @@ -56,6 +59,8 @@ }; } + erouter->postEvent(EventBase::stateTransitionEGID,reinterpret_cast(this),EventBase::deactivateETID,0,getName(),0); + RemoveReference(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Transitions/RandomTrans.h ./Behaviors/Transitions/RandomTrans.h --- ../Tekkotsu_3.0/Behaviors/Transitions/RandomTrans.h 2005-01-04 14:51:41.000000000 -0500 +++ ./Behaviors/Transitions/RandomTrans.h 2006-11-13 09:44:06.000000000 -0500 @@ -22,7 +22,7 @@ RandomTrans(const std::string& name, StateNode* destination, float weight=1); //! Add a destination node with weight==1 (see addDestination(StateNode* dest, float weight) to specify a different weight) - virtual void RandomTrans::addDestination(StateNode* destination) { + virtual void addDestination(StateNode* destination) { addDestination(destination,1); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/Behaviors/Transitions/VisualTargetCloseTrans.h ./Behaviors/Transitions/VisualTargetCloseTrans.h --- ../Tekkotsu_3.0/Behaviors/Transitions/VisualTargetCloseTrans.h 2005-06-13 17:22:41.000000000 -0400 +++ ./Behaviors/Transitions/VisualTargetCloseTrans.h 2007-06-26 00:27:45.000000000 -0400 @@ -33,15 +33,11 @@ ASSERTRET(ve!=NULL,"Casting error"); float x=ve->getCenterX(); float y=ve->getCenterY(); - unsigned int IRDistOffset=-1U; + unsigned int IRDistOffset=::IRDistOffset; //The ERS-7 adds more IR distance sensors, so we have to - //break it down by model so we can specify which one - 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; + //break it down so we can specify which one + if(RobotName == ERS7Info::TargetName) + IRDistOffset=(distanceThreshold<350) ? ERS7Info::NearIRDistOffset : ERS7Info::FarIRDistOffset; if(x*x+y*y<0.02f && IRDistOffset!=-1U && state->sensors[IRDistOffset] class Sketch; +//! Representation of the robot on the world map class AgentData : public BaseData { private: Point center_pt; @@ -49,15 +50,14 @@ const NEWMAT::ColumnVector& groundplane); //! Transformations. (Virtual in BaseData.) - virtual void applyTransform(const NEWMAT::Matrix& Tmat); - + virtual void applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref=unspecified); virtual unsigned short getDimension() const { return 3; } AngTwoPi getOrientation() const { return orientation; } protected: - //! Updates orientation according to @param orientation_pt + //! Updates orientation according to #orientation_pt void updateOrientation(); private: diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/BaseData.cc ./DualCoding/BaseData.cc --- ../Tekkotsu_3.0/DualCoding/BaseData.cc 2006-08-10 23:20:22.000000000 -0400 +++ ./DualCoding/BaseData.cc 2007-11-11 18:57:20.000000000 -0500 @@ -1,5 +1,5 @@ #include "Macrodefs.h" -#include "Measures.h" +#include "Shared/Measures.h" #include "Sketch.h" // this must precede references to Sketch #include "BaseData.h" @@ -8,11 +8,13 @@ #include "SketchDataRoot.h" #include "ShapeSpace.h" +using namespace std; + namespace DualCoding { BoundingBox::BoundingBox(const BoundingBox &b1, const BoundingBox &b2) : - xmin(min(b1.xmin,b2.xmin)), ymin(min(b1.ymin,b2.ymin)), - xmax(max(b1.xmax,b2.xmax)), ymax(max(b1.ymax,b2.ymax)) {} + xmin(std::min(b1.xmin,b2.xmin)), ymin(std::min(b1.ymin,b2.ymin)), + xmax(std::max(b1.xmax,b2.xmax)), ymax(std::max(b1.ymax,b2.ymax)) {} BoundingBox::BoundingBox(const std::vector &vec) : xmin(0), ymin(0), xmax(0), ymax(0) { @@ -142,18 +144,22 @@ bool BaseData::isSameColorAs(const ShapeRoot& other) const { return getColor() == other->getColor(); } -void BaseData::setColor(string const &color_name) { +void BaseData::setColor(const std::string &color_name) { setColor(ProjectInterface::getColorRGB(color_name)); } -void BaseData::setColor(rgb new_color) { +void BaseData::setColor(const rgb &new_color) { color_rgb = new_color; if ( rendering_sketch != NULL ) (*rendering_sketch)->setColor(new_color); } +void BaseData::setColor(const unsigned int color_index) { + setColor(ProjectInterface::getColorRGB(color_index)); +} + -bool BaseData::isMobile() const { return mobile; } +bool BaseData::getMobile() const { return mobile; } void BaseData::setMobile(bool _mobile) { mobile = _mobile; } @@ -176,7 +182,7 @@ void BaseData::increaseConfidence(int n, int maxConfidence) { confidence += n; if ( maxConfidence > 0 ) - confidence = min(confidence, maxConfidence); + confidence = std::min(confidence, maxConfidence); } void BaseData::increaseConfidence(const BaseData& other, int maxConfidence) { diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/BaseData.h ./DualCoding/BaseData.h --- ../Tekkotsu_3.0/DualCoding/BaseData.h 2006-08-10 23:20:22.000000000 -0400 +++ ./DualCoding/BaseData.h 2007-04-06 22:18:33.000000000 -0400 @@ -8,7 +8,7 @@ #include "Wireless/Socket.h" // needed for sout #include "Macrodefs.h" -#include "Measures.h" +#include "Shared/Measures.h" #include "Point.h" #include "Shared/newmat/newmat.h" #include "Motion/Kinematics.h" @@ -22,9 +22,11 @@ class SketchSpace; template class Sketch; class ShapeRoot; +class ShapeSpace; class SketchDataRoot; template class Shape; +//! Bounding box of a shape; used for coordinate calculations class BoundingBox { public: coordinate_t xmin, ymin, xmax, ymax; @@ -45,6 +47,7 @@ std::ostream& operator<< (std::ostream& out, const BoundingBox &b); +//! Base class that all shape data classes inherit from, e.g., @a LineData, @a BlobData, etc. class BaseData { public: friend class ShapeRoot; @@ -145,8 +148,9 @@ //! Set shape and rendering sketch color. //@{ - void setColor(std::string const &color_name); - void setColor(rgb new_color); + void setColor(const std::string &color_name); + void setColor(const rgb &new_color); + void setColor(const unsigned int color_index); //@} //! Test if shape colors are the same. @@ -173,7 +177,7 @@ //! Mobility //@{ - bool isMobile() const; + bool getMobile() const; void setMobile(bool mobility); //@} @@ -188,7 +192,7 @@ virtual void printParams() const=0; //! Apply a transformation matrix to the shape. - virtual void applyTransform(const NEWMAT::Matrix& Tmat)=0; + virtual void applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref=unspecified)=0; //! Project to ground plane using given matrix virtual void projectToGround(const NEWMAT::Matrix& camToBase, @@ -203,10 +207,12 @@ //! If no such rendering exists, it is created. Sketch& getRendering(); +private: //! Render into a sketch space. virtual Sketch* render() const=0; //@} +public: //! Copy operator. Assumes "&other =? this" check is done by the sub class calling this operator BaseData& operator=(const BaseData& other); }; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/BlobData.cc ./DualCoding/BlobData.cc --- ../Tekkotsu_3.0/DualCoding/BlobData.cc 2006-08-02 17:27:14.000000000 -0400 +++ ./DualCoding/BlobData.cc 2007-11-11 18:57:20.000000000 -0500 @@ -19,17 +19,21 @@ #include "BrickOps.h" +using namespace std; + namespace DualCoding { BlobData::BlobData(ShapeSpace& _space, const Point &_topLeft, const Point &_topRight, const Point &_bottomLeft, const Point &_bottomRight, const float _area, const std::vector &_runvec, - const BlobOrientation_t _orientation, rgb _rgbvalue) : + const BlobOrientation_t _orientation, coordinate_t _assumedHeight, + rgb _rgbvalue) : BaseData(_space, getStaticType()), + orientation(_orientation), assumedHeight(_assumedHeight), topLeft(_topLeft), topRight(_topRight), bottomLeft(_bottomLeft), bottomRight(_bottomRight), - area(_area), runvec(_runvec), orientation(_orientation) + area(_area), runvec(_runvec) { setColor(_rgbvalue); } @@ -43,12 +47,13 @@ } void BlobData::printParams() const { - cout << "Blob" - << " tl=" << topLeft.coords - << " tr=" << topRight.coords - << " bl=" << bottomLeft.coords - << " br=" << bottomRight.coords - << " area=" << area + cout << "Type = " << getTypeName() << " ID=" << getId() << " ParentID=" << getParentId() << endl; + printf(" color = %d %d %d\n",getColor().red,getColor().green,getColor().blue); + cout << " tl=" << NEWMAT::printmat(topLeft.coords) << endl + << " tr=" << NEWMAT::printmat(topRight.coords) << endl + << " bl=" << NEWMAT::printmat(bottomLeft.coords) << endl + << " br=" << NEWMAT::printmat(bottomRight.coords) << endl + << " area=" << area << ", assumedHeight=" << assumedHeight << endl; } @@ -77,39 +82,24 @@ } break; case pillar: - case pinata: - cout << "BlobData::render() : Can't yet render blobs in pillar/pinata orientations" << endl; + case poster: + cout << "BlobData::render() : Can't yet render blobs in pillar/poster orientations" << endl; break; } return result; } //! Transformations. (Virtual in BaseData.) -void BlobData::applyTransform(const NEWMAT::Matrix& Tmat) { - switch ( orientation ) { - - case groundplane: - topLeft.applyTransform(Tmat); - topRight.applyTransform(Tmat); - bottomLeft.applyTransform(Tmat); - bottomRight.applyTransform(Tmat); - break; - - case pillar: - case pinata: - // *** HACK: THIS IS NOT THE RIGHT WAY TO CALCULATE HEIGHT - Point height_incr((topLeft-bottomLeft + topRight-bottomRight) / 2); - height_incr.coords << 0 << 0; - bottomLeft.applyTransform(Tmat); - bottomRight.applyTransform(Tmat); - topLeft = bottomLeft + height_incr; - topRight = bottomRight + height_incr; - break; - }; +void BlobData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) { + topLeft.applyTransform(Tmat,newref); + topRight.applyTransform(Tmat,newref); + bottomLeft.applyTransform(Tmat,newref); + bottomRight.applyTransform(Tmat,newref); } void BlobData::projectToGround(const NEWMAT::Matrix& camToBase, const NEWMAT::ColumnVector& gndplane) { + float const camblob_area = area; switch ( orientation ) { case groundplane: topLeft.projectToGround(camToBase,gndplane); @@ -118,53 +108,42 @@ bottomRight.projectToGround(camToBase,gndplane); break; case pillar: - case pinata: bottomLeft.projectToGround(camToBase,gndplane); bottomRight.projectToGround(camToBase,gndplane); - // DST HACK -- hard coded pillar height 100 mm topLeft = bottomLeft; topRight = bottomRight; - topLeft.coords(3) += 100; - topRight.coords(3) += 100; - } - update_derived_properties(); -} - - /* -void BlobData::projectToGround(int xres, int yres, const NEWMAT::ColumnVector& gndplane) { - switch ( orientation ) { - case groundplane: - topLeft.projectToGround(xres,yres,gndplane); - topRight.projectToGround(xres,yres,gndplane); - bottomLeft.projectToGround(xres,yres,gndplane); - bottomRight.projectToGround(xres,yres,gndplane); + topLeft.coords(3) += assumedHeight; + topRight.coords(3) += assumedHeight; + break; + case poster: + // create an elevated plane by shifting the ground plane by assumedHeight + NEWMAT::ColumnVector elevatedPlane = gndplane; + float displacement = assumedHeight * sqrt(elevatedPlane(1) * elevatedPlane(1) + elevatedPlane(2) * elevatedPlane(2) + elevatedPlane(3) * elevatedPlane(3)); + elevatedPlane(4) += elevatedPlane(3) >= 0 ? displacement : -displacement; + + // Project the centroid onto the elevated plane and set all 4 points to the new centroid + Point centroid = getCentroid(); + centroid.projectToGround(camToBase, elevatedPlane); + bottomLeft = centroid; + bottomRight = centroid; + topLeft = centroid; + topRight = centroid; break; - case pillar: - case pinata: - bottomLeft.projectToGround(xres,yres,gndplane); - bottomRight.projectToGround(xres,yres,gndplane); - // DST HACK -- hard coded pillar height 100 mm - topLeft = bottomLeft; - topRight = bottomRight; - topLeft.coords(3) += 100; - topRight.coords(3) += 100; } update_derived_properties(); + area = camblob_area; // stick with cam area for now } - */ void BlobData::update_derived_properties() { switch ( orientation ) { case groundplane: - // DST HACK -- should use the formula for area of a quadrilaterl - area = (topRight+bottomRight-topLeft-bottomLeft).coords(1) * - (bottomLeft+bottomRight-topLeft-topRight).coords(2) / 4; - break; - case pillar: - case pinata: - // fill this in later + case poster: + // DST HACK -- should project each run to ground and integrate, + // or use the formula for area of a quadrilateral + area = fabs((topRight+bottomRight-topLeft-bottomLeft).coords(1)) * + fabs((bottomLeft+bottomRight-topLeft-topRight).coords(2)) / 4; break; } } @@ -194,42 +173,69 @@ topRight = (topRight*confidence + other_blob->topRight*other_conf) / sumconf; bottomLeft = (bottomLeft*confidence + other_blob->bottomLeft*other_conf) / sumconf; bottomRight = (bottomRight*confidence + other_blob->bottomRight*other_conf) / sumconf; + float const newArea = (area*confidence + other_blob->area*other_conf) / sumconf; update_derived_properties(); + area = newArea; // fixup because update_derived_properties currently messes up on pillar and poster areas return true; } +//================================================================ +// Blob extraction + std::vector > BlobData::extractBlobs(const Sketch &sketch, - const set& colors, int minarea, - BlobData::BlobOrientation_t orient, + int minarea, + BlobOrientation_t orient, + coordinate_t height, int maxblobs) { // We could multiply sketch*color_index to convert to uchar and // spare ourselves the for loop that follows for setting blob // colors, but this way is actually much faster because we skip all // the pixel-wise multiplications. Casting Sketch to // Sketch is done with a simple reinterpret_cast; no copying. - std::vector > result(extractBlobs((Sketch&)sketch,colors,minarea,orient,maxblobs)); - rgb rgbvalue(sketch->getColor()); + set dummyColors; + dummyColors.insert(1); // converting bool to uchar always yields color index 1 + map minareas; + minareas[1] = minarea; + map orients; + orients[1] = orient; + map heights; + heights[1] = height; + std::vector > result(extractBlobs((Sketch&)sketch,dummyColors,minareas,orients,heights,maxblobs)); + rgb sketchColor(sketch->getColor()); for ( std::vector >::iterator it = result.begin(); it != result.end(); it++ ) - (*it)->setColor(rgbvalue); + (*it)->setColor(sketchColor); return result; } std::vector > -BlobData::extractBlobs(const Sketch &sketch, int minarea, - BlobData::BlobOrientation_t orient, int maxblobs) { +BlobData::extractBlobs(const Sketch &sketch, + int minarea, + BlobOrientation_t orient, + coordinate_t height, + int maxblobs) { const int numColors = ProjectInterface::getNumColors(); - set colors; - for (int i = 1; i < numColors; i++) colors.insert(i); - return extractBlobs(sketch, colors, minarea, orient, maxblobs); + set colors; + map minareas; + map orients; + map heights; + for (int i = 1; i < numColors; i++) { + colors.insert(i); + minareas[i] = minarea; + orients[i] = orient; + heights[i] = height; + } + return extractBlobs(sketch, colors, minareas, orients, heights, maxblobs); } - std::vector > BlobData::extractBlobs(const Sketch &sketch, - const set& colors, int minarea, - BlobData::BlobOrientation_t orient, int maxblobs) { + const std::set& colors, + const std::map& minareas, + const std::map& orients, + const std::map& heights, + int maxblobs) { int parent = sketch->getId(); uchar *pixels = &((*sketch.pixels)[0]); @@ -254,16 +260,19 @@ result.clear(); ShapeSpace &ShS = sketch->getSpace().getDualSpace(); // for ( size_t color=1; color < numColors; color++ ) { - for (set::const_iterator it = colors.begin(); + for (set::const_iterator it = colors.begin(); it != colors.end(); it++) { - // const CMVision::region* list_head = ccs[color].list; - const CMVision::region* list_head = ccs[*it].list; + int const color = *it; + const CMVision::region* list_head = ccs[color].list; if ( list_head != NULL ) { - // const rgb rgbvalue = ProjectInterface::getColorRGB(color); - const rgb rgbvalue = ProjectInterface::getColorRGB(*it); + const rgb rgbvalue = ProjectInterface::getColorRGB(color); + int const minarea = (minareas.find(color)==minareas.end()) ? 50 : const_cast&>(minareas)[color]; + BlobOrientation_t const orient = (orients.find(color)==orients.end()) ? + groundplane : const_cast&>(orients)[color]; + coordinate_t const height = (heights.find(color)==heights.end()) ? 0 : const_cast&>(heights)[color]; for (int i=0; list_head!=NULL && iarea >= minarea; list_head = list_head->next, i++) { - BlobData* blobdat = new_blob(ShS,*list_head, rle_buffer, orient, rgbvalue); + BlobData* blobdat = new_blob(ShS,*list_head, rle_buffer, orient, height, rgbvalue); blobdat->setParentId(parent); result.push_back(Shape(blobdat)); } @@ -275,20 +284,11 @@ return result; } -std::vector > -BlobData::extractBlobs(const Sketch &sketch, int minarea, - BlobData::BlobOrientation_t orient, int maxblobs) { - const int numColors = ProjectInterface::getNumColors(); - set colors; - for (int i = 1; i < numColors; i++) colors.insert(i); - return extractBlobs(sketch, colors, minarea, orient, maxblobs); -} - - BlobData* BlobData::new_blob(ShapeSpace& space, const CMVision::region ®, - const CMVision::run *rle_buff, - const BlobData::BlobOrientation_t orient, + const CMVision::run *rle_buff, + const BlobOrientation_t orient, + const coordinate_t height, const rgb rgbvalue) { int const x1 = reg.x1; int const y1 = reg.y1; @@ -312,34 +312,34 @@ return new BlobData(space, Point(x1,y1),Point(x2,y1), Point(x1,y2),Point(x2,y2), - reg.area, runvec, orient, rgbvalue); + reg.area, runvec, orient, height, rgbvalue); else return new BlobData(space, Point(x2,y2),Point(x1,y2), Point(x2,y1),Point(x1,y1), - reg.area, runvec, orient, rgbvalue); + reg.area, runvec, orient, height, rgbvalue); } +//================================================================ +// Corner extraction: General call to find the corners of a blob. +// Used in brick / pyramid extraction +// +// +// Requires the number of corners you expect to find. +// Currently just uses shape fitting to find the corners +// Originally used either the derivative or diagonal approaches to finding corners +// +// Shape fitting works very well, but is very slow. +// (no attempt was made to optimize it though) +// +// The old diagonal/derivative approach is at the bottom +// It is much faster, but less robust. + std::vector BlobData::findCorners(unsigned int nExpected, + std::vector& candidates, + float &bestValue) { - - - // General call to find the corners of a blob. - // Used in brick / pyramid extraction - // - // - // Requires the number of corners you expect to find. - // Currently just uses shape fitting to find the corners - // Originally used either the derivative or diagonal approaches to finding corners - // - // Shape fitting works very well, but is very slow. - // (no attempt was made to optimize it though) - // - // The old diagonal/derivative approach is at the bottom - // It is much faster, but less robust. - std::vector BlobData::findCorners(unsigned int nExpected, std::vector& candidates, float &bestValue) - { std::vector fitCorners = findCornersShapeFit(nExpected, candidates, bestValue); // debug output @@ -454,8 +454,6 @@ return fitCorners; - - // Old method, used the diagonal and derivative approaches and took the better results // Was generally much faster, but broke down in some special cases /* @@ -905,7 +903,7 @@ // Comparison predicates -bool BlobData::areaLessThan::operator() (const Shape &b1, const Shape &b2) const { +bool BlobData::AreaLessThan::operator() (const Shape &b1, const Shape &b2) const { return b1->getArea() < b2->getArea(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/BlobData.h ./DualCoding/BlobData.h --- ../Tekkotsu_3.0/DualCoding/BlobData.h 2006-07-21 13:57:44.000000000 -0400 +++ ./DualCoding/BlobData.h 2007-03-28 05:19:17.000000000 -0400 @@ -4,6 +4,7 @@ #include #include +#include #include #include @@ -11,8 +12,9 @@ #include "Vision/cmv_types.h" #include "BaseData.h" // superclass -#include "Point.h" // Point data member -#include "ShapeTypes.h" // blobDataType +#include "Point.h" // Point data member +#include "SketchTypes.h" // uchar +#include "ShapeTypes.h" // blobDataType #include "ShapeFuns.h" namespace DualCoding { @@ -25,10 +27,12 @@ class BlobData : public BaseData { public: - // Bounding quadrilateral; may not be square when projected to world space. - Point topLeft, topRight, bottomLeft, bottomRight; - - float area; + //! Assumed orientation of the blob in 3D space. + enum BlobOrientation_t { + groundplane, //!< 2D shape lying flat on the ground + pillar, //!< 3D shape standing on the ground + poster //!< 3D shape hanging vertically in space + }; struct run { public: @@ -38,24 +42,24 @@ x(_x), y(_y), width(_width) {} }; - const std::vector runvec; - - //! Assumed orientation of the blob in 3D space. - enum BlobOrientation_t { - groundplane, //!< 2D shape lying flat on the ground - pillar, //!< 3D shape standing on the ground - pinata //!< 3D shape hanging vertically in space - } orientation; + // Data members + BlobOrientation_t orientation; //!< Orientation of the blob + coordinate_t assumedHeight; //!< Assumed height above ground of blob centroid (for poster) or top (for pillar) + //! Bounding quadrilateral: may not be square when projected to ground space. + Point topLeft, topRight, bottomLeft, bottomRight; + float area; //!< Area of the blob; may not be integer when projected to ground space + const std::vector runvec; //!< Runs (for rendering in camera space) public: //! Constructor BlobData(ShapeSpace& _space, const Point &_topLeft, const Point &_topRight, const Point &_bottomLeft, const Point &_bottomRight, - const float _area, - const std::vector &_runvec, - const BlobOrientation_t _orientation, - const rgb rgbvalue); + const float _area=0, + const std::vector &_runvec=std::vector(), + const BlobOrientation_t _orientation=groundplane, + const coordinate_t assumedHeight=0, + const rgb rgbvalue=rgb()); static ShapeType_t getStaticType() { return blobDataType; } @@ -73,7 +77,7 @@ virtual void printParams() const; //! Transformations. (Virtual in BaseData.) - virtual void applyTransform(const NEWMAT::Matrix& Tmat); + virtual void applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref=unspecified); //! Project to ground // virtual void projectToGround(int xres, int yres, const NEWMAT::ColumnVector& groundplane); @@ -93,45 +97,48 @@ //! Import blobs from Sketch as a vector of Shape static std::vector > - extractBlobs(const Sketch &sketch, - const set& colors, int minarea=0, - BlobData::BlobOrientation_t orient=BlobData::groundplane, - int maxblobs=50); - static std::vector > - extractBlobs(const Sketch &sketch, int minarea=0, - BlobData::BlobOrientation_t orient=BlobData::groundplane, + extractBlobs(const Sketch &sketch, + int minarea=25, + BlobOrientation_t orient=BlobData::groundplane, + coordinate_t height=0, int maxblobs=50); - //! Import blobs from Sketch as a vector of Shape + //! Import blobs of all colors from Sketch as a vector of Shape static std::vector > - extractBlobs(const Sketch &sketch, - const set& colors, int minarea=0, - BlobData::BlobOrientation_t orient=BlobData::groundplane, + extractBlobs(const Sketch &sketch, + int minarea=25, + BlobOrientation_t orient=BlobData::groundplane, + const coordinate_t height=0, int maxblobs=50); + + //! Import blobs of specified colors from Sketch as a vector of Shape static std::vector > - extractBlobs(const Sketch &sketch, int minarea=0, - BlobData::BlobOrientation_t orient=BlobData::groundplane, - int maxblobs=50); + extractBlobs(const Sketch &sketch, + const std::set& colors, + const std::map& minareas, + const std::map& orients, + const std::map& heights, + int maxblobs); - static BlobData* - new_blob(ShapeSpace& space, + //! Utility function for making a new blob instance from CMVision's region data structures + static BlobData* new_blob(ShapeSpace& space, const CMVision::region ®, const CMVision::run *rle_buff, - const BlobData::BlobOrientation_t orient, + const BlobOrientation_t orient, + const coordinate_t height, const rgb rgbvalue); + //!@name Corner extraction for rectangular blobs + //@{ std::vector findCorners(unsigned int nExpected, std::vector& candidates, float &bestValue); - std::vector findCornersDerivative(); - std::vector findCornersDiagonal(); - std::vector findCornersShapeFit(unsigned int ncorners, std::vector& candidates, float &bestValue); - + //@} // comparison predicates - class areaLessThan : public BinaryShapePred { + class AreaLessThan : public BinaryShapePred { public: bool operator() (const Shape &b1, const Shape &b2) const; }; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/BrickData.cc ./DualCoding/BrickData.cc --- ../Tekkotsu_3.0/DualCoding/BrickData.cc 2006-09-23 23:45:20.000000000 -0400 +++ ./DualCoding/BrickData.cc 2007-11-11 18:57:20.000000000 -0500 @@ -19,6 +19,7 @@ #include "ShapePoint.h" #include "Region.h" +using namespace std; namespace DualCoding { @@ -96,15 +97,15 @@ //! Transformations. (Virtual in BaseData.) -void BrickData::applyTransform(const NEWMAT::Matrix& Tmat) { - GFL.applyTransform(Tmat); - GFR.applyTransform(Tmat); - GBL.applyTransform(Tmat); - GBR.applyTransform(Tmat); - TFL.applyTransform(Tmat); - TFR.applyTransform(Tmat); - TBL.applyTransform(Tmat); - TBR.applyTransform(Tmat); +void BrickData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) { + GFL.applyTransform(Tmat,newref); + GFR.applyTransform(Tmat,newref); + GBL.applyTransform(Tmat,newref); + GBR.applyTransform(Tmat,newref); + TFL.applyTransform(Tmat,newref); + TFR.applyTransform(Tmat,newref); + TBL.applyTransform(Tmat,newref); + TBR.applyTransform(Tmat,newref); } void BrickData::projectToGround(const NEWMAT::Matrix& camToBase, @@ -129,7 +130,8 @@ TBR.setCoords(GBR.coordX(), GBR.coordY(), GBR.coordZ() + BRHeight); centroid = (GFL + GFR + GBL + GBR)/4; centroid.setCoords(centroid.coordX(), centroid.coordY(), brickHeight/2); - std::cout<<"New centroid: ("< #include -#include #include "Shared/newmat/newmat.h" @@ -74,7 +73,7 @@ virtual void printParams() const; //! Transformations. (Virtual in BaseData.) - void applyTransform(const NEWMAT::Matrix& Tmat); + void applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref=unspecified); //! Project to ground virtual void projectToGround(const NEWMAT::Matrix& camToBase, @@ -90,9 +89,9 @@ std::vector > blobs2, std::vector > blobs3); - static Shape extractBrick(ShapeSpace& space, vector > &blobs); + static Shape extractBrick(ShapeSpace& space, std::vector > &blobs); - static vector findOrthogonalBoundingBox(ShapeSpace& space, Shape blob, Point centroid, Shape parallel); + static std::vector findOrthogonalBoundingBox(ShapeSpace& space, Shape blob, Point centroid, Shape parallel); private: //! Render into a sketch space and return reference. (Private.) Sketch* render() const; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/BrickOps.cc ./DualCoding/BrickOps.cc --- ../Tekkotsu_3.0/DualCoding/BrickOps.cc 2006-08-10 23:19:53.000000000 -0400 +++ ./DualCoding/BrickOps.cc 2007-11-11 18:57:20.000000000 -0500 @@ -1,6 +1,8 @@ #include "BrickOps.h" #include "visops.h" +using namespace std; + namespace DualCoding { /* Begin functionality for distance-from-center methods of finding the corners of a blob*/ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/BrickOps.h ./DualCoding/BrickOps.h --- ../Tekkotsu_3.0/DualCoding/BrickOps.h 2006-08-10 23:19:53.000000000 -0400 +++ ./DualCoding/BrickOps.h 2007-11-11 18:57:20.000000000 -0500 @@ -25,19 +25,19 @@ void applyGaussian(float x[], float gx[], int len); - float getBoundingQuadrilateralScore(BlobData &blob, vector& corners, Sketch edgeImage, + float getBoundingQuadrilateralScore(BlobData &blob, std::vector& corners, Sketch edgeImage, int& borderCount, ShapeSpace &space); float getBoundingQuadrilateralInteriorPointRatio(BlobData &blob, - vector& corners, + std::vector& corners, ShapeSpace &space); - float getQuadrilateralArea(vector& corners); + float getQuadrilateralArea(std::vector& corners); - int countBorderPixelFit(BlobData &blob, const vector &corners, + int countBorderPixelFit(BlobData &blob, const std::vector &corners, Sketch edges, ShapeSpace &space); - int pickMove(vector scores, float weight); + int pickMove(std::vector scores, float weight); } #endif /* _BRICK_OPS_H_ */ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/DualCoding.h ./DualCoding/DualCoding.h --- ../Tekkotsu_3.0/DualCoding/DualCoding.h 2006-07-21 13:57:47.000000000 -0400 +++ ./DualCoding/DualCoding.h 2007-11-09 14:01:16.000000000 -0500 @@ -5,9 +5,9 @@ #define LOADED_DualCoding_h_ //! Dual coding vision representations (Sketches and Shapes) +/*! For tutorial, see http://www.cs.cmu.edu/~dst/Tekkotsu/Tutorial/vr-intro.shtml */ namespace DualCoding {} -#include "Measures.h" #include "ShapeTypes.h" #include "SketchTypes.h" @@ -15,61 +15,30 @@ #include "ShapeSpace.h" #include "ShapeFuns.h" -#include "SketchDataRoot.h" -#include "SketchData.h" -#include "SketchPoolRoot.h" -#include "SketchPool.h" #include "Sketch.h" #include "SketchIndices.h" #include "Region.h" #include "visops.h" -#include "Point.h" -#include "EndPoint.h" - -#include "BaseData.h" #include "ShapeRoot.h" - -#include "LineData.h" #include "ShapeLine.h" - -#include "EllipseData.h" #include "ShapeEllipse.h" - -#include "PointData.h" #include "ShapePoint.h" - -#include "AgentData.h" #include "ShapeAgent.h" - -#include "SphereData.h" #include "ShapeSphere.h" - -#include "BlobData.h" #include "ShapeBlob.h" - -#include "PolygonData.h" #include "ShapePolygon.h" - -#include "BrickData.h" #include "ShapeBrick.h" - -#include "PyramidData.h" #include "ShapePyramid.h" +#include "ShapeLocalizationParticle.h" -#include "ViewerConnection.h" #include "VisualRoutinesStateNode.h" -#include "VisualRoutinesBehavior.h" // this must preceed mapbuilders, pilot and lookout -#include "VRmixin.h" +#include "VisualRoutinesBehavior.h" // this must precede mapbuilders, pilot and lookout -#include "LookoutRequests.h" #include "Lookout.h" -#include "MapBuilderRequests.h" #include "MapBuilder.h" #include "Pilot.h" -#include "Particle.h" -#include "ParticleShapes.h" -#include "ParticleFilter.h" +#include "PFShapeSLAM.h" #endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/EllipseData.cc ./DualCoding/EllipseData.cc --- ../Tekkotsu_3.0/DualCoding/EllipseData.cc 2006-08-02 17:27:13.000000000 -0400 +++ ./DualCoding/EllipseData.cc 2007-11-11 18:57:20.000000000 -0500 @@ -6,7 +6,7 @@ #include "BaseData.h" // superclass #include "Point.h" // Point data member -#include "Measures.h" // coordinate_t; AngPi data member +#include "Shared/Measures.h" // coordinate_t; AngPi data member #include "ShapeTypes.h" // ellipseDataType #include "SketchSpace.h" @@ -20,13 +20,16 @@ #include "EllipseData.h" #include "ShapeEllipse.h" +using namespace std; + namespace DualCoding { inline int round(float x) { return (int) ceil((double)x-0.5f); } -EllipseData::EllipseData(ShapeSpace& _space, const Point &c) + EllipseData::EllipseData(ShapeSpace& _space, const Point &c, + const float _semimajor, const float _semiminor, const float _orientation) : BaseData(_space, getStaticType()), - center_pt(c), semimajor(0), semiminor(0), orientation(0) + center_pt(c), semimajor(_semimajor), semiminor(_semiminor), orientation(_orientation) { center_pt.setRefFrameType(getRefFrameType()); mobile = ELLIPSE_DATA_MOBILE; } @@ -65,22 +68,14 @@ } //! Print information about this shape. (Virtual in BaseData.) -void -EllipseData::printParams() const { - cout << "Type = " << getTypeName(); - cout << "Shape ID = " << getId() << endl; - cout << "Parent ID = " << getParentId() << endl; - - // Print critical points. - cout << endl; - cout << "center{" << centerPt().coordX() << ", " << centerPt().coordY() << "}" << endl; - - cout << "semimajor = " << getSemimajor() << endl; - cout << "semiminor = " << getSemiminor() << endl; - cout << "orientation = " << getOrientation() << endl; - printf("color = %d %d %d\n",getColor().red,getColor().green,getColor().blue); - cout << "mobile = " << isMobile() << endl; - cout << "viewable = " << isViewable() << endl; +void EllipseData::printParams() const { + cout << "Type = " << getTypeName() << " ID=" << getId() << " ParentID=" << getParentId() << endl; + printf(" color = %d %d %d\n",getColor().red,getColor().green,getColor().blue); + cout << " center = " << centerPt() << endl; + cout << " semimajor = " << getSemimajor() + << ", semiminor = " << getSemiminor() + << ", orientation = " << getOrientation() << endl; + cout << " mobile = " << getMobile() << ", viewable = " << isViewable() << endl; } pair EllipseData::findFeaturePoints() const { @@ -101,12 +96,12 @@ } //! Transformations. (Virtual in BaseData.) -void EllipseData::applyTransform(const NEWMAT::Matrix& Tmat) { +void EllipseData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) { pair featurePts = findFeaturePoints(); - center_pt.applyTransform(Tmat); + center_pt.applyTransform(Tmat,newref); // orientation = orientation - (AngTwoPi)atan2(Tmat(1,2),Tmat(1,1)); - featurePts.first.applyTransform(Tmat); - featurePts.second.applyTransform(Tmat); + featurePts.first.applyTransform(Tmat,newref); + featurePts.second.applyTransform(Tmat,newref); updateProperties(featurePts.first, featurePts.second); } @@ -155,7 +150,7 @@ { const float AREA_TOLERANCE = 0.5; const int REGION_THRESH = 25; - NEW_SKETCH_N(labels,usint,visops::oldlabelcc(sketch,visops::EightWayConnect)); + NEW_SKETCH_N(labels,uint,visops::oldlabelcc(sketch,visops::EightWayConnect)); list regionlist = Region::extractRegions(labels,REGION_THRESH); std::vector > ellipses; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/EllipseData.h ./DualCoding/EllipseData.h --- ../Tekkotsu_3.0/DualCoding/EllipseData.h 2006-05-09 18:37:55.000000000 -0400 +++ ./DualCoding/EllipseData.h 2007-11-11 18:57:20.000000000 -0500 @@ -9,7 +9,7 @@ #include "BaseData.h" // superclass #include "Point.h" // Point data member -#include "Measures.h" // coordinate_t; AngPi data member +#include "Shared/Measures.h" // coordinate_t; AngPi data member namespace DualCoding { @@ -29,7 +29,7 @@ public: //! Constructor - EllipseData(ShapeSpace& _space, const Point &c); + EllipseData(ShapeSpace& _space, const Point &c, const float _semimajor=25, const float _semiminor=15, const float _orientation=0); static ShapeType_t getStaticType() { return ellipseDataType; } @@ -40,7 +40,7 @@ void setCentroidPt(const Point& other) { center_pt.setCoords(other); } //! finds points where semiminor or semimajor axis touchs the circumference of ellipse - pair findFeaturePoints() const; + std::pair findFeaturePoints() const; //! updates major/minor axis and orientation from feature points void updateProperties(const Point& minorPt, const Point& majorPt); @@ -60,7 +60,7 @@ virtual void printParams() const; //! Transformations. (Virtual in BaseData.) - void applyTransform(const NEWMAT::Matrix& Tmat); + void applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref=unspecified); //! Project to ground virtual void projectToGround(const NEWMAT::Matrix& camToBase, diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/EndPoint.h ./DualCoding/EndPoint.h --- ../Tekkotsu_3.0/DualCoding/EndPoint.h 2006-02-12 00:32:01.000000000 -0500 +++ ./DualCoding/EndPoint.h 2007-01-25 14:07:30.000000000 -0500 @@ -2,7 +2,7 @@ #ifndef _ENDPOINT_H_ #define _ENDPOINT_H_ -#include "Measures.h" +#include "Shared/Measures.h" #include "Point.h" namespace DualCoding { diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/LineData.cc ./DualCoding/LineData.cc --- ../Tekkotsu_3.0/DualCoding/LineData.cc 2006-10-03 19:24:05.000000000 -0400 +++ ./DualCoding/LineData.cc 2007-11-11 18:57:20.000000000 -0500 @@ -18,6 +18,8 @@ #include "LineData.h" #include "ShapeLine.h" +using namespace std; + namespace DualCoding { DATASTUFF_CC(LineData); @@ -181,25 +183,26 @@ //! Print information about this shape. (Virtual in BaseData.) void LineData::printParams() const { cout << "Type = " << getTypeName() << " ID=" << getId() << " ParentID=" << getParentId() << endl; - cout << "end1{" << end1Pt().coordX() << ", " << end1Pt().coordY() << "}"; - cout << " active=" << end1Pt().isActive(); - cout << " valid=" << end1Pt().isValid() << endl; + cout << " end1{" << end1Pt().coordX() << ", " << end1Pt().coordY() << "}" + << " active=" << end1Pt().isActive() + << " valid=" << end1Pt().isValid() << endl; - cout << "end2{" << end2Pt().coordX() << ", " << end2Pt().coordY() << "}"; - cout << " active=" << end2Pt().isActive(); - cout << " valid=" << end2Pt().isValid() << std::endl; + cout << " end2{" << end2Pt().coordX() << ", " << end2Pt().coordY() << "}" + << " active=" << end2Pt().isActive() + << " valid=" << end2Pt().isValid() << std::endl; - cout << "rho_norm=" << rho_norm << " theta_norm=" << theta_norm; - cout << " orientation=" << getOrientation() - << " length=" << getLength() << endl; + cout << " rho_norm=" << rho_norm + << ", theta_norm=" << theta_norm + << ", orientation=" << getOrientation() + << ", length=" << getLength() << endl; - printf("color = %d %d %d\n",getColor().red,getColor().green,getColor().blue); + printf(" color = %d %d %d\n",getColor().red,getColor().green,getColor().blue); - cout << " mobile=" << isMobile() << endl; - cout << " viewable=" << isViewable() << endl; + cout << " mobile=" << getMobile() + << ", viewable=" << isViewable() << endl; vector abc = lineEquation_abc(); - printf("equ = %f %f %f\n",abc[0],abc[1],abc[2]); + printf(" equ = %f %f %f\n",abc[0],abc[1],abc[2]); } void LineData::printEnds() const { @@ -212,12 +215,12 @@ -//! Transformations. -//@{ +// *** Transformations. *** // + //! Apply a transformation to this shape. -void LineData::applyTransform(const NEWMAT::Matrix& Tmat) { - end1Pt().applyTransform(Tmat); - end2Pt().applyTransform(Tmat); +void LineData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) { + end1Pt().applyTransform(Tmat,newref); + end2Pt().applyTransform(Tmat,newref); update_derived_properties(); } @@ -228,8 +231,8 @@ update_derived_properties(); } -//! Logical endpoints -//@{ + +// *** Logical endpoints *** // EndPoint& LineData::leftPt() { return end1Pt().isLeftOf(end2Pt()) ? end1_pt : end2_pt; } EndPoint& LineData::rightPt() { return end1Pt().isLeftOf(end2Pt()) ? end2_pt : end1_pt; } @@ -312,6 +315,22 @@ else return otherline->end2Pt(); } +Shape LineData::firstPtShape() { + Shape result(new PointData(*space, firstPt())); + result->setName("firstPt"); + result->inheritFrom(*this); + result->setViewable(false); + return result; +} + +Shape LineData::secondPtShape() { + Shape result(new PointData(*space, secondPt())); + result->setName("secondPt"); + result->inheritFrom(*this); + result->setViewable(false); + return result; +} + coordinate_t LineData::firstPtCoord() const { return isNotVertical() ? const_cast(this)->firstPt().coordX() : @@ -336,9 +355,8 @@ secondPt(otherline).coordY(); } -//@} -//! Functions to set endpoints. +// *** Functions to set endpoints. *** // void LineData::setEndPts(const EndPoint& _end1_pt, const EndPoint& _end2_pt) { end1_pt.setCoords(_end1_pt); @@ -355,8 +373,7 @@ } -//! Properties functions. -//@{ +// *** Properties functions. *** // std::pair LineData::lineEquation_mb() const { float m; @@ -408,9 +425,8 @@ return(abc); } -//@} - //! Determine parameters a, b, c satisfying the equation ax + by = c. +//! Determine parameters a, b, c satisfying the equation ax + by = c. std::vector LineData::lineEquation_abc() const { float dx = end2Pt().coordX() - end1Pt().coordX(); float dy = end2Pt().coordY() - end1Pt().coordY(); @@ -446,11 +462,10 @@ return(abc); } -//@} -//! Functions to set values dealing with orientation. -//@{ +// *** Functions to set values dealing with orientation. *** // + void LineData::update_derived_properties() { rho_norm = perpendicularDistanceFrom(origin_pt); const vector abc = lineEquation_abc(); @@ -491,8 +506,8 @@ */ -//! These functions are true based on line length. -//@{ +// *** These functions are true based on line length. *** // + bool LineData::isLongerThan(const Shape& other) const { return length > other->length; } @@ -504,7 +519,6 @@ bool LineData::isShorterThan(float ref_length) const { return length < ref_length; } -//@} bool LineData::isBetween(const Point &p, const LineData &other) const { if (getOrientation() == other.getOrientation()) { // parallel lines @@ -527,8 +541,9 @@ } } -//! Check intersection. -//{ + +// *** Check intersection. *** // + bool LineData::intersectsLine(const Shape& other) const { return intersectsLine(other.getData()); @@ -682,8 +697,8 @@ } } -//! Distance. -//{ + +// *** Distance. *** // float LineData::perpendicularDistanceFrom(const Point& otherPt) const { // NOTE that this formula is rather slow, as it involves the @@ -704,17 +719,12 @@ return(d); } -//} // ================================================== // BEGIN LINE EXTRACTION CODE // ================================================== -//!@name Line Extraction. -//@{ - - #define BEG_DIST_THRESH 2 #define END_DIST_THRESH 2 @@ -731,7 +741,7 @@ ShapeSpace &ShS = SkS.getDualSpace(); // approximate largest skel region with line - NEW_SKETCH_N(labels,usint,visops::oldlabelcc(skeleton,visops::EightWayConnect)); + NEW_SKETCH_N(labels,uint,visops::oldlabelcc(skeleton,visops::EightWayConnect)); list regions = Region::extractRegions(labels,EXTRACT_LINE_MIN_AREA); if ( regions.empty() ) { ShapeRoot invalid; // need to define a named variable to avoid warning on next line @@ -765,7 +775,7 @@ // cout << " x_normpoint=" << x_normpoint << " y_normpoint=" << y_normpoint // << " m=" << m << " b=" << b <scanHorizForEndPts(skelDist,occlusions,m,b); else @@ -802,7 +812,7 @@ PointData most_distant_pt(ShS, skelchunk.mostDistantPtFrom(ln)); // cout << "Point of split: " << most_distant_pt.getCentroid() << endl; const Sketch& point_rendering = most_distant_pt.getRendering(); - usint const clear_dist = 10; + uint const clear_dist = 10; NEW_SKETCH_N(not_too_close, bool, visops::edist(point_rendering) >= clear_dist); skeleton &= not_too_close; return extractLine(skeleton, occlusions); @@ -813,7 +823,7 @@ return ShapeRootType(invalid,LineData); } -void LineData::scanHorizForEndPts(const Sketch& skelDist, +void LineData::scanHorizForEndPts(const Sketch& skelDist, const Sketch& occlusions, float m, float b) { // Scan along the infinite line, looking for segments in the image. @@ -887,7 +897,7 @@ balanceEndPointHoriz(end2_pt,occlusions,m,b); } -void LineData::scanVertForEndPts(const Sketch& skelDist, +void LineData::scanVertForEndPts(const Sketch& skelDist, const Sketch& occlusions, float m, float b) { // Scan along the infinite line, looking for segments in the image. @@ -1021,12 +1031,11 @@ //! Clear out pixels that are on or close to this line. void LineData::clearLine(Sketch& sketch) { const Sketch& line_rendering = getRendering(); - usint const clear_dist = 5; + uint const clear_dist = 5; Sketch not_too_close = (visops::edist(line_rendering) >= clear_dist); sketch &= not_too_close; } -//@} // ================================================== @@ -1356,7 +1365,7 @@ float m = (y1 != 0) ? -(x1/y1) : BIG_SLOPE; float b = y1 - m*x1; - NEW_SKETCH_N(skelDist,usint,visops::edist(skinny)); + NEW_SKETCH_N(skelDist,uint,visops::edist(skinny)); if ( abs(m) <= 1 ) // when |slope| <= 1, scan along x, else scan along y line->scanHorizForEndPts(skelDist,occlusions,m,b); else @@ -1486,9 +1495,14 @@ abs( line1->getRhoNorm() - line2->getRhoNorm() ) <= dist_tol; } -bool LineData::IsHorizontal::operator() (const Shape &line) { +bool LineData::IsHorizontal::operator() (const Shape &line) const { const AngPi orient = line->getOrientation(); return (orient <= threshold) || (orient >= M_PI - threshold); } +bool LineData::IsVertical::operator() (const Shape &line) const { + const AngPi orient = line->getOrientation(); + return (orient >= threshold) && (orient <= M_PI - threshold); + } + } // namespace diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/LineData.h ./DualCoding/LineData.h --- ../Tekkotsu_3.0/DualCoding/LineData.h 2006-09-23 23:45:20.000000000 -0400 +++ ./DualCoding/LineData.h 2007-11-11 18:57:20.000000000 -0500 @@ -4,14 +4,12 @@ #include #include -#include #include "BaseData.h" // superclass #include "EndPoint.h" // EndPoint data member -#include "Measures.h" // coordinate_t; AngPi data member #include "ShapeFuns.h" #include "ShapePoint.h" -#include "SketchTypes.h" // usint +#include "SketchTypes.h" // uint #include "Shared/mathutils.h" // deg2rad namespace DualCoding { @@ -21,6 +19,7 @@ #define EXTRACT_LINE_MIN_AREA 20 #define DEFAULT_MIN_LENGTH 4.0 +//! A line shape, with two endpoints, a length, orientation, etc. class LineData : public BaseData { private: EndPoint end1_pt; @@ -39,7 +38,7 @@ public: //! Constructor - LineData(ShapeSpace& _space, const Point &p1, const Point &p2) + LineData(ShapeSpace& _space, const EndPoint &p1, const EndPoint &p2) : BaseData(_space,getStaticType()), end1_pt(p1), end2_pt(p2), rho_norm(0), theta_norm(0), orientation(0), length(0) { update_derived_properties(); } @@ -68,10 +67,10 @@ void setInfinite(bool value=true); BoundingBox getBoundingBox() const { - return BoundingBox(min(end1_pt.coordX(),end2_pt.coordX()), - min(end1_pt.coordY(),end2_pt.coordY()), - max(end1_pt.coordX(),end2_pt.coordX()), - max(end1_pt.coordY(),end2_pt.coordY())); + return BoundingBox(std::min(end1_pt.coordX(),end2_pt.coordX()), + std::min(end1_pt.coordY(),end2_pt.coordY()), + std::max(end1_pt.coordX(),end2_pt.coordX()), + std::max(end1_pt.coordY(),end2_pt.coordY())); } //! Match lines based on their parameters. (Virtual in BaseData.) @@ -96,7 +95,7 @@ virtual void printParams() const; //! Transformations. (Virtual in BaseData.) - virtual void applyTransform(const NEWMAT::Matrix& Tmat); + virtual void applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref=unspecified); //! Project to ground virtual void projectToGround(const NEWMAT::Matrix& camToBase, @@ -104,7 +103,14 @@ virtual unsigned short getDimension() const { return 1; } - //! Point Access functions. + void printEnds() const; + void setEndPts(const EndPoint& _end1_pt, const EndPoint& _end2_pt); + + //!@name Point access functions. + /*! The first point of a line is the leftmost point if the line is horizontal, + else the topmost point. With an optional Shape argument, uses the current + line's orientation to pick the appropriate point of the other line. + */ //@{ EndPoint& end1Pt() { return end1_pt; } EndPoint& end2Pt() { return end2_pt; } @@ -121,38 +127,33 @@ Shape topPtShape(); Shape bottomPtShape(); - //! The first point of a line is the leftmost point if the line is horizontal, - //! else the topmost point. With an optional Shape argument, uses the current - //! line's orientation to pick the appropriate point of the other line. - //@{ EndPoint& firstPt(); EndPoint& firstPt(const Shape &otherline) const; EndPoint& secondPt(); EndPoint& secondPt(const Shape &otherline) const; + Shape firstPtShape(); + Shape secondPtShape(); + coordinate_t firstPtCoord() const; coordinate_t firstPtCoord(const Shape &otherline) const; coordinate_t secondPtCoord() const; coordinate_t secondPtCoord(const Shape &otherline) const; //@} - - void printEnds() const; - void setEndPts(const EndPoint& _end1_pt, const EndPoint& _end2_pt); - - //! Properties functions. + //!@name Properties functions //@{ float getRhoNorm() const { return rho_norm; } AngTwoPi getThetaNorm() const { return theta_norm; } AngPi getOrientation() const { return orientation; } float getLength() const { return length; } - pair lineEquation_mb() const; - vector lineEquation_abc() const; - vector lineEquation_abc_xz() const; + std::pair lineEquation_mb() const; + std::vector lineEquation_abc() const; + std::vector lineEquation_abc_xz() const; //@} -public: - + //!@name Orientation functions + //@{ bool isNotVertical() const; //!< True if line orientation is far enough from vertical bool isOverlappedWith(const LineData& otherline, int amount=0) const; // bool isRoughlyPerpendicularTo(Shape &other); @@ -160,7 +161,7 @@ //@} - //! Predicates based on line length. + //!@name Predicates based on line length //@{ bool isLongerThan(const Shape& other) const; bool isLongerThan(float ref_length) const; @@ -171,7 +172,7 @@ //! Check if point falls between the two lines bool isBetween(const Point &p, const LineData &other) const; - //! Check intersection. + //!@name Check line intersection //@{ bool intersectsLine(const Shape& other) const; bool intersectsLine(const LineData& other) const; @@ -193,16 +194,14 @@ bool pointOnLine(const Point& p); //! Distance. - //@{ float perpendicularDistanceFrom(const Point& other) const; - //@} // ================================================== // BEGIN SKETCH MANIPULATION AND LINE EXTRACTION CODE // ================================================== - //! Extraction. + //!@name Line extraction //@{ //! Extracts most prominent line from a skeletonized image. @@ -212,6 +211,7 @@ //! It's often useful to use the original sketch as an occluder static Shape extractLine(Sketch& skelsketch, const Sketch& occlusions); + //@} //! Helper functions used by extractLine(). //@{ @@ -221,17 +221,17 @@ //! Clears a line from a sketch. void clearLine(Sketch& sketch); - void scanHorizForEndPts(const Sketch& skelDist, const Sketch& occlusions, + void scanHorizForEndPts(const Sketch& skelDist, const Sketch& occlusions, float m, float b); - void scanVertForEndPts(const Sketch& skelDist, const Sketch& occlusions, + void scanVertForEndPts(const Sketch& skelDist, const Sketch& occlusions, float m, float b); void balanceEndPointHoriz(EndPoint &pt, Sketch const &occluders, float m, float b); void balanceEndPointVert(EndPoint &pt, Sketch const &occluders, float m, float b); - static vector > extractLines(Sketch const& sketch, int const num_lines=10); + static std::vector > extractLines(Sketch const& sketch, int const num_lines=10); - static vector > extractLines(Sketch const& skel, + static std::vector > extractLines(Sketch const& skel, Sketch const& occluders, int const num_lines=10); @@ -245,14 +245,16 @@ //@} - //! Comparison predicates used by shape functions + //!@name Comparison predicates used by shape functions //@{ + //! True if line1 shorter than line2 class LengthLessThan : public BinaryShapePred { public: bool operator() (const Shape &ln1, const Shape &ln2) const; }; + //! True if difference in line orientations is <= tolerance (default 20 deg) class ParallelTest : public BinaryShapePred { public: AngPi tolerance; @@ -261,6 +263,7 @@ }; + //! True if difference in line orientations is 90 deg +/- tolerance (default 20 deg) class PerpendicularTest : public BinaryShapePred { public: AngPi tolerance; @@ -269,6 +272,7 @@ }; + //! True if line orientations are within @a ang_tol (default 20 deg) and normpoints are within @a dist_tol (default 10 units) class ColinearTest : public BinaryShapePred { public: AngPi ang_tol; @@ -278,14 +282,23 @@ bool operator() (const Shape &line1, const Shape &ln2) const; }; + //! Predicate returns true if line orientation is within @a threshold of horizontal class IsHorizontal : public UnaryShapePred { public: IsHorizontal(AngPi thresh=M_PI/6) : UnaryShapePred(), threshold(thresh) {} - bool operator() (const Shape &line); + bool operator() (const Shape &line) const; private: AngPi threshold; }; + //! Predicate returns true if line orientation is within threshold of vertical + class IsVertical : public UnaryShapePred { + public: + IsVertical(AngPi thresh=M_PI/3) : UnaryShapePred(), threshold(thresh) {} + bool operator() (const Shape &line) const; + private: + AngPi threshold; + }; //@} @@ -295,7 +308,7 @@ private: static const int extractorGapTolerance = 8; - //! Rendering. + //!@name Rendering. //@{ //! Render into a sketch space and return reference. diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/LocalizationParticleData.cc ./DualCoding/LocalizationParticleData.cc --- ../Tekkotsu_3.0/DualCoding/LocalizationParticleData.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/LocalizationParticleData.cc 2007-11-11 18:57:20.000000000 -0500 @@ -0,0 +1,80 @@ +//-*-c++-*- + +#include + +#include "Sketch.h" +#include "LocalizationParticleData.h" +#include "ShapeLocalizationParticle.h" + +using namespace std; + +namespace DualCoding { + +LocalizationParticleData::LocalizationParticleData(ShapeSpace& _space, + const Point &_location, + const AngTwoPi _orientation, + const float _weight) : + BaseData(_space, getStaticType()), location(_location), orientation(_orientation), weight(_weight) {} + +LocalizationParticleData::LocalizationParticleData(ShapeSpace &_space, + const PFShapeLocalization::particle_type& particle) : + BaseData(_space, getStaticType()), + location(Point(particle.x,particle.y,0,_space.getRefFrameType())), + orientation(particle.theta), + weight(particle.weight) +{} + +LocalizationParticleData::LocalizationParticleData(const LocalizationParticleData &other) : + BaseData(other), location(other.location), orientation(other.orientation), weight(other.weight) {} + + +DATASTUFF_CC(LocalizationParticleData); + +bool LocalizationParticleData::updateParams(const ShapeRoot&, bool) { return true; } + +void LocalizationParticleData::applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref) { + location.applyTransform(Tmat,newref); + orientation += atan2(Tmat(1,2),Tmat(1,1)); +} + +void LocalizationParticleData::printParams() const { + cout << "Type = " << getTypeName() << " ID=" << getId() << " ParentID=" << getParentId() << endl; + cout << " location{" << location.coordX() << ", " << location.coordY() << "}" + << ", orientation = " << orientation << endl; +} + +bool LocalizationParticleData::isMatchFor(const ShapeRoot& other) const { + if (!(isSameTypeAs(other) && isSameColorAs(other))) + return false; + const Shape& other_particle = ShapeRootTypeConst(other,LocalizationParticleData); + float dist = location.distanceFrom(other_particle->getCentroid()); + return dist < 20; // *** DST hack +} + +LocalizationParticleData& LocalizationParticleData::operator=(const LocalizationParticleData &other) { + if ( this != &other ) { + BaseData::operator=(other); + location = other.location; + orientation = other.orientation; + weight = other.weight; + } + return *this; +} + +//! Render into a sketch space and return a pointer. (Private.) +Sketch* LocalizationParticleData::render() const { + SketchSpace &SkS = space->getDualSpace(); + NEWMAT::ColumnVector ctr(getCentroid().getCoords()); + SkS.applyTmat(ctr); + int const cx = int(ctr(1)); + int const cy = int(ctr(2)); + Sketch& draw_result = + *new Sketch(SkS, "render("+getName()+")"); + draw_result = false; + draw_result(cx,cy) = true; + return &draw_result; +} + + +} // namespace + diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/LocalizationParticleData.h ./DualCoding/LocalizationParticleData.h --- ../Tekkotsu_3.0/DualCoding/LocalizationParticleData.h 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/LocalizationParticleData.h 2007-03-28 05:19:18.000000000 -0400 @@ -0,0 +1,74 @@ +//-*-c++-*- +#ifndef _LOCALIZATIONPARTICLEDATA_H_ +#define _LOCALIZATIONPARTICLEDATA_H_ + +#include "Shared/Measures.h" // coordinate_t; AngPi data member +#include "Shared/newmat/newmat.h" + +#include "BaseData.h" // superclass +#include "PFShapeLocalization.h" +#include "Point.h" // Point data member +#include "ShapeTypes.h" // localizationParticleDataType + +namespace DualCoding { + +class ShapeRoot; +class SketchSpace; +template class Sketch; + +class LocalizationParticleData : public BaseData { +private: + Point location; + AngTwoPi orientation; + float weight; + +public: + + //! Constructor + LocalizationParticleData(ShapeSpace& _space, const Point &_location, const AngTwoPi _orientation, const float _weight=0); + + LocalizationParticleData(ShapeSpace &space, const PFShapeLocalization::particle_type& particle); + + //! Copy constructor + LocalizationParticleData(const LocalizationParticleData &other); + + static ShapeType_t getStaticType() { return localizationParticleDataType; } + + DATASTUFF_H(LocalizationParticleData); + + //! Centroid. (Virtual in BaseData.) + virtual Point getCentroid() const { return location; } + void setCentroid(const Point& other) { location.setCoords(other); } + + AngTwoPi getOrientation() const { return orientation; } + void setOrientation(const AngTwoPi _orientation) {orientation = _orientation; } + + float getWeight() const { return weight;} + void setWeight(float w) { weight=w; } + + virtual bool isMatchFor(const ShapeRoot& other) const; + + virtual bool updateParams(const ShapeRoot&, bool force=false); + + //! Print information about this shape. (Virtual in BaseData.) + virtual void printParams() const; + + virtual void applyTransform(const NEWMAT::Matrix& Tmat, const ReferenceFrameType_t newref=unspecified); + + virtual void projectToGround(const NEWMAT::Matrix&, + const NEWMAT::ColumnVector&) {} + + virtual unsigned short getDimension() const { return 2; } + + LocalizationParticleData& operator=(const LocalizationParticleData &other); + +private: + //! Render into a sketch space and return pointer. (Private.) + virtual Sketch* render() const; + + +}; + +} // namespace + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/Lookout.cc ./DualCoding/Lookout.cc --- ../Tekkotsu_3.0/DualCoding/Lookout.cc 2006-10-02 14:49:16.000000000 -0400 +++ ./DualCoding/Lookout.cc 2007-11-18 01:47:01.000000000 -0500 @@ -4,131 +4,494 @@ #include "Events/LocomotionEvent.h" #include "Events/LookoutEvents.h" #include "Events/VisionObjectEvent.h" -#include "Motion/HeadPointerMC.h" -#include "Motion/PostureMC.h" -#include "Motion/MMAccessor.h" -#include "Motion/MotionSequenceMC.h" -#include "Motion/MotionManager.h" #include "Shared/ProjectInterface.h" +#include "Shared/mathutils.h" #include "Shared/WorldState.h" #include "Vision/RegionGenerator.h" #include "VRmixin.h" #include "LookoutRequests.h" #include "Lookout.h" +#include "MapBuilder.h" +#include "ShapeBlob.h" +#include "ShapeLine.h" +#include "ShapePolygon.h" -namespace DualCoding { - -// some stuff for convenience -typedef SegmentedColorGenerator::color_class_state color_class_state; - -inline float getIR() { -#ifdef TGT_ERS7 - return state->sensors[NearIRDistOffset]; -#else - return state->sensors[IRDistOffset]; -#endif -} +using namespace mathutils; +using namespace std; +namespace DualCoding { Lookout::Lookout() : BehaviorBase("Lookout"), - pointer_id(MotionManager::invalid_MC_ID), - sequence_id(MotionManager::invalid_MC_ID), - requests(), curReq(NULL), landmarkInView(true), - idCounter(0) + pixelHistograms(VRmixin::camSkS.getNumPixels()), distanceSamples(), + pointer_id(MotionManager::invalid_MC_ID), + posture_id(MotionManager::invalid_MC_ID), + sequence_id(MotionManager::invalid_MC_ID), + requests(), curReq(NULL), curPAR(NULL), successSave(false), + trackerState(inactive), idCounter(0) {} void Lookout::DoStart() { BehaviorBase::DoStart(); + SharedObject head_mc; + SharedObject posture_mc; + SharedObject mseq_mc; + pointer_id = motman->addPersistentMotion(head_mc,MotionManager::kIgnoredPriority); + posture_id = motman->addPersistentMotion(posture_mc,MotionManager::kIgnoredPriority); + sequence_id = motman->addPersistentMotion(mseq_mc,MotionManager::kIgnoredPriority); + cout << "Lookout starting up: pointer_id=" << pointer_id + << " posture_id=" << posture_id + << " sequence_id=" << sequence_id << endl; + erouter->addListener(this,EventBase::motmanEGID,pointer_id,EventBase::statusETID); + erouter->addListener(this,EventBase::motmanEGID,posture_id,EventBase::statusETID); + erouter->addListener(this,EventBase::motmanEGID,sequence_id,EventBase::statusETID); } void Lookout::DoStop() { - curReq = NULL; - while (!requests.empty()) { - if (requests.front() != NULL) - delete requests.front(); - requests.pop(); - } motman->removeMotion(pointer_id); pointer_id = MotionManager::invalid_MC_ID; + motman->removeMotion(posture_id); + posture_id = MotionManager::invalid_MC_ID; motman->removeMotion(sequence_id); sequence_id = MotionManager::invalid_MC_ID; + curReq = NULL; + curPAR = NULL; + while (!requests.empty()) { + delete requests.front(); + requests.pop(); + } BehaviorBase::DoStop(); } - /* -vector Lookout::getVisionObjectData() { +vector Lookout::groundSearchPoints() { + vector gazePts; + gazePts.push_back(Point( 200, -250, -100, egocentric)); + gazePts.push_back(Point( 800,-1000, -100, egocentric)); + gazePts.push_back(Point(1200, 0, -100, egocentric)); + gazePts.push_back(Point( 800, 1000, -100, egocentric)); + gazePts.push_back(Point( 200, 250, -100, egocentric)); + gazePts.push_back(Point( 200, 0, -100, egocentric)); + gazePts.push_back(Point( 400, 0, -100, egocentric)); + gazePts.push_back(Point( 800, 0, -100, egocentric)); + return gazePts; +} +unsigned int Lookout::executeRequest(const LookoutRequest &req) { + switch (req.getHeadMotionType()) { + case LookoutRequest::noMotion: + case LookoutRequest::pointAt: + pushRequest(req); + break; + case LookoutRequest::scan: + pushRequest(req); + break; + case LookoutRequest::track: + pushRequest(req); + break; + case LookoutRequest::search: + pushRequest(req); + break; + default: + cout << "Lookout::executeRequest: unknown request type " << req.getHeadMotionType() << endl; + }; + const unsigned int reqid = requests.back()->requestID = ++idCounter; + executeRequest(); + return reqid; } -vector Lookout::getIRData() { +void Lookout::executeRequest() { + if ( curReq != NULL || requests.empty() ) + return; + curReq = requests.front(); + cout << "Lookout executing " << LookoutRequest::headMotionTypeNames[curReq->getHeadMotionType()] + << " request, id=" << curReq->requestID << endl; + curPAR = dynamic_cast(curReq); + + // If we're going to be computing pixel or distance modes, clear the bins first + if ( curPAR != NULL && curPAR->numSamples > 1 ) + switch ( curPAR->getResultType() ) { + case LookoutRequest::imageResult: + for ( unsigned int i=0; i 0) distanceSamples.pop(); + break; +#endif + default: + break; + } + // now dispatch on the head motion type + trackerState = inactive; + switch (curReq->getHeadMotionType()) { + case LookoutRequest::noMotion: + erouter->addTimer(this, settle_timer, curPAR->motionSettleTime, false); + break; + case LookoutRequest::pointAt: + moveHeadToPoint(); + break; + case LookoutRequest::scan: + setupScan(); + break; + case LookoutRequest::track: + setupTrack(); + break; + case LookoutRequest::search: + setupSearch(); + break; + default: + cout << "Lookout::executeRequest(): unknown request " << curReq->getHeadMotionType() << endl; + break; + }; } - */ -void Lookout::processScan(const EventBase& event) { +void Lookout::relax() { + motman->setPriority(pointer_id,MotionManager::kIgnoredPriority); + motman->setPriority(posture_id,MotionManager::kIgnoredPriority); + motman->setPriority(sequence_id,MotionManager::kIgnoredPriority); +} + +void Lookout::moveHeadToPoint() { + Point pt = curPAR->gazePt; + switch ( pt.getRefFrameType() ) { + case unspecified: + case camcentric: + cout << "Warning: Lookout gaze point " << curPAR->gazePt << " reference frame must be egocentric or allocentric" << endl; + pt.setRefFrameType(egocentric); + case egocentric: + break; + case allocentric: + pt.applyTransform(VRmixin::mapBuilder.worldToLocalTranslateMatrix,egocentric); + pt.applyTransform(VRmixin::mapBuilder.worldToLocalRotateMatrix,egocentric); + } + // point head based on Camera or IR reference frame + switch ( curPAR->getResultType() ) { + case LookoutRequest::noResult: + case LookoutRequest::imageResult: { + successSave = MMAccessor(pointer_id)->lookAtPoint(pt.coordX(),pt.coordY(),pt.coordZ()); + motman->setPriority(pointer_id,MotionManager::kStdPriority); + motman->setPriority(posture_id,MotionManager::kIgnoredPriority); + motman->setPriority(sequence_id,MotionManager::kIgnoredPriority); + } + break; +#ifdef TGT_HAS_IR_DISTANCE + case LookoutRequest::distanceResult: { +#ifdef TGT_ERS7 + successSave = MMAccessor(posture_id)->solveLinkVector(pt.coords,NearIRFrameOffset,Kinematics::pack(0,0,1)); +#else + successSave = MMAccessor(posture_id)->solveLinkVector(pt.coords,IRFrameOffset,Kinematics::pack(0,0,1)); +#endif + motman->setPriority(posture_id,MotionManager::kStdPriority); + motman->setPriority(pointer_id,MotionManager::kIgnoredPriority); + motman->setPriority(sequence_id,MotionManager::kIgnoredPriority); + } + break; +#endif + case LookoutRequest::interestPoints: + cout << "Lookout error: this scan type cannot return interest points." << endl; + return; + } +} + +void Lookout::processEvent(const EventBase& event) { + if ( curReq == NULL ) { + if ( event.getGeneratorID() == EventBase::motmanEGID && + event.getTypeID() == EventBase::statusETID ) + return; // harmless motman status event from startup + else { + cout << "Error: Lookout received an event when not executing a request:\n"; + cout << " " << event.getDescription(true,3) << endl; + return; + } + } + + switch (curReq->getHeadMotionType()) { + case LookoutRequest::noMotion: + case LookoutRequest::pointAt: + processPointAtEvent(event); + break; + case LookoutRequest::scan: + processScanEvent(event); + break; + case LookoutRequest::track: + processTrackEvent(event); + break; + case LookoutRequest::search: + processSearchEvent(event); + break; + default: + cout << "Lookout::processEvent: unknown head motion request type: " + << curReq->getHeadMotionType() << ", event: " << event.getDescription() << endl; + break; + }; +} + +void Lookout::processPointAtEvent(const EventBase& event) { + switch (event.getGeneratorID()) { + case EventBase::motmanEGID: + if ( event.getSourceID() == pointer_id || event.getSourceID() == posture_id ) { + // head motion complete, now wait for head to settle + motman->setPriority(event.getSourceID(), MotionManager::kBackgroundPriority); + erouter->addTimer(this, settle_timer, curPAR->motionSettleTime, false); + } + break; + + case EventBase::timerEGID: + if (event.getSourceID() == settle_timer || event.getSourceID() == sample_timer) { + switch (curReq->getResultType()) { + case LookoutRequest::imageResult: + erouter->addListener(this, EventBase::visRegionEGID, + ProjectInterface::visRegionSID,EventBase::statusETID); + break; +#ifdef TGT_HAS_IR_DISTANCE + case LookoutRequest::distanceResult: + erouter->addListener(this, EventBase::sensorEGID, SensorSrcID::UpdatedSID); + break; +#endif + case LookoutRequest::noResult: + default: + requestComplete(successSave); + break; + }; + } + break; + + case EventBase::visRegionEGID: + erouter->removeListener(this, EventBase::visRegionEGID); + VRmixin::camSkS.clear(); + VRmixin::camShS.clear(); + curPAR->image.bind((curPAR->sketchFunc)()); + ++curPAR->sampleCounter; + if ( curPAR->numSamples == 1 || findPixelModes() == true ) { + curPAR->toBaseMatrix = kine->jointToBase(curPAR->joint); + requestComplete(successSave); + } + else + erouter->addTimer(this, sample_timer, curPAR->sampleInterval, false); + break; + + case EventBase::sensorEGID: + erouter->removeListener(this, EventBase::sensorEGID); +#ifdef TGT_HAS_IR_DISTANCE + if ( findDistanceMode() == true ) { + curPAR->toBaseMatrix = kine->jointToBase(curPAR->joint); + requestComplete(successSave); + } else +#endif + erouter->addTimer(this, sample_timer, curPAR->sampleInterval, false); + break; + + default: + cout << "Lookout::processPointAtEvent: unknown event " << event.getDescription() << endl; + break; + }; +} + +bool Lookout::findPixelModes() { + // update our counts using the new sammple + for (size_t i = 0; iimage[i]]++; + if ( curPAR->sampleCounter < curPAR->numSamples ) + return false; + // we have enough samples; compute the mode + for (size_t i = 0; i::const_iterator it = pixelHistograms[i].begin(); + it != pixelHistograms[i].end(); it++) + if (it->second > maxCount) { + maxCount = it->second; + maxChar = it->first; + } + curPAR->image[i] = maxChar; + } + return true; +} + +#ifdef TGT_HAS_IR_DISTANCE +float Lookout::getDistanceModeValue() { + int const npops = distanceSamples.size() / 2; + for ( int i=0; isensors[FarIRDistOffset] > 400 ) // far IR goes 200-1500; near IR goes 50-500 + return state->sensors[FarIRDistOffset]; + else + return state->sensors[NearIRDistOffset]; +#else + return state->sensors[IRDistOffset]; +#endif +} + +bool Lookout::findDistanceMode() { + distanceSamples.push(getIR()); + return (int)distanceSamples.size() < curPAR->numSamples; +} +#endif // TGT_HAS_IR_DISTANCE + +void Lookout::requestComplete(bool success) { + cout << "Lookout request " << curReq->requestID << " complete." << endl; + erouter->removeTimer(this); + erouter->removeListener(this,EventBase::sensorEGID); + erouter->removeListener(this,EventBase::visSegmentEGID); + erouter->removeListener(this,EventBase::visRegionEGID); + erouter->removeListener(this,EventBase::visObjEGID); + LookoutRequest *saveReq = curReq; + curReq = NULL; + curPAR = NULL; + requests.pop(); + switch ( saveReq->getHeadMotionType() ) { + case LookoutRequest::noMotion: + case LookoutRequest::pointAt: + switch ( saveReq->getResultType() ) { + case LookoutRequest::noResult: + erouter->postEvent(LookoutPointAtEvent(success,static_cast(saveReq)->toBaseMatrix, + EventBase::lookoutEGID, saveReq->requestID, EventBase::deactivateETID)); + break; + case LookoutRequest::imageResult: + erouter->postEvent(LookoutSketchEvent(success,static_cast(saveReq)->image, + static_cast(saveReq)->toBaseMatrix, + EventBase::lookoutEGID, saveReq->requestID, EventBase::deactivateETID)); + break; +#ifdef TGT_HAS_IR_DISTANCE + case LookoutRequest::distanceResult: + erouter->postEvent(LookoutIREvent(success,getDistanceModeValue(), static_cast(saveReq)->toBaseMatrix, + EventBase::lookoutEGID, saveReq->requestID, EventBase::deactivateETID)); + break; +#endif + default: + cout << "Lookout::requestComplete(): Unknown type returned by getResultType()\n"; + } + break; + + case LookoutRequest::scan: + erouter->postEvent(LookoutScanEvent(static_cast(saveReq)->tasks, + EventBase::lookoutEGID,saveReq->requestID, EventBase::deactivateETID)); + break; + + case LookoutRequest::track: + erouter->postEvent(EventBase(EventBase::lookoutEGID,saveReq->requestID, EventBase::deactivateETID)); + break; + + case LookoutRequest::search: { + Sketch image(VRmixin::sketchFromSeg()); +#ifdef TGT_HAS_CAMERA + const NEWMAT::Matrix camToBase = kine->jointToBase(CameraFrameOffset); +#else + NEWMAT::Matrix camToBase(4,4); + camToBase << ROBOOP::fourbyfourident; +#endif + erouter->postEvent(LookoutSketchEvent(success, image, camToBase, + EventBase::lookoutEGID, saveReq->requestID, EventBase::deactivateETID)); + break; + } + + default: + cout << "Lookout::requestComplete(): Unknown head motion type\n"; + } + + // The postEvent above may have led to a series of functions + // returning and eventually shutting down the Lookout or starting + // another request, so make sure we're still in the same state + // before proceeding. + delete saveReq; + if ( curReq == NULL && !requests.empty() ) + executeRequest(); +} + +typedef SegmentedColorGenerator::color_class_state color_class_state; + +void Lookout::storeVisionRegionDataTo(vector& data, const set& colors, int minArea) { + const unsigned char *img = + ProjectInterface::defRegionGenerator->getImage(ProjectInterface::fullLayer,0); + const color_class_state *regions = reinterpret_cast (img); + for (set::const_iterator it = colors.begin(); + it != colors.end(); it++) + for (int i = 0; i < regions[*it].num; i++) + if ((regions[*it].list+i)->area > minArea) { + data.push_back(findLocationFor(regions[*it].list)); + cout << regions[*it].name << " at " << data.back() << endl; + } + else break; +} + +#ifdef TGT_HAS_IR_DISTANCE +void Lookout::storeIRDataTo(vector& data) { + NEWMAT::ColumnVector ray = Kinematics::pack(0,0,getIR()); + cout << "dist= " << ray(3) << ", in base frame= "; +#ifdef TGT_ERS7 + NEWMAT::ColumnVector baseCoords = kine->jointToBase(NearIRFrameOffset)*ray; +#else //not ERS7 + NEWMAT::ColumnVector baseCoords = kine->jointToBase(IRFrameOffset)*ray; +#endif + data.push_back(Point(baseCoords(1),baseCoords(2),baseCoords(3))); + cout << data.back() << endl; +} +#endif + + +//================ Scan Request ================ + +void Lookout::processScanEvent(const EventBase& event) { // cout << "Lookout::processScan: " << event.getName() << endl; static bool listeningObjEGID = false; - static ScanRequest* curScanReq = dynamic_cast(curReq); - if (curScanReq != curReq) curScanReq = dynamic_cast(curReq); + const LookoutScanRequest* curScanReq = dynamic_cast(curReq); + switch (event.getGeneratorID()) { case EventBase::motmanEGID: - if (event.getTypeID() != EventBase::deactivateETID) return; // head arrived at the start of motion sequence, add listeners and start scan sequence if (event.getSourceID() == pointer_id) { - pointer_id = MotionManager::invalid_MC_ID; - motman->setPriority(sequence_id,MotionManager::kStdPriority); - MMAccessor(sequence_id)->play(); - for (unsigned int i = 0; i < curScanReq->tasks.size(); i++) { - const ScanRequest::Task* task = curScanReq->tasks[i]; - if (task->getTaskType() == ScanRequest::Task::visObj) { - const ScanRequest::VisionTask& vTask = *dynamic_cast(task); - for(set::const_iterator color_it = vTask.index.begin(); - color_it != vTask.index.end(); color_it++) - erouter->addListener(this,EventBase::visObjEGID, *color_it); - } - erouter->addTimer(this,scan_timer+i,0,false); // for initial measurements - erouter->addTimer(this,scan_timer+i, - (int) ((float) task->interval/(float)curScanReq->scanSpeed),true); - } + erouter->addTimer(this,settle_timer,curScanReq->motionSettleTime,false); } else if (event.getSourceID() == sequence_id) { - sequence_id = MotionManager::invalid_MC_ID; + motman->setPriority(sequence_id,MotionManager::kBackgroundPriority); requestComplete(); } break; + case EventBase::timerEGID: // time to take some measurements - if (event.getSourceID()-scan_timer < curScanReq->tasks.size()) { - ScanRequest::Task* task = curScanReq->tasks[event.getSourceID()-scan_timer]; - if (task->getTaskType() == ScanRequest::Task::visReg) { - ScanRequest::VisionRegionTask* vrt = dynamic_cast(task); + if ( event.getSourceID() == settle_timer ) + triggerScanMotionSequence(); + else if ( event.getSourceID() >= scan_timer && event.getSourceID()-scan_timer < curScanReq->tasks.size() ) { + LookoutRequest::Task* task = curScanReq->tasks[event.getSourceID()-scan_timer]; + if (task->getTaskType() == LookoutRequest::Task::visRegTask) { + LookoutRequest::VisionRegionTask* vrt = dynamic_cast(task); storeVisionRegionDataTo(vrt->data,vrt->index,vrt->minArea); } - else if (task->getTaskType() == ScanRequest::Task::visObj) { + else if (task->getTaskType() == LookoutRequest::Task::visObjTask) { erouter->addListener(this, EventBase::visSegmentEGID, ProjectInterface::visSegmentSID,EventBase::statusETID); listeningObjEGID = true; } - else if (task->getTaskType() == ScanRequest::Task::ir) +#ifdef TGT_HAS_IR_DISTANCE + else if (task->getTaskType() == LookoutRequest::Task::irTask) storeIRDataTo(task->data); +#endif } break; + case EventBase::visSegmentEGID: if (listeningObjEGID) listeningObjEGID = false; else erouter->removeListener(this, EventBase::visSegmentEGID); break; + case EventBase::visObjEGID: if (listeningObjEGID) { - const VisionObjectEvent voe = *static_cast(&event); - for (vector::iterator it = curScanReq->tasks.begin(); + const VisionObjectEvent &voe = static_cast(event); + for (vector::const_iterator it = curScanReq->tasks.begin(); it != curScanReq->tasks.end(); it++) - if ((*it)->getTaskType() == ScanRequest::Task::visObj) { - ScanRequest::VisionTask& vTask = *dynamic_cast(*it); + if ((*it)->getTaskType() == LookoutRequest::Task::visObjTask) { + LookoutRequest::VisionTask& vTask = *dynamic_cast(*it); if (vTask.index.find(event.getSourceID()) != vTask.index.end()) { - vTask.data.push_back(findLocationFor(&voe)); + vTask.data.push_back(findLocationFor(voe)); cout << "VisionObject at " << vTask.data.back() << endl; break; } @@ -141,109 +504,235 @@ }; } -void Lookout::storeVisionRegionDataTo(vector& data, const set& colors, int minArea) { - const unsigned char *img = - ProjectInterface::defRegionGenerator->getImage(ProjectInterface::fullLayer,0); - const color_class_state *regions = reinterpret_cast (img); - for (set::const_iterator it = colors.begin(); - it != colors.end(); it++) - for (int i = 0; i < regions[*it].num; i++) - if ((regions[*it].list+i)->area > minArea) { - data.push_back(findLocationFor(regions[*it].list)); - cout << regions[*it].name << " at " << data.back() << endl; - } - else break; +void Lookout::setupScan() { + const LookoutScanRequest *curScan = dynamic_cast(curReq); + cout << "scan speed: " << curScan->scanSpeed + << " (rad / millisec)\n"; + if ( !curScan->searchArea.isValid() ) { + cout << "Invalid search area in LookoutScanRequest" << endl; + return; + } + switch ( curScan->searchArea->getType() ) { + case lineDataType: { + const Shape &line = ShapeRootTypeConst(curScan->searchArea,LineData); + scanAlongLine(line->firstPt(), line->secondPt()); + break; + } + case polygonDataType:{ + const Shape &poly = ShapeRootTypeConst(curScan->searchArea,PolygonData); + scanAlongPolygon(poly->getVertices()); + break; + } + default: + cout << "Invalid shape type for LookoutRequest searchArea: must be line or polygon" << endl; + } } + +void Lookout::triggerScanMotionSequence() { + const LookoutScanRequest* curScanReq = dynamic_cast(curReq); + for (unsigned int i = 0; i < curScanReq->tasks.size(); i++) { + const LookoutRequest::Task* task = curScanReq->tasks[i]; + if (task->getTaskType() == LookoutRequest::Task::visObjTask) { + const LookoutRequest::VisionTask& vTask = *dynamic_cast(task); + for(set::const_iterator color_it = vTask.index.begin(); + color_it != vTask.index.end(); color_it++) + erouter->addListener(this,EventBase::visObjEGID, *color_it); + } + erouter->addTimer(this,scan_timer+i,0,false); // for initial measurements + int snap_interval = (int)((float)task->dTheta / (float)curScanReq->scanSpeed); + cout << "Lookout scan snap_interval = " << snap_interval << endl; + erouter->addTimer(this, scan_timer+i, snap_interval, true); + } + motman->setPriority(pointer_id,MotionManager::kIgnoredPriority); + motman->setPriority(posture_id,MotionManager::kIgnoredPriority); + motman->setPriority(sequence_id,MotionManager::kStdPriority); + MMAccessor(sequence_id)->play(); +} + +void Lookout::scanAlongLine(const Point& startPt, const Point& endPt) { + motman->setPriority(pointer_id,MotionManager::kIgnoredPriority); // just to be safe + MMAccessor pointer_acc(pointer_id); + + // first compute joint angles for final point + pointer_acc->lookAtPoint(endPt.coordX(),endPt.coordY(),endPt.coordZ()); + std::vector anglesA(NumHeadJoints); + for(unsigned int i=0; igetJointValue(i); -void Lookout::storeIRDataTo(vector& data) { - NEWMAT::ColumnVector ray = Kinematics::pack(0,0,getIR()); - cout << "dist= " << ray(3) << ", in base frame= "; -#ifdef TGT_ERS7 - NEWMAT::ColumnVector baseCoords = kine->jointToBase(NearIRFrameOffset)*ray; -#else //not ERS7 - NEWMAT::ColumnVector baseCoords = kine->jointToBase(IRFrameOffset)*ray; + // now compute angles for initial point, so when the motion command is executed we'll start there + pointer_acc->lookAtPoint(startPt.coordX(),startPt.coordY(),startPt.coordZ()); + std::vector anglesB(NumHeadJoints); + for(unsigned int i=0; igetJointValue(i); + + float total_joint_distance=0; + for(unsigned int i=0; i(curReq)->scanSpeed); + + // begin at start point (the HeadPointerMC will have brought us there) and move smoothly to end point + motman->setPriority(sequence_id,MotionManager::kIgnoredPriority); // just to be safe + MMAccessor mseq_acc(sequence_id); + mseq_acc->clear(); + mseq_acc->pause(); +#ifdef TGT_HAS_HEAD + for(unsigned int i=0; isetOutputCmd(HeadOffset+i,anglesA[i]); #endif - data.push_back(Point(baseCoords(1),baseCoords(2),baseCoords(3))); - cout << data.back() << endl; + mseq_acc->advanceTime(movement_time); +#ifdef TGT_HAS_HEAD + for(unsigned int i=0; isetOutputCmd(HeadOffset+i,anglesB[i]); +#endif + motman->setPriority(posture_id,MotionManager::kIgnoredPriority); // just to be safe + motman->setPriority(pointer_id,MotionManager::kStdPriority); // start head moving to where scan begins; completion will trigger the scan } -bool Lookout::findPixelModes(StoreModeImageRequest& modeRequest) { - static size_t npixels = modeRequest.image->getNumPixels(); - static vector > colorCount(npixels); - if (modeRequest.numImages-- == 0) { // finished collecting samples, find modes - for (size_t i = 0; i::const_iterator it = colorCount[i].begin(); - it != colorCount[i].end(); it++) - if (it->second > maxCount) { - maxCount = it->second; - maxChar = it->first; +void Lookout::scanAlongPolygon(vector const &vertices, const bool closed) { + if ( vertices.size() == 0 ) + requestComplete(); + vector vertices_copy(vertices); + const Point startPt = vertices_copy[0]; + if ( closed ) + vertices_copy.push_back(startPt); + motman->setPriority(pointer_id,MotionManager::kBackgroundPriority); // just to be safe + motman->setPriority(sequence_id,MotionManager::kBackgroundPriority); // just to be safe + MMAccessor pointer_acc(pointer_id); + MMAccessor mseq_acc(sequence_id); + mseq_acc->pause(); + mseq_acc->clear(); + float const speed = dynamic_cast(curReq)->scanSpeed; + std::vector anglesA(NumHeadJoints,0), anglesB(NumHeadJoints,0); + for ( vector::const_iterator it = vertices_copy.begin(); it != vertices_copy.end(); ++it ) { + pointer_acc->lookAtPoint((*it).coordX(),(*it).coordY(),(*it).coordZ()); + for(unsigned int i=0; igetJointValue(i); + if ( it != vertices_copy.begin() ) { + float total_joint_distance=0; + for(unsigned int i=0; iadvanceTime(movement_time); + } +#ifdef TGT_HAS_HEAD + for(unsigned int i=0; isetOutputCmd(HeadOffset+i,anglesB[i]); + mseq_acc->advanceTime(200); + for(unsigned int i=0; isetOutputCmd(HeadOffset+i,anglesB[i]); +#endif + anglesA.swap(anglesB); } - modeRequest.image[i] = maxChar; - colorCount[i].clear(); - } - return true; - } - else { - for (size_t i = 0; ilookAtPoint(startPt.coordX(),startPt.coordY(),startPt.coordZ()); + motman->setPriority(pointer_id,MotionManager::kStdPriority); // start head moving to where scan begins; completion will trigger the scan +} + +//================ Track Requests ================ + +void Lookout::setupTrack() { + // point the head at the object, then start tracking it and posting status events + LookoutTrackRequest *curTR = static_cast(curReq); + const ShapeRoot &target = curTR->targetShape; + curTR->cindex = ProjectInterface::getColorIndex(target->getColor()); + Point egoLoc = target->getCentroid(); + if ( target->getRefFrameType() == camcentric ) { +#ifndef TGT_HAS_CAMERA + std::cerr << "Lookout::setupTrack target has camcentric reference frame, but target model doesn't have a camera" << std::endl; +#else + const NEWMAT::Matrix camToBase = kine->jointToBase(CameraFrameOffset); + egoLoc.projectToGround(camToBase,kine->calculateGroundPlane()); // assume head hasn't moved, or we're screwed +#endif } + trackerState = moveToAcquire; + erouter->addListener(this, EventBase::visRegionEGID, ProjectInterface::visRegionSID, EventBase::deactivateETID); + motman->setPriority(posture_id,MotionManager::kIgnoredPriority); + motman->setPriority(sequence_id,MotionManager::kIgnoredPriority); + motman->setPriority(pointer_id,MotionManager::kStdPriority); + successSave = MMAccessor(pointer_id)->lookAtPoint(egoLoc.coordX(), egoLoc.coordY(), egoLoc.coordZ()); } - -void Lookout::processPointAt(const EventBase& event) { - // cout << "Lookout::processPointAt: " << event.getName() << endl; - switch (event.getGeneratorID()) { - case EventBase::motmanEGID: // head motion complete, wait for duration - if (event.getSourceID() == pointer_id && event.getTypeID() == EventBase::deactivateETID) { - pointer_id = MotionManager::invalid_MC_ID; - erouter->addTimer(this, dur_timer, dynamic_cast(curReq)->duration, false); + +void Lookout::processTrackEvent(const EventBase &event) { + const LookoutTrackRequest *curTR = static_cast(curReq); + switch ( event.getGeneratorID() ) { + + case EventBase::visRegionEGID: { + if ( trackerState == moveToAcquire ) return; // ignore vision events while slewing head to initial position + const color_class_state *ccs = reinterpret_cast + (ProjectInterface::defRegionGenerator->getImage(ProjectInterface::fullLayer,0)); + const color_class_state &col = ccs[curTR->cindex]; + if ( col.list == NULL || col.list->area <= curTR->minBlobArea ) { + if ( trackerState != lost ) { + trackerState = lost; + erouter->addTimer(this,lost_timer,2000,false); // wait for object to reappear + } + return; } + // test succeeded + erouter->removeTimer(this,lost_timer); + trackerState = tracking; + Point target = findLocationFor(col.list); + MMAccessor(pointer_id)->lookAtPoint(target.coordX(),target.coordY(),target.coordZ()); + } break; - case EventBase::timerEGID: - if (event.getSourceID() == dur_timer) { - PointAtBase* baseReq = dynamic_cast(curReq); - switch (baseReq->getPointAtType()) { - case PointAtBase::storeImage: - erouter->addListener(this, EventBase::visSegmentEGID, - ProjectInterface::visSegmentSID,EventBase::statusETID); - break; - case PointAtBase::measureDist: - erouter->addListener(this, EventBase::sensorEGID, SensorSourceID::UpdatedSID); - break; - default: - requestComplete(); - break; - }; + + + case EventBase::motmanEGID: + switch ( trackerState ) { + case inactive: + break; + case moveToAcquire: + trackerState = tracking; + break; + case tracking: + case searching: + case centering: + case lost: break; } - case EventBase::visSegmentEGID: { - erouter->removeListener(this, EventBase::visSegmentEGID); - PointAtBase* baseReq = dynamic_cast(curReq); - baseReq->toBaseMatrix = kine->jointToBase(baseReq->joint); - VRmixin::camSkS.clear(); - StoreImageRequest& storeImageReq = *dynamic_cast(curReq); - storeImageReq.image = (*storeImageReq.sketchFunc)(); - if (StoreModeImageRequest* modeReq = dynamic_cast(curReq)) - if ( ! findPixelModes(*modeReq) ) { - erouter->addTimer(this, dur_timer, modeReq->interval, false); - return; - } - requestComplete(); - } break; - case EventBase::sensorEGID: { - MeasureDistanceRequest* measureDistReq = dynamic_cast(curReq); - measureDistReq->toBaseMatrix = kine->jointToBase(measureDistReq->joint); - measureDistReq->val = getIR(); - requestComplete(); - } + + case EventBase::timerEGID: + if ( event.getSourceID() == lost_timer ) // object out of sight for too long: give up? Or search? + stopTrack(); break; + default: - cout << "Lookout::processPointAt: unknown event " << event.getName() << endl; break; - }; + } +} + +void Lookout::stopTrack() { + if ( curReq != NULL && curReq->getHeadMotionType() == LookoutRequest::track ) + requestComplete(false); +} + +Point Lookout::findLocationFor(const float normX, const float normY) { + // NEWMAT::ColumnVector ground_plane = kine->calculateGroundPlane(); + NEWMAT::ColumnVector cameraPt(4); + config->vision.computeRay(normX, normY, cameraPt(1),cameraPt(2),cameraPt(3)); +#ifdef TGT_HAS_IR_DISTANCE + cameraPt *= getIR(); +#else + cameraPt *= 400; +#endif + cameraPt(4) = 1; + // groundPt = kine->projectToPlane(CameraFrameOffset, cameraPt, + // BaseFrameOffset, ground_plane, BaseFrameOffset); +#ifndef TGT_HAS_CAMERA + const NEWMAT::ColumnVector groundPt = cameraPt; +#else + const NEWMAT::Matrix camToBase(kine->jointToBase(CameraFrameOffset)); + const NEWMAT::ColumnVector groundPt = camToBase * cameraPt; +#endif + return Point(groundPt(1),groundPt(2),groundPt(3),egocentric); } + /* void Lookout::processTrackVision(float normX, float normY, bool isCurrentlyVisible) { if (!isCurrentlyVisible && landmarkInView) { // landmark just lost @@ -264,7 +753,7 @@ erouter->postEvent(EventBase::lookoutEGID, curReq->getRequestID(), EventBase::statusETID,0); } -void Lookout::processTrack(const EventBase& event) { +void Lookout::processTrackEvent(const EventBase& event) { switch (event.getGeneratorID()) { case EventBase::visObjEGID: if (event.getTypeID()==EventBase::statusETID) { @@ -309,161 +798,8 @@ }; } */ -unsigned int Lookout::executeRequest(const LookoutRequest& req) { - switch (req.getHeadMotionType()) { - case LookoutRequest::none: - switch (dynamic_cast(&req)->getPointAtType()) { - case PointAtBase::storeImage: - if (dynamic_cast(&req)) - pushRequest(req); - else - pushRequest(req); - break; - case PointAtBase::measureDist: - pushRequest(req); break; - default: - cout << "Lookout::executeRequest: unknown PointAtRequest type\n"; - break; - }; - break; - case LookoutRequest::pointAt: - switch (dynamic_cast(&req)->getPointAtType()) { - case PointAtBase::none: - pushRequest(req); break; - case PointAtBase::storeImage: - if (dynamic_cast(&req)) - pushRequest(req); - else - pushRequest(req); - break; - case PointAtBase::measureDist: - pushRequest(req); break; - default: cout << "Lookout::executeRequest: unknown PointAtRequest type\n"; break; - }; - break; - case LookoutRequest::scan: - if (dynamic_cast(&req)->getScanType() == ScanRequest::line) - pushRequest(req); - else - pushRequest(req); - break; - case LookoutRequest::track: - pushRequest(req); break; - default: - cout << "Lookout::executeRequest: unknown request type " << req.getHeadMotionType() << endl; - }; - requests.back()->requestID = idCounter++; - // cout << " request " << requests.back()->requestID << " added to request queue\n"; - if (requests.size() == 1) - executeRequest(); - return requests.back()->requestID; -} -// triggers action to start completing each type of front of request queue -void Lookout::executeRequest() { - curReq = requests.front(); - // cout << "Executing Lookout Request #" << curReq->requestID << endl; - switch (curReq->getHeadMotionType()) { - case LookoutRequest::none: - erouter->addTimer(this,dur_timer,dynamic_cast(curReq)->duration,false); - break; - case LookoutRequest::pointAt: { - const Point& pt = dynamic_cast(curReq)->gazePt; - if (dynamic_cast(curReq)->getPointAtType() == PointAtBase::measureDist) { - SharedObject pstmc; -#ifdef TGT_ERS7 - pstmc->solveLinkVector(pt.coords,NearIRFrameOffset,Kinematics::pack(0,0,1)); -#else - pstmc->solveLinkVector(pt.coords,IRFrameOffset,Kinematics::pack(0,0,1)); -#endif - pointer_id = motman->addPrunableMotion(pstmc); - } - else { - SharedObject hpmc; - hpmc->lookAtPoint(pt.coordX(),pt.coordY(),pt.coordZ()); - pointer_id = motman->addPrunableMotion(hpmc); - } - erouter->addListener(this,EventBase::motmanEGID,pointer_id); - } - break; - case LookoutRequest::scan: - erouter->addListener(this,EventBase::motmanEGID); - scan(); - break; - default: - cout << "Lookout::executeRequest(): unknown request " << curReq->getHeadMotionType() << endl; - break; - }; -} - -void Lookout::requestComplete() { - // remove all timer and listeners for now - erouter->removeListener(this); - erouter->removeTimer(this); - if (curReq->getHeadMotionType() == LookoutRequest::scan) { - erouter->postEvent(LookoutScanEvent - (dynamic_cast(curReq)->tasks, - EventBase::lookoutEGID,curReq->requestID, EventBase::deactivateETID)); - } - else if (curReq->getHeadMotionType() == LookoutRequest::pointAt - || curReq->getHeadMotionType() == LookoutRequest::none) { - const NEWMAT::Matrix& toBase = dynamic_cast(curReq)->toBaseMatrix; - switch (dynamic_cast(curReq)->getPointAtType()) { - case PointAtRequest::none: - erouter->postEvent(LookoutPointAtEvent(toBase,EventBase::lookoutEGID, - curReq->requestID, EventBase::deactivateETID)); - break; - case PointAtRequest::storeImage: - erouter->postEvent(LookoutSketchEvent - (dynamic_cast(curReq)->image, - toBase,EventBase::lookoutEGID, - curReq->requestID, EventBase::deactivateETID)); - break; - case PointAtRequest::measureDist: - erouter->postEvent(LookoutIREvent - (dynamic_cast(curReq)->val, - toBase,EventBase::lookoutEGID, - curReq->requestID, EventBase::deactivateETID)); - break; - default: - cout << "Lookout::requestComplete(): Unknown type returned by getPointAtType()\n"; - }; - } - requests.pop(); - // delete curReq; - curReq = NULL; - if (!requests.empty()) - executeRequest(); -} - - -void Lookout::processEvent(const EventBase& event) { - cout << "Lookout::processEvent got " << event.getDescription() << endl; - if (NULL==curReq) { - cout << "Should not get any event when executing no request\n"; - return; - } - switch (curReq->getHeadMotionType()) { - case LookoutRequest::scan: - processScan(event); - break; - /* - case LookoutRequest::track: - processTrack(event); - break; - case LookoutRequest::localize: - // ?????? - break; - */ - case LookoutRequest::pointAt: - case LookoutRequest::none: - processPointAt(event); break; - default: - cout << "Lookout::processEvent: unknown request type: " << event.getName() << endl; - break; - }; -} /* void Lookout::trackObjectAt(float horiz, float vert) { setPanPrior(false); @@ -495,115 +831,127 @@ } */ -Point Lookout::findLocationFor(float normX, float normY) { - NEWMAT::ColumnVector ground_plane = kine->calculateGroundPlane(); - NEWMAT::ColumnVector cameraPt(4), groundPt(4); - config->vision.computeRay(normX, normY, cameraPt(1),cameraPt(2),cameraPt(3)); - cameraPt(4) = 1; - groundPt = kine->projectToPlane(CameraFrameOffset, cameraPt, - BaseFrameOffset, ground_plane, BaseFrameOffset); - return Point(groundPt(1),groundPt(2),groundPt(3),egocentric); -} +// ================ Search Requests ================ -void Lookout::scan() { - cout << "scan speed: " << dynamic_cast(curReq)->scanSpeed - << " (rad / millisec)\n"; - if (dynamic_cast(curReq)->getScanType()==ScanRequest::line) { - const ScanAlongLineRequest& scanLineReq = *dynamic_cast(curReq); - scanAlongLine(scanLineReq.beginPt, scanLineReq.endPt); - } - else { - const ScanAreaRequest& scanLineReq = *dynamic_cast(curReq); - scanArea(scanLineReq.left, scanLineReq.right, scanLineReq.far, scanLineReq.near); +/* Move head to the target shape. If we encounter the desired color +region along the way, stop moving. Otherwise, upon reaching the +target, begin a spiral search sequence. If the sequence completes +without finding the color region, return failure. Else find the +center of the color region, move the head there, wait for the camera +to settle, take a picture, and return success. */ + +void Lookout::setupSearch() { + // point the head at expected object location, then start a spiral search + LookoutSearchRequest *curSR = static_cast(curReq); + const ShapeRoot &target = curSR->targetShape; + curSR->cindex = ProjectInterface::getColorIndex(target->getColor()); + Point egoLoc = target->getCentroid(); + if ( target->getRefFrameType() == allocentric ) { + egoLoc.applyTransform(VRmixin::mapBuilder.worldToLocalTranslateMatrix,egocentric); + egoLoc.applyTransform(VRmixin::mapBuilder.worldToLocalRotateMatrix,egocentric); } + cout << "Lookout: search target estimated at " << egoLoc << ", color=" << curSR->cindex << endl; + erouter->addListener(this, EventBase::visRegionEGID,ProjectInterface::visRegionSID,EventBase::deactivateETID); + MMAccessor(pointer_id)->lookAtPoint(egoLoc.coordX(), egoLoc.coordY(), egoLoc.coordZ()); + motman->setPriority(pointer_id,MotionManager::kStdPriority); + trackerState = moveToAcquire; } -void Lookout::scanAlongLine(const Point& startPt, const Point& endPt) { - SharedObject hpmc; - - hpmc->lookAtPoint(endPt.coordX(),endPt.coordY(),endPt.coordZ()); - const float pan1 = hpmc->getJointValue(PanOffset); - const float tilt1 = hpmc->getJointValue(TiltOffset); - const float roll1 = hpmc->getJointValue(RollOffset); - - hpmc->lookAtPoint(startPt.coordX(),startPt.coordY(),startPt.coordZ()); - const float pan0 = hpmc->getJointValue(PanOffset); - const float tilt0 = hpmc->getJointValue(TiltOffset); - const float roll0 = hpmc->getJointValue(RollOffset); - - const unsigned int time = (unsigned int) - (sqrt((pan1-pan0)*(pan1-pan0)+(tilt1-tilt0)*(tilt1-tilt0)+(roll1-roll0)*(roll1-roll0)) - / dynamic_cast(curReq)->scanSpeed); - - SharedObject scanmc; - scanmc->pause(); - scanmc->setOutputCmd(HeadOffset+TiltOffset,tilt0); - scanmc->setOutputCmd(HeadOffset+RollOffset,roll0); - scanmc->setOutputCmd(HeadOffset+PanOffset,pan0); - scanmc->advanceTime(time); - scanmc->setOutputCmd(HeadOffset+TiltOffset,tilt1); - scanmc->setOutputCmd(HeadOffset+RollOffset,roll1); - scanmc->setOutputCmd(HeadOffset+PanOffset,pan1); - - pointer_id = motman->addPrunableMotion(hpmc); // moves head to where scan begins; - // actual scan motion sequence (paused), played when pointer_id above is completed - sequence_id = motman->addPrunableMotion(scanmc,MotionManager::kIgnoredPriority); -} +void Lookout::processSearchEvent(const EventBase &event) { + // cout << "Lookout::processSearchEvent: " << event.getDescription() << endl; + const LookoutSearchRequest *curSR = static_cast(curReq); + switch ( event.getGeneratorID() ) { -void Lookout::scanArea(coordinate_t left, coordinate_t right, - coordinate_t far, coordinate_t near) { - const float& scanSpeed = dynamic_cast(curReq)->scanSpeed; - static const float tiltJointValDelta = mathutils::deg2rad(15.0); // - const int tiltTime = (int) (tiltJointValDelta / scanSpeed); - const float z_coord = -150;//1/wmap.ground_plane(3); - unsigned short counter = 1; // or 3 if start from left - - SharedObject hpmc; //! < virtual headpointer, never gets added to motman - near = (near > 100) ? near : 100; - const Point firstPt(near, (counter < 2) ? right : left, z_coord); // start of motion sequence - hpmc->lookAtPoint(firstPt.coordX(),firstPt.coordY(),firstPt.coordZ()); - float tiltJointVal = hpmc->getJointValue(TiltOffset); + case EventBase::motmanEGID: + if ( event.getSourceID() == pointer_id || event.getSourceID() == sequence_id ) + erouter->addTimer(this, settle_timer, 500, false); + else + return; + break; - // define scan waypoints in terms of joint angles instead of base frame coordinates (x,y,z) - // this way you can make sure to cover entire region but not scanning the same region twice - // would be harder to achieve this if I did this in terms of base frame coords + case EventBase::visRegionEGID: { + const color_class_state *ccs = reinterpret_cast + (ProjectInterface::defRegionGenerator->getImage(ProjectInterface::fullLayer,0)); + const color_class_state &col = ccs[curSR->cindex]; + // insert test to see if desired object has been seen: + if ( col.list != NULL && col.list->area >= curSR->minBlobArea ) + erouter->removeListener(this,EventBase::visRegionEGID); + else + return; + // test succeeded + const Point center = findLocationFor(col.list); + cout << "Lookout search: center = " << center << " area = " << col.list->area << " color = " << curSR->cindex << endl; + // MMAccessor(pointer_id)->lookAtPoint(center.coordX(),center.coordY(),center.coordZ()); + // motman->setPriority(pointer_id,MotionManager::kStdPriority); + motman->setPriority(pointer_id,MotionManager::kIgnoredPriority); + motman->setPriority(posture_id,MotionManager::kIgnoredPriority); + motman->setPriority(sequence_id,MotionManager::kIgnoredPriority); + trackerState = centering; + erouter->addTimer(this, settle_timer, 500, false); + break; + } - hpmc->lookAtPoint(far,left,z_coord); - const float tiltJointValFar = hpmc->getJointValue(TiltOffset); - const float panJointValLeftFar = hpmc->getJointValue(PanOffset); - hpmc->lookAtPoint(far,right,z_coord); - const float panJointValRiteFar = hpmc->getJointValue(PanOffset); - hpmc->lookAtPoint(near,left,z_coord); - float panJointValLeft = hpmc->getJointValue(PanOffset); - hpmc->lookAtPoint(near,right,z_coord); - float panJointValRite = hpmc->getJointValue(PanOffset); - const int panTime = (int) (fabs(panJointValLeft-panJointValRite)/scanSpeed); + case EventBase::timerEGID: + switch ( trackerState ) { + case moveToAcquire: // camera has settled, so start the search + triggerSearchMotionSequence(); + break; + case searching: + case centering: // camera has settled, so grab a picture and return + requestComplete(); + break; + default: + break; + } - const float panJointLeftDelta = (tiltJointValFar-tiltJointVal < 0.01) ? 0 : - (panJointValLeftFar-panJointValLeft)/(tiltJointValFar-tiltJointVal)*tiltJointValDelta; - const float panJointRiteDelta = (tiltJointValFar-tiltJointVal < 0.01) ? 0 : - (panJointValRiteFar-panJointValRite)/(tiltJointValFar-tiltJointVal)*tiltJointValDelta; + default: + break; + } +} - SharedObject scanmc; - scanmc->pause(); - while (true) { - counter++; - counter = counter % 4; - if (counter%2 == 1) { - tiltJointVal += tiltJointValDelta; - if (tiltJointVal > tiltJointValFar) break; - if (counter == 1) panJointValRite += panJointRiteDelta; - else panJointValLeft += panJointLeftDelta; - } - scanmc->advanceTime((counter%2==1) ? tiltTime : panTime); - scanmc->setOutputCmd(HeadOffset+TiltOffset, tiltJointVal); - scanmc->setOutputCmd(HeadOffset+RollOffset,0); - scanmc->setOutputCmd(HeadOffset+PanOffset, (counter < 2) ? panJointValRite : panJointValLeft); +void Lookout::triggerSearchMotionSequence() { + cout << "Lookout: beginning search motion sequence" << endl; + const LookoutSearchRequest *curSR = static_cast(curReq); + const ShapeRoot &target = curSR->targetShape; + Point egoLoc = target->getCentroid(); + if ( target->getRefFrameType() == allocentric ) { + egoLoc.applyTransform(VRmixin::mapBuilder.worldToLocalTranslateMatrix,egocentric); + egoLoc.applyTransform(VRmixin::mapBuilder.worldToLocalRotateMatrix,egocentric); } - hpmc->lookAtPoint(firstPt.coordX(),firstPt.coordY(),firstPt.coordZ()); - pointer_id = motman->addPrunableMotion(hpmc); // moves head to where scan begins - // actual scan motion sequence (paused), played when pointer_id above is completed - sequence_id = motman->addPrunableMotion(scanmc,MotionManager::kIgnoredPriority); + int const nspirals = 5; + float const xstep=50, ystep=100; + HeadPointerMC hpmc_temp; + std::vector jointvals(0); + jointvals.reserve(nspirals*4*NumHeadJoints); + for (int i=1; i<=nspirals; i++) { + searchAt(hpmc_temp,jointvals,egoLoc+Point(-i*xstep, 0)); + searchAt(hpmc_temp,jointvals,egoLoc+Point( 0, i*ystep)); + searchAt(hpmc_temp,jointvals,egoLoc+Point( i*xstep, 0)); + searchAt(hpmc_temp,jointvals,egoLoc+Point( 0, -i*ystep)); + } + // Calculate all the joint positions first (above), then lock the + // motionsequence command and quickly copy them over. + MMAccessor mseq_acc(sequence_id); + mseq_acc->clear(); +#ifdef TGT_HAS_HEAD + for ( std::vector::const_iterator it = jointvals.begin(); it != jointvals.end(); ) { + mseq_acc->advanceTime(800); + for(unsigned int i=0; isetOutputCmd(HeadOffset+i,*(it++)); + } + mseq_acc->advanceTime(800); +#endif + mseq_acc->play(); + motman->setPriority(sequence_id,MotionManager::kStdPriority); + trackerState = searching; +} + +void Lookout::searchAt(HeadPointerMC &hpmc_temp, + std::vector &jointvals, + const Point &target) { + hpmc_temp.lookAtPoint(target.coordX() ,target.coordY() ,target.coordZ()); + for(unsigned int i=0; i + #include "Behaviors/BehaviorBase.h" +#include "Motion/MMAccessor.h" #include "Motion/MotionManager.h" +#include "Motion/HeadPointerMC.h" #include "Motion/MotionSequenceMC.h" +#include "Motion/PostureMC.h" #include "Events/VisionObjectEvent.h" #include "LookoutRequests.h" namespace DualCoding { -class LookoutRequest; -class StoreModeImageRequest; -class ModeImageRequest; +//! The Lookout accepts LookoutRequests to move the head and collect sensor information. /*! - The Lookout's job is to move the head and collect sensor information. - Head motion can be none (user will point the head himself), pointAt, scan, or track. - The data collected can be an image or distance reading, or for scan operations, it - can be a list of locations where certain VisionObject or VisionRegion streams - reported hits. + HeadMotionType can be none (user will point the head himself), + pointAt, scan, track, or search. The data collected can be an image + or distance reading, or for scan operations, it can be a list of + locations where certain VisionObject or VisionRegion streams reported + hits. */ @@ -28,85 +30,125 @@ public: //! Constructor Lookout(); - //! Destructor - virtual ~Lookout() {} - + virtual void DoStart(); virtual void DoStop(); virtual void processEvent(const EventBase& event); - static std::string getClassDescription() { return "moves head according to request"; } + static std::string getClassDescription() { return "Moves head and collects data according to request"; } virtual std::string getDescription() const { return getClassDescription(); } virtual unsigned int executeRequest(const LookoutRequest&); - // virtual bool removeRequest(unsigned int req_id); - // bool isBusy() const { return !requests.empty(); } + bool busy() { return curReq != NULL; } + void stopTrack(); + + static std::vector groundSearchPoints(); //!< returns a vector of points for searching the ground around the robot + + void moveHeadToPoint(); // bool isLandmarkVisible() const { return landmark_inview; } - // bool isExecuting(LookoutRequest::HeadMotionType_t type) const - // { return !requests.empty() && getCurrentRequest().getHeadMotionType() == type; } - static Point findLocationFor(float, float); - static const unsigned int Invalid_LO_ID=(unsigned int)-1; + void relax(); //!< Inactivates all Lookout motion commands; called when MapBuilder finishes + + static Point findLocationFor(const float normX, const float normY); + static const unsigned int invalid_LO_ID=(unsigned int)-1; protected: virtual void executeRequest(); - virtual void requestComplete(); + virtual void requestComplete(bool result=true); + template void pushRequest(const LookoutRequest& req) { - requests.push(new T(*dynamic_cast(&req))); + requests.push(new T(dynamic_cast(req))); } - //! PointAtReqeust - void processPointAt(const EventBase& event); - bool findPixelModes(StoreModeImageRequest& modeRequest); - void getData(); + //@name PointAtRequest functions + //@{ + void processPointAtEvent(const EventBase& event); + bool findPixelModes(); +#ifdef TGT_HAS_IR_DISTANCE + bool findDistanceMode(); + float getDistanceModeValue(); +#endif + //@} + + //@name ScanRequest functions + //@{ + void setupScan(); + void triggerScanMotionSequence(); + void processScanEvent(const EventBase& event); + void scanAlongLine(const Point& start,const Point& end); + void scanAlongPolygon(const std::vector& vertices, const bool closed=false); + void scanArea(const Point &topLeft, const Point &topRight, + const Point &bottomLeft, const Point &bottomRight); + + static Point findLocationFor(const VisionObjectEvent& visev) { + return findLocationFor(visev.getCenterX(), visev.getCenterY()); + } + + static Point findLocationFor(const CMVision::region* reg) { + return findLocationFor(2.0*reg->cen_x/VRmixin::camSkS.getWidth()-1.0, + 2.0*reg->cen_y/VRmixin::camSkS.getHeight()-1.0); + } + + void storeVisionRegionDataTo(std::vector&, const std::set&, int); +#ifdef TGT_HAS_IR_DISTANCE + void storeIRDataTo(std::vector&); +#endif + //@} + + //@name TrackRequest functions + //@{ + void setupTrack(); + void processTrackEvent(const EventBase& event); /* - //! TrackRequest - void processTrack(const EventBase& event); void processTrackVision(float normX, float normY, bool isCurrentlyVisible); void trackObjectAt(float normX, float normY); //! tracks point at [normX,normY] in cam frame Point findLocationFor(float, float); */ - //! ScanReqeust - void processScan(const EventBase& event); - void scan(); - void scanAlongLine(const Point& start,const Point& end); - void scanArea(coordinate_t left, coordinate_t right, - coordinate_t far, coordinate_t near); - // void storeRegionLocation(const SegmentedColorGenerator::color_class_state*); - // void storeVisionObjectLocation(const VisionObjectEvent*); - Point findLocationFor(const VisionObjectEvent* voe) { - return findLocationFor(voe->getCenterX(), voe->getCenterY()); - } - Point findLocationFor(const CMVision::region* reg) { - return findLocationFor - (2.0*reg->cen_x/VRmixin::camSkS.getWidth()-1.0, - 2.0*reg->cen_y/VRmixin::camSkS.getHeight()-1.0); - } + //@} + + //@name SearchRequest functions + //@{ + void setupSearch(); + void processSearchEvent(const EventBase& event); + void triggerSearchMotionSequence(); + static void searchAt(HeadPointerMC &hpmc_temp, + std::vector &jointvals, + const Point &target); + //@} - void storeVisionRegionDataTo(vector&, const set&, int); - void storeIRDataTo(vector&); - // float scanSpeed() const { return baseRange/base_pan_time; } //(rad/msec) private: Lookout& operator=(const Lookout&); Lookout(const Lookout&); protected: - MotionManager::MC_ID pointer_id; //!< a HeadPointerMC object - MotionManager::MC_ID sequence_id; //!< id for scan/find motion sequence - queue requests; - LookoutRequest *curReq; - bool /*pan_prior, */landmarkInView; + std::vector > pixelHistograms; + std::priority_queue distanceSamples; + + MotionManager::MC_ID pointer_id; //!< id for HeadPointerMC for pointing the camera + MotionManager::MC_ID posture_id; //!< id for PostureMC for pointing the IR sensors + MotionManager::MC_ID sequence_id; //!< id for MotionSequenceMC for scanning + std::queue requests; //!< queue of pending LookoutRequest instances, including the current request + LookoutRequest *curReq; //!< pointer to request currently being executed + LookoutPointRequest *curPAR; //!< current Point-At request (same object as curReq) + bool successSave; + + enum TrackerStates { + inactive, + moveToAcquire, + tracking, + searching, + centering, + lost + } trackerState; unsigned int idCounter; enum LookoutTimerSourceId_t { - start_pan, - pan_complete, - reset_pan, - no_event, - dur_timer, + settle_timer = 1, + sample_timer, + lost_timer, scan_timer, //scan_timer has to be at the end of enum list }; }; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/LookoutRequests.cc ./DualCoding/LookoutRequests.cc --- ../Tekkotsu_3.0/DualCoding/LookoutRequests.cc 2006-08-11 18:21:33.000000000 -0400 +++ ./DualCoding/LookoutRequests.cc 2007-11-11 18:57:21.000000000 -0500 @@ -1,5 +1,22 @@ #include "LookoutRequests.h" namespace DualCoding { -const float ScanRequest::defSpd = 0.0015; + +const float LookoutScanRequest::defSpd = 0.0015; + +LookoutScanRequest::~LookoutScanRequest() { + for (std::vector::const_iterator it = tasks.begin(); + it != tasks.end(); it++) + delete *it; } + +const char* const LookoutRequest::headMotionTypeNames[numHeadMotionTypes] = { + "noMotion", + "pointAt", + "scan", + "track", + "search" +}; + + +} // namespace diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/LookoutRequests.h ./DualCoding/LookoutRequests.h --- ../Tekkotsu_3.0/DualCoding/LookoutRequests.h 2006-08-11 18:21:33.000000000 -0400 +++ ./DualCoding/LookoutRequests.h 2007-11-18 01:47:01.000000000 -0500 @@ -5,312 +5,284 @@ #include "Shared/newmat/newmat.h" #include "Shared/ProjectInterface.h" #include "Shared/WorldState.h" -#include "DualCoding.h" + +#include "Sketch.h" +#include "Point.h" +#include "ShapeRoot.h" #include "VRmixin.h" namespace DualCoding { +//! Base class for requests to the Lookout + class LookoutRequest { public: enum HeadMotionType_t { - none, - pointAt, - scan, - track + noMotion, //!< use current head position + pointAt, //!< move head to specified gaze point + scan, //!< scan head along specified path + track, //!< move head to track object + search, //!< spiral search for a known object + numHeadMotionTypes }; - virtual HeadMotionType_t getHeadMotionType() const = 0; - //! Constructor - LookoutRequest() : requestID((unsigned int) -1) {} - virtual ~LookoutRequest() {} - //! Lookout assigns non-zero value when added to queue - unsigned int requestID; -protected: - //! Copy constructor - LookoutRequest(const LookoutRequest& req) : requestID(req.requestID) {} -private: - LookoutRequest& operator=(const LookoutRequest& req); -}; - -// ---------- pointAt requests ---------- -class PointAtBase : public LookoutRequest { -protected: - //! Constructor - PointAtBase(unsigned int _joint, unsigned int _duration) - : joint(_joint), toBaseMatrix(), duration(_duration) {} - - //! Copy constructor - PointAtBase(const PointAtBase& base) - : LookoutRequest(base), joint(base.joint), - toBaseMatrix(base.toBaseMatrix), duration(base.duration) {} - -public: - enum PointAtRequestType_t { none, storeImage, measureDist }; - virtual PointAtRequestType_t getPointAtType() const = 0;//const { return none; } - virtual HeadMotionType_t getHeadMotionType() const { return LookoutRequest::none; } - unsigned int joint; //!< frame from which base frame transformation matrix is created, e.g., Camera, IR, etc. - NEWMAT::Matrix toBaseMatrix; //!< transformation matrix from joint frame to base frame - //! how much time to wait before taking measurements or throw completion event after head reaches gazePt. - unsigned int duration; -}; - -class PointAtRequest : virtual public PointAtBase { -public: - //! Constructor - PointAtRequest(const Point& pt, unsigned int dur=1000) - : PointAtBase(CameraFrameOffset,dur), gazePt(pt) {}; - //! Copy constructor - PointAtRequest(const PointAtRequest& req) - : PointAtBase(req), gazePt(req.gazePt) {} + static const char* const headMotionTypeNames[numHeadMotionTypes]; - virtual PointAtRequestType_t getPointAtType() const { return none; } - virtual HeadMotionType_t getHeadMotionType() const { return pointAt; } - Point gazePt; //!< point to look at in base frame coordinates -}; + HeadMotionType_t getHeadMotionType() const { return headMotionType; } + void setHeadMotionType(const HeadMotionType_t htype) { headMotionType = htype; } -class StoreImageRequest : virtual public PointAtBase { -public: - //! Constructor - StoreImageRequest(Sketch (&func)()=VRmixin::sketchFromSeg, - unsigned int dur=1000) - : PointAtBase(CameraFrameOffset,dur), image(), sketchFunc(func) {}; - //! Copy constructor - StoreImageRequest(const StoreImageRequest& req) - : PointAtBase(req), image(req.image), sketchFunc(req.sketchFunc) {} + enum LookoutResultType_t { + noResult, //!< don't return anything (just move the head) + imageResult, //!< take a picture +#ifdef TGT_HAS_IR_DISTANCE + distanceResult, //!< measure distance with IR rangefinder +#endif + interestPoints //!< collection of interest points (from scanning) + }; - virtual PointAtRequestType_t getPointAtType() const { return storeImage; } - Sketch image; // (*sketchFunc)(); // (&func)()=VRmixin::sketchFromSeg, - unsigned int dur=1000) - : PointAtBase(CameraFrameOffset,dur), PointAtRequest(pt,dur), StoreImageRequest(func,dur) {} - //! Copy constructor - StoreImageAtRequest(const StoreImageAtRequest& req) - : PointAtBase(req), PointAtRequest(req), StoreImageRequest(req) {} + LookoutRequest(HeadMotionType_t htype=noMotion, LookoutResultType_t rtype=noResult) : + headMotionType(htype), resultType(rtype), requestID(0) {} - virtual PointAtRequestType_t getPointAtType() const { return storeImage; } - virtual HeadMotionType_t getHeadMotionType() const { return pointAt; } -private: - StoreImageAtRequest& operator=(const StoreImageAtRequest&); -}; - -class StoreModeImageRequest : public StoreImageRequest { -public: - //! Constructor - StoreModeImageRequest(unsigned int num, unsigned int _interval=100, - Sketch (&func)()=VRmixin::sketchFromSeg, - unsigned int dur=1000) - : PointAtBase(CameraFrameOffset,dur), StoreImageRequest(func,dur), - numImages(num), interval(_interval) {}; + //! Destructor + virtual ~LookoutRequest() {} //! Copy constructor - StoreModeImageRequest(const StoreModeImageRequest& req) - : PointAtBase(req), StoreImageRequest(req), - numImages(req.numImages), interval(req.interval) {} - - unsigned int numImages; //!< number of image(sketch) over which to take mode - unsigned int interval; //!< interval between storing images -}; + LookoutRequest(const LookoutRequest &req) : + headMotionType(req.headMotionType), + resultType(req.resultType), + requestID(req.requestID) {} -class StoreModeImageAtRequest : public PointAtRequest, public StoreModeImageRequest { -public: - //! Constructor - StoreModeImageAtRequest (const Point& pt, unsigned int num, unsigned int _interval=100, - Sketch (&func)()=VRmixin::sketchFromSeg, unsigned int dur=1000) - : PointAtBase(CameraFrameOffset,dur), PointAtRequest(pt,dur), - StoreModeImageRequest(num,_interval,func) {} - //! Copy constructor - StoreModeImageAtRequest(const StoreModeImageAtRequest& req) - : PointAtBase(req), PointAtRequest(req), StoreModeImageRequest(req) {} + HeadMotionType_t headMotionType; + LookoutResultType_t resultType; + unsigned int requestID; //!< Non-zero value assigned when the request is added to the queue - virtual PointAtRequestType_t getPointAtType() const { return storeImage; } - virtual HeadMotionType_t getHeadMotionType() const { return pointAt; } private: - StoreModeImageAtRequest& operator=(const StoreModeImageAtRequest&); -}; + LookoutRequest& operator=(const LookoutRequest&); -class MeasureDistanceRequest : virtual public PointAtBase { public: - //! Constructor - MeasureDistanceRequest(unsigned int dur=1000) - : PointAtBase( -#ifdef TGT_ERS7 - NearIRFrameOffset -#else - IRFrameOffset -#endif - ,dur), val() {}; - //! Copy Constructor - MeasureDistanceRequest(const MeasureDistanceRequest& req) - : PointAtBase(req), val(req.val) {} - - virtual PointAtRequestType_t getPointAtType() const { return measureDist; } - float val; -}; + // ------------ Tasks that may be implemented during a scan or track request ---------------- + //! Base class for Lookout tasks; cannot instantiate directly + class Task { + public: + enum TaskType_t { noTask, visObjTask, visRegTask, irTask }; -class MeasureDistanceAtRequest : public PointAtRequest, public MeasureDistanceRequest { -public: - //! Constructor - MeasureDistanceAtRequest(const Point& pt, unsigned int dur=1000) - : PointAtBase(0/*overwritten by MeasureDistanceRequest constructor*/,dur), - PointAtRequest(pt,dur), MeasureDistanceRequest(dur) {}; - //! Copy constructor - MeasureDistanceAtRequest(const MeasureDistanceAtRequest& req) - : PointAtBase(req), PointAtRequest(req), MeasureDistanceRequest(req) {} - virtual PointAtRequestType_t getPointAtType() const { return measureDist; } - virtual HeadMotionType_t getHeadMotionType() const { return pointAt; } -}; + virtual TaskType_t getTaskType() const = 0; + virtual Task* clone() const = 0; -//-------------------------------------------- + //! Constructor + Task(AngPi _dTheta) : dTheta(_dTheta), data() {} -// ------------SCAN requests------------------- -class ScanRequest : public LookoutRequest { -public: - class Task; - virtual HeadMotionType_t getHeadMotionType() const { return scan; } - enum ScanType_t { line,area }; - virtual ScanType_t getScanType() const = 0; - vector tasks; - float scanSpeed; // in rad/msec - static const float defSpd; - ScanRequest(const ScanRequest& req) - : LookoutRequest(req), tasks(), scanSpeed(req.scanSpeed) { - for (vector::const_iterator it = req.tasks.begin(); - it != req.tasks.end(); it++) - addTask(**it); - } - ScanRequest(float _speed=defSpd) - : tasks(), scanSpeed(_speed) {} - ScanRequest(const Task& _task, float _speed=defSpd) - : tasks(), scanSpeed(_speed) { addTask(_task); } - void addTask(const Task& t) { - switch (t.getTaskType()) { - case Task::visReg: - tasks.push_back(new VisionRegionTask(*dynamic_cast(&t))); break; - case Task::visObj: - tasks.push_back(new VisionObjectTask(*dynamic_cast(&t))); break; - case Task::ir: - tasks.push_back(new IRTask(*dynamic_cast(&t))); break; - default: return; - }; - } - virtual ~ScanRequest() { - for (vector::const_iterator it = tasks.begin(); - it != tasks.end(); it++) - delete *it; - } + //! Copy constructor + Task(const Task& t) : dTheta(t.dTheta), data(t.data) {} - // ------------Tasks may be implemented during scan ------------------- - class Task { - public: - enum taskType_t { none, visObj, visReg, ir }; - virtual taskType_t getTaskType() const = 0;// { return none; } + //! Destructor virtual ~Task() {} - AngPi interval; //!< interval of measurements - vector data; //!< measured data stored here in base frame coordinates - Task(const Task& t) : interval(t.interval), data(t.data) {} - protected: - Task(AngPi _interval) : interval(_interval), data() {} - private: + + AngPi dTheta; //!< angular step size during scan + std::vector data; //!< measured data stored here in base frame coordinates Task& operator=(const Task&); }; class IRTask : public Task { public: - IRTask(AngPi _interval) : Task(_interval) {} + IRTask(AngPi _dTheta) : Task(_dTheta) {} IRTask(const IRTask& t) : Task(t) {} - virtual taskType_t getTaskType() const { return ir; } - // virtual ~IRTask() { cout << "deleting IRTask " << this << endl; } + virtual TaskType_t getTaskType() const { return irTask; } + virtual Task* clone() const { return new IRTask(*this); } }; - //base class for vision tasks, should not be instantiated + //! Base class for vision tasks, should not be instantiated class VisionTask : public Task { public: - virtual taskType_t getTaskType() const { return none; } - set index; + virtual TaskType_t getTaskType() const { return noTask; } + std::set index; VisionTask(const VisionTask& vt) : Task(vt), index(vt.index) {} - protected: - VisionTask(const set& _index, AngPi _interval) - : Task(_interval), index(_index) {} - VisionTask(int _index, AngPi _interval) - : Task(_interval), index() { index.insert(_index); } + VisionTask(const std::set& _index, AngPi _dTheta) + : Task(_dTheta), index(_index) {} + VisionTask(int _index, AngPi _dTheta) + : Task(_dTheta), index() { index.insert(_index); } + virtual Task* clone() const { return new VisionTask(*this); } }; //! Uses bult-in object detectors (like pink ball detector) via VisionObjectEvent stream class VisionObjectTask : public VisionTask { public: - VisionObjectTask(const set& sid, AngPi _interval) - : VisionTask(sid,_interval) {} + VisionObjectTask(const std::set& sid, AngPi _dTheta=0) + : VisionTask(sid,_dTheta) {} VisionObjectTask(const VisionObjectTask& vot) : VisionTask(vot) {} - virtual taskType_t getTaskType() const { return visObj; } + virtual TaskType_t getTaskType() const { return visObjTask; } + virtual Task* clone() const { return new VisionObjectTask(*this); } }; //! Uses built-in colored region detectors via Region event stream class VisionRegionTask : public VisionTask { public: - VisionRegionTask(const set& colorIndex, AngPi _interval, + VisionRegionTask(const std::set& colorIndex, AngPi _dTheta=0, unsigned int _minArea=200) - : VisionTask(colorIndex,_interval), minArea(_minArea) {} - VisionRegionTask(int colorIndex, AngPi _interval, + : VisionTask(colorIndex,_dTheta), minArea(_minArea) {} + VisionRegionTask(int colorIndex, AngPi _dTheta, unsigned int _minArea=200) - : VisionTask(colorIndex,_interval), minArea(_minArea) {} + : VisionTask(colorIndex,_dTheta), minArea(_minArea) {} VisionRegionTask(const VisionRegionTask& vrt) : VisionTask(vrt), minArea(vrt.minArea) {} - virtual taskType_t getTaskType() const { return visReg; } + virtual TaskType_t getTaskType() const { return visRegTask; } + virtual Task* clone() const { return new VisionRegionTask(*this); } unsigned int minArea; }; + // ---------------- end of Task classes ---------------- + }; -class ScanAlongLineRequest : public ScanRequest { +//================ LookoutPointRequest ================ + +//! Take a picture of or measure a distance to a point in space +class LookoutPointRequest : public LookoutRequest { public: - ScanAlongLineRequest(const Point& bPt, const Point& ePt, float _speed=defSpd) - : ScanRequest(_speed), beginPt(bPt), endPt(ePt) {} - ScanAlongLineRequest(const ScanAlongLineRequest& req) - : ScanRequest(req), beginPt(req.beginPt), endPt(req.endPt) {} - // virtual HeadMotionType_t getHeadMotionType() const { return scanLine; } - virtual ScanType_t getScanType() const { return line; } - Point beginPt, endPt; + //! Constructor + LookoutPointRequest() : + LookoutRequest(noMotion,imageResult), +#ifdef TGT_HAS_CAMERA + joint(CameraFrameOffset), +#else + joint(0), +#endif + toBaseMatrix(), + gazePt(), motionSettleTime(1000), + numSamples(1), sampleCounter(0), sampleInterval(0), + image(), sketchFunc(VRmixin::sketchFromSeg) + {} + + //! Copy constructor + LookoutPointRequest(const LookoutPointRequest &p) + : LookoutRequest(p), joint(p.joint), toBaseMatrix(p.toBaseMatrix), + gazePt(p.gazePt), motionSettleTime(p.motionSettleTime), + numSamples(p.numSamples), sampleCounter(p.sampleCounter), sampleInterval(p.sampleInterval), + image(p.image), sketchFunc(p.sketchFunc) + {} + +public: + void setTarget(const Point &target) { + gazePt = target; + headMotionType = pointAt; + } + + unsigned int joint; //!< joint reference frame from which base frame transformation matrix is created, e.g., Camera, IR, etc. + NEWMAT::Matrix toBaseMatrix; //!< transformation matrix from joint frame to base frame + Point gazePt; //!< point to look at; can be in either egocentric or allocentric reference frame + unsigned int motionSettleTime; //!< Time in msec to wait before taking measurements or throwing completion event after head reaches gazePt. + int numSamples; //!< Number of samples to take; if > 1, return the mode (for pixels) or median (for distance) + int sampleCounter; //!< Number of samples collected so far + int sampleInterval; //!< Interval in msec between successive samples + Sketch image; // (*sketchFunc)(); // tasks; + float scanSpeed; //!< speed in rad/msec + ShapeRoot searchArea; + unsigned int motionSettleTime; + + static const float defSpd; //!< default scan speed + + //! Constructor + LookoutScanRequest(float _speed=defSpd): + LookoutRequest(scan,interestPoints), + tasks(), scanSpeed(_speed), searchArea(), motionSettleTime(1000) {} + + //! Constructor + LookoutScanRequest(const Task& _task, float _speed=defSpd) : + LookoutRequest(scan,interestPoints), tasks(), scanSpeed(_speed), searchArea(), motionSettleTime(1000) + { addTask(_task); } + + //! Copy constructor + LookoutScanRequest(const LookoutScanRequest& req) + : LookoutRequest(req), tasks(), scanSpeed(req.scanSpeed), searchArea(req.searchArea), motionSettleTime(req.motionSettleTime) + { + for (std::vector::const_iterator it = req.tasks.begin(); + it != req.tasks.end(); it++) + addTask(**it); + } + + //! Destructor + virtual ~LookoutScanRequest(); + + void addTask(const Task& t) { tasks.push_back(t.clone()); } + +}; // end of LookoutScanRequest + + + +// ================ LookoutTrackRequest ================ + +// ! Track an object with the head +class LookoutTrackRequest : public LookoutRequest { +public: + + friend class Lookout; + + //! Constructor + LookoutTrackRequest(const ShapeRoot& target=ShapeRoot()) + : LookoutRequest(track), targetShape(target), + minBlobArea(80), exitTest(NULL), cindex(0) {} + + //! Copy constructor + LookoutTrackRequest(const LookoutTrackRequest& req) + : LookoutRequest(req), targetShape(req.targetShape), + minBlobArea(req.minBlobArea), exitTest(req.exitTest), cindex(req.cindex) {} + + ShapeRoot targetShape; + int minBlobArea; //!< Minimum acceptable size of a region + bool (*exitTest)(); //!< If true, target has been found + +private: + color_index cindex; + + LookoutTrackRequest& operator=(const LookoutTrackRequest&); //!< don't call this }; -//-------------------------------------------- -// ------------TRACK requests------------------- -class TrackRequest : public LookoutRequest { +// ================ LookoutSearchRequest ================ + +//! Search for an object and return when found +class LookoutSearchRequest : public LookoutRequest { public: - TrackRequest(const vector& _landmarks, const ShapeRoot& _area) - : landmarks(_landmarks), area(_area), lmLocation(), lmInView(false) {} - TrackRequest(const vector& _landmarks, const Point& pt, const ShapeRoot& _area) - : landmarks(_landmarks), area(_area), lmLocation(pt), lmInView(false) {} - virtual HeadMotionType_t getHeadMotionType() const { return track; } - vector landmarks; - ShapeRoot area; - Point lmLocation; - bool lmInView; + + friend class Lookout; + + //! Constructor + LookoutSearchRequest(const ShapeRoot& target=ShapeRoot()) + : LookoutRequest(search,imageResult), targetShape(target), + minBlobArea(80), exitTest(NULL), cindex(0) {} + + //! Copy constructor + LookoutSearchRequest(const LookoutSearchRequest& req) + : LookoutRequest(req), targetShape(req.targetShape), + minBlobArea(req.minBlobArea), exitTest(req.exitTest), cindex(req.cindex) {} + + ShapeRoot targetShape; + int minBlobArea; //!< Minimum acceptable size of a region + bool (*exitTest)(); //!< If true, target has been found + +private: + color_index cindex; + + LookoutSearchRequest& operator=(const LookoutSearchRequest&); //!< don't call this }; -//-------------------------------------------- + } // namespace diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/MapBuilder.cc ./DualCoding/MapBuilder.cc --- ../Tekkotsu_3.0/DualCoding/MapBuilder.cc 2006-10-03 19:26:06.000000000 -0400 +++ ./DualCoding/MapBuilder.cc 2007-11-18 01:47:02.000000000 -0500 @@ -1,31 +1,36 @@ //-*-c++-*- #include "Events/EventRouter.h" -#include "Events/LookoutEvents.h" #include "Events/TextMsgEvent.h" +#include "Events/LookoutEvents.h" #include "Motion/HeadPointerMC.h" #include "Shared/mathutils.h" #include "Shared/newmat/newmat.h" -#include "Measures.h" +#include "Shared/Measures.h" #include "EllipseData.h" // for extractEllipses #include "SphereData.h" // for extractSpheres #include "BlobData.h" // for extractSpheres #include "PolygonData.h" // for extractPolygons +#include "SphereData.h" +#include "TargetData.h" // for extractTarget #include "ShapeRoot.h" #include "ShapeLine.h" #include "ShapeEllipse.h" #include "ShapeBlob.h" #include "ShapePolygon.h" +#include "ShapeSphere.h" +#include "ShapeTarget.h" #include "Sketch.h" // for NEW_SKETCH #include "visops.h" #include "VRmixin.h" -#include "MapBuilder.h" +#include "LookoutRequests.h" #include "Lookout.h" +#include "MapBuilder.h" -namespace DualCoding { +using namespace std; -typedef map > colorMap; +namespace DualCoding { inline float distSq(const NEWMAT::ColumnVector& vec) { return vec(1)*vec(1) + vec(2)*vec(2) + vec(3)*vec(3); @@ -33,31 +38,46 @@ MapBuilder::MapBuilder() : BehaviorBase("MapBuilder"), - camSkS(VRmixin::getCamSkS()), - camShS(VRmixin::getCamSkS().getDualSpace()), - groundShS(VRmixin::groundShS), - local(maps::local,VRmixin::getLocalSkS()), world(maps::world,VRmixin::getWorldSkS()), - cur(&local), xres(camSkS.getWidth()), yres(camSkS.getHeight()), - ground_plane(4), theAgent(VRmixin::theAgent), + camSkS(VRmixin::getCamSkS()), camShS(camSkS.getDualSpace()), + groundShS(VRmixin::getGroundShS()), + localSkS(VRmixin::getLocalSkS()), localShS(localSkS.getDualSpace()), + worldSkS(VRmixin::getWorldSkS()), worldShS(worldSkS.getDualSpace()), + xres(camSkS.getWidth()), yres(camSkS.getHeight()), + ground_plane(4), + theAgent(VRmixin::theAgent), localToWorldMatrix(NEWMAT::IdentityMatrix(4)), worldToLocalTranslateMatrix(NEWMAT::IdentityMatrix(4)), worldToLocalRotateMatrix(NEWMAT::IdentityMatrix(4)), badGazePoints(), - agentAtOrigin(true), requests(), curReq(NULL), idCounter(0), maxDistSq(0), - pointAtID(Lookout::Invalid_LO_ID), scanID(Lookout::Invalid_LO_ID), + requests(), curReq(NULL), idCounter(0), maxDistSq(0), + pointAtID(Lookout::invalid_LO_ID), scanID(Lookout::invalid_LO_ID), nextGazePoint() {} void MapBuilder::DoStart() { - cout << "MapBuilder::DoStart()\n"; + if ( verbosity & MBVstart ) + cout << "MapBuilder::DoStart()\n"; BehaviorBase::DoStart(); - erouter->addListener(this,EventBase::textmsgEGID); + + camSkS.clear(); camShS.clear(); + groundShS.clear(); + localSkS.clear(); localShS.clear(); + worldSkS.clear(); worldShS.clear(); + badGazePoints.clear(); + setAgent(Point(0,0,0),0); + + erouter->addListener(this,EventBase::textmsgEGID); // listen for commands to move to next gaze point + erouter->addListener(this,EventBase::lookoutEGID); // listen for Lookout completion events } +bool MapBuilder::retain = true; +MapBuilder::MapBuilderVerbosity_t MapBuilder::verbosity = -1U; + /* - since MapBuilder is constructed as static from VRmixin, - destructor doesn't get called until dog shuts down. - And we have to do everything assumed to be done in destructor in DoStop, - such as clearing request queue and setting parameters to the initial values as set in constructor + Since MapBuilder is constructed as static from VRmixin, the + destructor doesn't get called until the robot shuts down. We have + to do everything assumed to be done in destructor in DoStop, such as + clearing the request queue and setting parameters to the initial + values as set in the constructor. */ void MapBuilder::DoStop() { cout << "MapBuilder::DoStop()\n"; @@ -66,153 +86,168 @@ requests.pop(); } curReq = NULL; - camSkS.clear(); camShS.clear(); - groundShS.clear(); - local.SkS.clear(); local.ShS.clear(); - world.SkS.clear(); world.ShS.clear(); - - badGazePoints.clear(); - local.gazePts.clear(); - world.gazePts.clear(); - setAgent(Point(0,0,0),0); - BehaviorBase::DoStop(); } -unsigned int MapBuilder::executeRequest(const MapBuilderRequest& req) { - switch (req.getRequestType()) { - case MapBuilderRequest::takeSnap: - if (const TakeSnapAtRequest* snapAtReq = dynamic_cast(&req)) - requests.push(new TakeSnapAtRequest(*snapAtReq)); - else - requests.push(new TakeSnapRequest(*dynamic_cast(&req))); - break; - case MapBuilderRequest::localMap: - if (const LocalMapTestRequest* testReq = dynamic_cast(&req)) - requests.push(new LocalMapTestRequest(*testReq)); - else - requests.push(new LocalMapRequest(*dynamic_cast(&req))); - break; - case MapBuilderRequest::worldMap: - if (const WorldMapTestRequest* testReq = dynamic_cast(&req)) - requests.push(new WorldMapTestRequest(*testReq)); - else - requests.push(new WorldMapRequest(*dynamic_cast(&req))); - break; - default: return -1U; - }; - requests.back()->requestID = idCounter++; - // cout << "MapBuilder::executeRequest: new request " << requests.back()->requestID - // << " added to the queue\n"; - if (requests.size() == 1) { - erouter->addListener(this,EventBase::lookoutEGID); - executeRequest(); - } - return requests.back()->requestID; +unsigned int MapBuilder::executeRequest(const MapBuilderRequest& req, unsigned int *req_id) { + MapBuilderRequest *newreq = new MapBuilderRequest(req); + const unsigned int this_request = ++idCounter; + newreq->requestID = this_request; + if ( req_id != NULL ) *req_id = this_request; + requests.push(newreq); + executeRequest(); + return this_request; } void MapBuilder::executeRequest() { - if ( requests.empty() || curReq != NULL ) return; + if ( curReq != NULL || requests.empty() ) return; curReq = requests.front(); - cout << "MapBuilder::executeRequest: execute " << curReq->requestID << endl; - erouter->postEvent(EventBase::mapbuilderEGID, requests.front()->requestID, EventBase::activateETID,0); - if (curReq->groundPlaneAssumption == MapBuilderRequest::onStand) - calculateGroundPlane(MapBuilderRequest::onStand); - - if (curReq->getRequestType() == MapBuilderRequest::takeSnap) - if (const TakeSnapAtRequest* snapAtReq = dynamic_cast(curReq)) - storeImageAt(snapAtReq->gazePt); - else - storeImage(); - else { - const LocalMapRequest& curMapReq = *dynamic_cast (&(*curReq)); - maxDistSq = curMapReq.maxDist*curMapReq.maxDist; - cur = (&curMapReq.shs==&local.ShS) ? &local : &world; - if (curMapReq.clearShapes) { // erase shapes and gaze points, start from scratch - cur->ShS.clear(); - cur->SkS.clear(); - cur->gazePts.clear(); - cur->baseToCamMats.clear(); - } - defineGazePts(cur->gazePts, curMapReq.area, curMapReq.doScan); - if ( determineNextGazePoint() ) - moveToNextGazePoint(); - else - requestComplete(); + if ( verbosity & MBVexecute ) + cout << "MapBuilder::executeRequest: execute " << curReq->requestID << endl; + erouter->postEvent(EventBase::mapbuilderEGID, curReq->requestID, EventBase::activateETID,0); + if ( (curReq->pursueShapes || curReq->doScan || curReq->worldTargets.size()>0) && + curReq->getRequestType() == MapBuilderRequest::cameraMap ) { + cout << "Warning: switching MapBuilderRequest type from cameraMap to localMap because request parameters require head movement." << endl; + curReq->requestType = MapBuilderRequest::localMap; } + + calculateGroundPlane(); + maxDistSq = curReq->maxDist*curReq->maxDist; + + if ( curReq->clearShapes ) { + curReq->gazePts.clear(); + curReq->baseToCamMats.clear(); + switch ( curReq->getRequestType() ) { + case MapBuilderRequest::localMap: + localShS.clear(); + localSkS.clear(); + break; + case MapBuilderRequest::worldMap: + worldShS.clear(); + worldSkS.clear(); + break; + default: + break; + } + } + + if ( curReq->immediateRequest() ) + grabCameraImageAndGo(); + else if ( !curReq->searchArea.isValid() & curReq->worldTargets.size() == 0 ) + storeImage(); + else { + defineGazePts(); + if ( curReq->doScan == true ) + return; // wait for Lookout to finish scanning + else if ( curReq->worldTargets.size() > 0 ) + doNextSearch(); + else if ( determineNextGazePoint() ) + moveToNextGazePoint(); + else + requestComplete(); + } } +/* +There are three kinds of events for MapBuilder::processEvent to handle: + +1. TextMsg event saying to move to the next gaze point; only needed +when we've asserted manual control of gaze point sequencing for +debugging. + +2. Lookout event announcing that a "point at" request is complete and +an image has been stored. We can now go ahead and process that image. + +3. Lookout event announcing that a "scan" request is complete and gaze +points have been stored. We can now start examining those gaze points. +*/ + + void MapBuilder::processEvent(const EventBase &e) { - cout << "MapBuilder::processEvent got " << e.getDescription() << endl; + if ( curReq == NULL) return; + if ( verbosity & MBVevents ) + cout << "MapBuilder got event " << e.getName() << "; current pointAtID=" << pointAtID + << " scanID=" << scanID << endl; + switch ( e.getGeneratorID() ) { - case EventBase::textmsgEGID: { - const TextMsgEvent &txtev = dynamic_cast(e); - if ( strcmp(txtev.getText().c_str(),"MoveHead") == 0 ) + case EventBase::textmsgEGID: + if ( strcmp(dynamic_cast(e).getText().c_str(),"MoveHead") == 0 ) moveToNextGazePoint(true); - break; } + break; + case EventBase::lookoutEGID: - // image stored, extract shapes and extend local/world shape space - cout << "MapBuilder got event " << e.getName() << " pointAtID=" << pointAtID << endl; - if (e.getSourceID() == pointAtID && e.getTypeID() == EventBase::deactivateETID) { - const Sketch& camImg = dynamic_cast(&e)->getSketch(); - NEW_SKETCH(camFrame, uchar, camImg); - const NEWMAT::Matrix& camToBase = dynamic_cast(&e)->toBaseMatrix; - const NEWMAT::Matrix baseToCam = camToBase.i(); - processImage(camFrame, camToBase, baseToCam); - if (curReq->getRequestType() == MapBuilderRequest::takeSnap) { - requestComplete(); - return; - } - if (cur->space == maps::local) - extendLocal(baseToCam); - else - extendWorld(baseToCam); - } - // gaze points defined by scan, start constructing map - else if (e.getSourceID() == scanID && e.getTypeID() != EventBase::activateETID) { + if ( e.getSourceID() == pointAtID ) + processImage(dynamic_cast(e)); + else if ( e.getSourceID() == scanID ) { const vector& pts = dynamic_cast(&e)->getTasks().front()->data; - cur->gazePts.insert(cur->gazePts.end(),pts.begin(), pts.end()); + cout << " doScan found " << pts.size() << " interest points." << endl; + curReq->gazePts.insert(curReq->gazePts.end(),pts.begin(), pts.end()); } - else - cout << "MapBuilder::processEvent(): unknown event " << e.getDescription() - << " pointAtID=" << pointAtID << endl; - - if ( requestExitTest() || !determineNextGazePoint() ) + else { + cout << "MapBuilder: unexpected Lookout event " << e.getDescription() + << ", current pointAtID=" << pointAtID << ", scanID=" << scanID << endl; + return; + } + // we've dealt with the event (processed an image or did a scan); now see what we should do next + if ( requestExitTest() ) requestComplete(); - else + else if ( curReq->worldTargets.size() > 0 ) + doNextSearch(); + else if ( determineNextGazePoint() ) moveToNextGazePoint(); + else + requestComplete(); break; + default: - cout << "Unexpected event " << e.getDescription() << endl; + cout << "MapBuilder received unexpected event: " << e.getDescription() << endl; } } -void MapBuilder::processImage(const Sketch& camFrame, - const NEWMAT::Matrix& camToBase, - const NEWMAT::Matrix& baseToCam) { - if (curReq->groundPlaneAssumption == MapBuilderRequest::onLegs) - calculateGroundPlane(MapBuilderRequest::onLegs); +void MapBuilder::processImage(const LookoutSketchEvent &e) { + camSkS.clear(); + if ( curReq->rawY ) { + NEW_SKETCH(rawY, uchar, VRmixin::sketchFromRawY()); + } + NEW_SKETCH(camFrame, uchar, e.getSketch()); + const NEWMAT::Matrix& camToBase = e.toBaseMatrix; + const NEWMAT::Matrix baseToCam = camToBase.i(); getCameraShapes(camFrame); - projectToGround(camToBase); - filterGroundShapes(baseToCam); + if (curReq->userCamProcessing != NULL) (*curReq->userCamProcessing)(); + if (curReq->getRequestType() > MapBuilderRequest::cameraMap) { + projectToGround(camToBase); + if (curReq->userGroundProcessing != NULL) (*curReq->userGroundProcessing)(); + filterGroundShapes(baseToCam); + } + switch ( curReq->getRequestType() ) { + case MapBuilderRequest::cameraMap: + case MapBuilderRequest::groundMap: + break; + case MapBuilderRequest::localMap: + extendLocal(baseToCam); + if (curReq->userLocalProcessing != NULL) (*curReq->userLocalProcessing)(); + break; + case MapBuilderRequest::worldMap: + extendWorld(baseToCam); + if (curReq->userWorldProcessing != NULL) (*curReq->userWorldProcessing)(); + } } bool MapBuilder::determineNextGazePoint() { - if (&cur->ShS == &world.ShS && !agentAtOrigin) { - cur->ShS.applyTransform(worldToLocalTranslateMatrix); - cur->ShS.applyTransform(worldToLocalRotateMatrix); - bool b = determineNextGazePoint(cur->ShS.allShapes()) || determineNextGazePoint(cur->gazePts); - cur->ShS.applyTransform(localToWorldMatrix); // transform back + if (curReq->getRequestType() == MapBuilderRequest::worldMap) { + worldShS.applyTransform(worldToLocalTranslateMatrix,egocentric); + worldShS.applyTransform(worldToLocalRotateMatrix,egocentric); + bool b = determineNextGazePoint(worldShS.allShapes()) || determineNextGazePoint(curReq->gazePts); + worldShS.applyTransform(localToWorldMatrix,allocentric); // transform back return b; } else - return determineNextGazePoint(cur->ShS.allShapes()) || determineNextGazePoint(cur->gazePts); + return determineNextGazePoint(localShS.allShapes()) || determineNextGazePoint(curReq->gazePts); } + bool MapBuilder::determineNextGazePoint(const vector& shapes) { - const LocalMapRequest *locReq = dynamic_cast(curReq); - if ( locReq == NULL || ! locReq->pursueShapes ) - return false; + if ( ! curReq->pursueShapes ) return false; HeadPointerMC headpointer_mc; for (vector::const_iterator it = shapes.begin(); it != shapes.end(); it++) { @@ -223,12 +258,15 @@ bool isLine = (*it)->isType(lineDataType); EndPoint p[2] = { isLine ? ld->end1Pt(): pd->end1Pt(), isLine ? ld->end2Pt() : pd->end2Pt()}; for (int i = 0; i < 2; i++) { - if (!p[i].isValid() && !isBadGazePoint(p[i]) + if ( !p[i].isValid() && !isBadGazePoint(p[i] ) && badGazePoints.end() == find(badGazePoints.begin(), badGazePoints.end(), (Point) p[i])) { - cout << "take snap at endpoint" << (i+1) << " of shape id " + cout << "Take image at endpoint" << (i+1) << " of shape id " << (*it)->getId() << " at " << p[i] << endl; - if (!headpointer_mc.lookAtPoint(p[i].coordX(),p[i].coordY(),p[i].coordZ())) + if ( !headpointer_mc.lookAtPoint(p[i].coordX(),p[i].coordY(),p[i].coordZ()) ) { + if ( verbosity & MBVbadGazePoint ) + cout << "MapBuilder noting unreachable gaze point " << (Point)p[i] << endl; badGazePoints.push_back((Point)p[i]); + } nextGazePoint = p[i]; return true; } @@ -236,11 +274,12 @@ } // look for shapes w/ <2 confidence if ((!(*it)->isType(agentDataType)) && + (*it)->getLastMatchId() != 0 && (*it)->getConfidence() <= 1 && ! isBadGazePoint((*it)->getCentroid()) && badGazePoints.end() == find(badGazePoints.begin(), badGazePoints.end(), (*it)->getCentroid())) { const Point pt = (*it)->getCentroid(); - cout << "take snap at shape " << (*it)->getId() + cout << "take image at shape " << (*it) << " (confidence level: " << (*it)->getConfidence() << ")" << endl; cout << " at " << pt << endl; if (! headpointer_mc.lookAtPoint(pt.coordX(),pt.coordY(),pt.coordZ())) @@ -248,29 +287,21 @@ nextGazePoint = pt; return true; } - cout << "Skipping shape " << (*it)->getId() - << " (confidence level: " << (*it)->getConfidence() << ")" << endl; + // cout << "Skipping shape " << (*it)->getId() + // << " (confidence level: " << (*it)->getConfidence() << ")" << endl; } return false; } bool MapBuilder::determineNextGazePoint(vector& gazePts) { - HeadPointerMC headpointer_mc; for (vector::iterator it = gazePts.begin(); it != gazePts.end(); it++) { - cout << "look at pre-defined gazePt: " << *it << endl; nextGazePoint = *it; gazePts.erase(it); - return true; + if ( ! isBadGazePoint(nextGazePoint) ) + return true; } - /* - cout << "Gaze points exhausted; " << badGazePoints.size() << " bad gaze points were eliminated: " << endl; - for (vector::const_iterator bad_it = badGazePoints.begin(); - bad_it != badGazePoints.end(); bad_it++) - cout << *bad_it << " "; - cout << endl; - */ return false; } @@ -279,142 +310,172 @@ cout << "curReq == NULL in moveToNextGazePoint!" << endl; return; } - cout << "curReq == " << (unsigned int)curReq << endl; - const LocalMapRequest* locReq = dynamic_cast(curReq); - if ( locReq != NULL && locReq->manualHeadMotion && manualOverride==false ) { + if ( (verbosity & MBVnextGazePoint) || (curReq->manualHeadMotion && manualOverride==false) ) + cout << "moveToNextGazePoint " << nextGazePoint << endl; + if ( curReq->manualHeadMotion && manualOverride==false ) { cout << "To proceed to this gaze point: !msg MoveHead" << endl; return; } - storeImageAt(nextGazePoint); + else + storeImage(nextGazePoint,true); } +void MapBuilder::doNextSearch() { + LookoutSearchRequest *curLSR = curReq->worldTargets.front(); + curReq->worldTargets.pop(); + pointAtID = VRmixin::lookout.executeRequest(*curLSR); +} + bool MapBuilder::isBadGazePoint(const Point& Pt) const { const coordinate_t x = Pt.coordX(); const coordinate_t y = Pt.coordY(); - return ( x*x + y*y > maxDistSq || - x < -30.0); + return ( x*x + y*y > maxDistSq || x < -30.0); } -void MapBuilder::storeImageAt(const Point& pt) { - pointAtID = VRmixin::lookout.executeRequest(StoreImageAtRequest(pt)); - cout << "MapBuilder sent new storeImageAt request; pointAtID=" << pointAtID << endl; +void MapBuilder::storeImage(const Point& pt, bool havepoint) { + LookoutPointRequest lreq; + if ( havepoint ) + lreq.setTarget(pt); + else + lreq.setHeadMotionType(LookoutRequest::noMotion); + lreq.motionSettleTime = curReq->motionSettleTime; + lreq.numSamples = curReq->numSamples; + lreq.sampleInterval = curReq->sampleInterval; + pointAtID = VRmixin::lookout.executeRequest(lreq); } -void MapBuilder::storeImage() { - pointAtID = VRmixin::lookout.executeRequest(StoreImageRequest()); - cout << "MapBuilder sent new storeImage request; pointAtID=" << pointAtID << endl; +void MapBuilder::grabCameraImageAndGo() { + // This is a performance hack to avoid calling the Lookout or event + // router, so the MapBuilder can produce results very quickly when + // we need real-time performance, e.g., for particle filtering where + // we take multiple snapshots. + pointAtID = 0; + Sketch camFrame(VRmixin::sketchFromSeg()); +#ifdef TGT_HAS_CAMERA + const NEWMAT::Matrix camToBase = kine->jointToBase(CameraFrameOffset); +#else + NEWMAT::Matrix camToBase(4,4); + camToBase << ROBOOP::fourbyfourident; +#endif + LookoutSketchEvent dummy(true, camFrame, camToBase, + EventBase::lookoutEGID, pointAtID, EventBase::deactivateETID); + processImage(dummy); + requestComplete(); } -void MapBuilder::scan(ScanRequest& req) { - set colors; - for (colorMap::const_iterator it1 = curReq->objectColors.begin(); +void MapBuilder::scanForGazePts() { + LookoutScanRequest lreq; + lreq.searchArea = curReq->searchArea; + lreq.motionSettleTime = curReq->motionSettleTime; + set colors; // colors of all the shape types we're looking for + for (map >::const_iterator it1 = curReq->objectColors.begin(); it1 != curReq->objectColors.end(); it1++) - for (set::const_iterator it2 = it1->second.begin(); + for (set::const_iterator it2 = it1->second.begin(); it2 != it1->second.end(); it2++) colors.insert(*it2); - req.addTask(ScanRequest::VisionRegionTask(colors,M_PI/18)); - scanID = VRmixin::lookout.executeRequest(req); + lreq.addTask(LookoutScanRequest::VisionRegionTask(colors,curReq->dTheta)); + scanID = VRmixin::lookout.executeRequest(lreq); } void MapBuilder::extendLocal(const NEWMAT::Matrix& baseToCam) { - vector all = local.ShS.allShapes(); - removeNoise(local.ShS, baseToCam); - matchSrcToDst(groundShS,local.ShS,curReq->objectColors[polygonDataType]); - removeGazePts(local.gazePts, baseToCam); - local.baseToCamMats.push_back(baseToCam); + vector all = localShS.allShapes(); + removeNoise(localShS, baseToCam); + matchSrcToDst(groundShS,localShS,curReq->objectColors[polygonDataType]); + removeGazePts(curReq->gazePts, baseToCam); + curReq->baseToCamMats.push_back(baseToCam); } void MapBuilder::extendWorld(const NEWMAT::Matrix& baseToCam) { - if (!agentAtOrigin) { - world.ShS.applyTransform(worldToLocalTranslateMatrix); - world.ShS.applyTransform(worldToLocalRotateMatrix); - } - removeNoise(world.ShS, baseToCam); - matchSrcToDst(groundShS,world.ShS,curReq->objectColors[polygonDataType]); - if (!agentAtOrigin) - world.ShS.applyTransform(localToWorldMatrix); - removeGazePts(world.gazePts,baseToCam); - world.baseToCamMats.push_back(baseToCam); + worldShS.applyTransform(worldToLocalTranslateMatrix,egocentric); + worldShS.applyTransform(worldToLocalRotateMatrix,egocentric); + removeNoise(worldShS, baseToCam); + matchSrcToDst(groundShS,worldShS,curReq->objectColors[polygonDataType]); + worldShS.applyTransform(localToWorldMatrix,allocentric); + removeGazePts(curReq->gazePts,baseToCam); + curReq->baseToCamMats.push_back(baseToCam); } bool MapBuilder::requestExitTest() { - if (const LocalMapTestRequest* localTestReq = dynamic_cast(curReq)) - return ((*localTestReq->exitTest)()); - else if (const WorldMapTestRequest* worldTestReq = dynamic_cast(curReq)) - return ((*worldTestReq->exitTest)()); - else + if ( curReq->exitTest == NULL ) return false; + else + return (*curReq->exitTest)(); } void MapBuilder::requestComplete() { const unsigned int reqID = curReq->requestID; - cout << "MapBuilderRequest " << reqID << " complete\n"; + if ( verbosity & MBVcomplete ) + cout << "MapBuilderRequest " << reqID << " complete\n"; delete curReq; curReq = NULL; requests.pop(); + erouter->postEvent(EventBase::mapbuilderEGID, reqID, EventBase::deactivateETID,0); if ( requests.empty() ) - erouter->removeListener(this,EventBase::lookoutEGID); // don't need to listen to Lookout events when there is no request pending - erouter->postEvent(EventBase::mapbuilderEGID, reqID, EventBase::statusETID,0); - executeRequest(); // execute next request AFTER status event has finished processing + VRmixin::lookout.relax(); + else + executeRequest(); // execute next request AFTER deactivate event has finished processing } -void MapBuilder::setAgent(const Point &location, const AngTwoPi heading) { - theAgent->setCentroidPt(location); - theAgent->setOrientation(heading); - const coordinate_t dx = location.coordX(); - const coordinate_t dy = location.coordY(); - const coordinate_t dz = location.coordZ(); - float localToWorld[] = {cos(heading), -sin(heading), 0, dx, - sin(heading),cos(heading), 0, dy, - 0, 0, 1, dz, - 0 ,0, 0, 1}; +void MapBuilder::setAgent(const Point &worldLocation, const AngTwoPi worldHeading) { + if ( verbosity & MBVsetAgent ) + cout << "Agent now at " << worldLocation << " hdg " << worldHeading + << " (= " << float(worldHeading)*180/M_PI << " deg.)" << endl; + theAgent->setCentroidPt(worldLocation); + theAgent->setOrientation(worldHeading); + const coordinate_t dx = worldLocation.coordX(); + const coordinate_t dy = worldLocation.coordY(); + const coordinate_t dz = worldLocation.coordZ(); + float const c = cos(worldHeading); + float const s = sin(worldHeading); + float localToWorld[] = {c, -s, 0, dx, + s, c, 0, dy, + 0, 0, 1, dz, + 0 , 0, 0, 1}; float worldToLocalTrans[] = {1, 0, 0, -dx, 0, 1, 0, -dy, 0, 0, 1, -dz, - 0 ,0, 0, 1}; - float worldToLocalRotate[] = {cos(heading), sin(heading), 0, 0, - -sin(heading),cos(heading), 0, 0, - 0, 0, 1, 0, - 0 ,0, 0, 1}; + 0 ,0, 0, 1}; + float worldToLocalRotate[] = { c, s, 0, 0, + -s, c, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1}; localToWorldMatrix << localToWorld; worldToLocalTranslateMatrix << worldToLocalTrans; worldToLocalRotateMatrix << worldToLocalRotate; - agentAtOrigin = (heading == 0 || heading == 2*M_PI) && dx == 0 && dy == 0; } void MapBuilder::moveAgent(coordinate_t const local_dx, coordinate_t const local_dy, AngTwoPi dtheta) { + Point const agentLoc = theAgent->getCentroid(); AngTwoPi const heading = theAgent->getOrientation(); float const c = cos(heading); float const s = sin(heading); float const dx = local_dx*c + local_dy*-s; float const dy = local_dx*s + local_dy*c; - setAgent(Point(dx+theAgent->getCentroid().coordX(), - dy+theAgent->getCentroid().coordY(), - theAgent->getCentroid().coordZ()), - dtheta+heading); + setAgent(agentLoc + Point(dx,dy,agentLoc.coordZ(),allocentric), + heading+dtheta); } void MapBuilder::importLocalToWorld() { - if (!agentAtOrigin) { - world.ShS.applyTransform(worldToLocalTranslateMatrix); - world.ShS.applyTransform(worldToLocalRotateMatrix); - } - matchSrcToDst(local.ShS, world.ShS); - if (!agentAtOrigin) - world.ShS.applyTransform(localToWorldMatrix); + worldShS.applyTransform(worldToLocalTranslateMatrix,egocentric); + worldShS.applyTransform(worldToLocalRotateMatrix,egocentric); + matchSrcToDst(localShS,worldShS); + worldShS.applyTransform(localToWorldMatrix,allocentric); } -void MapBuilder::importWorldToLocal(const ShapeRoot &worldShape) { - ShapeRoot localShape(local.ShS.importShape(worldShape)); - localShape->applyTransform(worldToLocalTranslateMatrix); - localShape->applyTransform(worldToLocalRotateMatrix); +ShapeRoot MapBuilder::importWorldToLocal(const ShapeRoot &worldShape) { + ShapeRoot localShape(localShS.importShape(worldShape)); + localShape->applyTransform(worldToLocalTranslateMatrix,egocentric); + localShape->applyTransform(worldToLocalRotateMatrix,egocentric); + return localShape; } bool MapBuilder::isPointVisible(const Point &pt, const NEWMAT::Matrix& baseToCam, float maxDistanceSq) { NEWMAT::ColumnVector camCoords = baseToCam*pt.coords; - if (camCoords(3) <=0 || distSq(camCoords) >= maxDistanceSq) return false; + // if (camCoords(3) <=0 || distSq(camCoords) >= maxDistanceSq) return false; + if ( camCoords(1)*camCoords(1)+camCoords(2)*camCoords(2) >= maxDistanceSq ) + return false; float normX,normY; // normalized coordinates in cam frame config->vision.computePixel(camCoords(1),camCoords(2),camCoords(3),normX,normY); return (fabs(normX) < 0.9 && fabs(normY) < 0.9); //normX and normY range from -1 to 1. Giving 10% offset here @@ -481,15 +542,17 @@ const coordinate_t cenX = (*ground_it)->getCentroid().coordX(); const coordinate_t cenY = (*ground_it)->getCentroid().coordY(); if (cenX*cenX + cenY*cenY > maxDistSq) { // too far - cout << "ground shape " << (*ground_it)->getId() << "(lastMatch " - << (*ground_it)->getLastMatchId() << ") too far, delete\n"; + if ( verbosity & MBVnotAdmissible ) + cout << "ground shape " << (*ground_it)->getId() << " (lastMatch " + << (*ground_it)->getLastMatchId() << ") too far, delete\n"; ground_it->deleteShape(); } NEWMAT::ColumnVector coords = Kinematics::pack(cenX,cenY,(*ground_it)->getCentroid().coordZ()); coords = baseToCam*coords; if (coords(3) < 0) { // negative z-coordinate in camera frame indicates projection failed - cout << "Projection failed for ground shape " << (*ground_it)->getId() - << "(lastMatch " << (*ground_it)->getLastMatchId() << "), delete\n"; + if ( verbosity & MBVprojectionFailed ) + cout << "Projection failed for ground shape " << (*ground_it)->getId() + << " (lastMatch " << (*ground_it)->getLastMatchId() << "): deleting\n"; ground_it->deleteShape(); } // if a line is extending to maxDistance, chop it off at maxdistance and mark the endpoint invalid @@ -538,12 +601,12 @@ } } -void MapBuilder::calculateGroundPlane(const MapBuilderRequest::GroundPlaneAssumption_t& gpa) { - switch(gpa) { +void MapBuilder::calculateGroundPlane() { + switch(curReq->groundPlaneAssumption) { case MapBuilderRequest::onLegs: ground_plane << kine->calculateGroundPlane(); - cout << "Neck pan = " << state->outputs[HeadOffset+PanOffset] << endl; - cout << "Calculated ground plane: " << NEWMAT::printmat(ground_plane) << endl; + if ( verbosity & MBVgroundPlane ) + cout << "Calculated ground plane: " << NEWMAT::printmat(ground_plane) << endl; break; case MapBuilderRequest::onStand: #ifdef TGT_ERS210 @@ -553,7 +616,9 @@ #endif // cout << "Hard-coded ground plane: " << NEWMAT::printmat(ground_plane) << endl; break; - }; + case MapBuilderRequest::custom: + ground_plane = curReq->customGroundPlane; + } } void MapBuilder::projectToGround(const NEWMAT::Matrix& camToBase) { @@ -562,7 +627,7 @@ void MapBuilder::matchSrcToDst(ShapeSpace &srcShS, ShapeSpace &dstShS, - set polCols, bool mergeSrc, bool mergeDst) { + set polCols, bool mergeSrc, bool mergeDst) { vector src_shapes = srcShS.allShapes(); vector dst_shapes = dstShS.allShapes(); vector polygon_edges; @@ -571,8 +636,9 @@ for (vector::iterator src_it = src_shapes.begin(); src_it != src_shapes.end(); src_it++ ) { if (!(*src_it)->isAdmissible()) { - cout << "shape " << (*src_it)->getId() << "(lastMatch " - << (*src_it)->getLastMatchId() << ") is not admissible:" << endl; + if ( verbosity & MBVnotAdmissible ) + cout << "shape " << (*src_it)->getId() << "(lastMatch " + << (*src_it)->getLastMatchId() << ") is not admissible:" << endl; (*src_it)->printParams(); src_shapes.erase(src_it--); } @@ -582,7 +648,7 @@ src_shapes.erase(src_it--); } else if ((*src_it)->isType(lineDataType)) { - const int colorIndex = ProjectInterface::getColorIndex((*src_it)->getColor()); + const color_index colorIndex = ProjectInterface::getColorIndex((*src_it)->getColor()); if ( polCols.end() != find(polCols.begin(), polCols.end(), colorIndex)) { polygon_edges.push_back(ShapeRootTypeConst(*src_it, LineData).getData()); src_shapes.erase(src_it--); @@ -597,7 +663,8 @@ for ( vector::iterator it2 = it+1; it2 != src_shapes.end(); it2++) if ((*it2)->isMatchFor(*it) && (*it)->updateParams(*it2)) { - cout << "merging shape " << (*it)->getId() << " and shape " << (*it2)->getId() << endl; + if ( verbosity & MBVshapesMerge ) + cout << "merging shape " << (*it)->getId() << " and shape " << (*it2)->getId() << endl; src_shapes.erase(it2--); } @@ -612,8 +679,9 @@ src_it != src_shapes.end(); src_it++) if ((*dst_it)->isMatchFor(*src_it) && (*dst_it)->updateParams((*src_it))) { (*dst_it)->increaseConfidence(*src_it); - // cout << "Matched src shape " << (*src_it)->getId() << " (lastMatch " << (*src_it)->getLastMatchId() - // << ") to dst shape " << (*dst_it)->getId() << endl; + if ( verbosity & MBVshapeMatch ) + cout << "Matched src shape " << (*src_it)->getId() << " (lastMatch " << (*src_it)->getLastMatchId() + << ") to dst shape " << (*dst_it)->getId() << endl; src_shapes.erase(src_it--); } } @@ -628,8 +696,9 @@ delete_it->deleteShape(); dstShS.importShapes(newPolygons); dstShS.importShapes(src_shapes); - cout << "Imported " << (src_shapes.size()+newPolygons.size()) << " new shape(s) from " - << srcShS.name << "ShS to " << dstShS.name << "ShS\n"; + if ( verbosity & MBVimportShapes ) + cout << "Imported " << (src_shapes.size()+newPolygons.size()) << " new shape(s) from " + << srcShS.name << "ShS to " << dstShS.name << "ShS\n"; // match shapes inside dstShS if (mergeDst) { @@ -649,8 +718,8 @@ } } -// decrease confidence of those shapes should have been seen in the last snap, -// remove shapes from shapespace if confidence becomes <0 +// decrease confidence of those shapes that should have been seen in the last snap, +// remove shapes from ShapeSpace if confidence becomes < 0 void MapBuilder::removeNoise(ShapeSpace& ShS, const NEWMAT::Matrix& baseToCam) { // cout << "MapBuilder::removeNoise()\n"; vector shapes = ShS.allShapes(); @@ -668,25 +737,27 @@ for (vector::iterator edge_it = edges.begin(); edge_it != edges.end(); edge_it++, edge_index++) { if (isLineVisible(*edge_it, baseToCam)) { - cout << "edge " << edge_index << " of polygon " << (*it)->getId() << "(confidence: " - << edge_it->getConfidence() << ") should be visible in this frame" << endl; + if ( verbosity & MBVshouldSee ) + cout << "edge " << edge_index << " of polygon " << (*it)->getId() << "(confidence: " + << edge_it->getConfidence() << ") should be visible in this frame" << endl; edge_it->decreaseConfidence(); } } vector brokenPolygons = polygon->updateState(); ShS.importShapes(brokenPolygons); if (!polygon->isAdmissible()) { - cout << "delete polygon " << (*it)->getId() << " from map" << endl; + if ( verbosity & MBVdeleteShape ) + cout << "delete polygon " << (*it)->getId() << " from map" << endl; it->deleteShape(); } } else if ((!(*it)->isType(agentDataType)) && isShapeVisible(*it, baseToCam, maxDistSq)){ (*it)->decreaseConfidence(); // decrease confidence by 1 for every visible shape - cout << "shape " << (*it)->getId() << "(confidence: " << (*it)->getConfidence() - << ") should be visible in this frame" << endl; if ((*it)->getConfidence() < 0 ) { - cout << "confidence < 0, shape " << (*it)->getId() << " deleted from map" << endl; - // it->deleteShape(); + if ( verbosity & MBVshouldSee ) + cout << "shape " << (*it)->getId() << "(confidence: " << (*it)->getConfidence() + << ") should be visible in this frame; deleted" << endl; + it->deleteShape(); } } } @@ -694,132 +765,68 @@ //================ Gaze points ================ -void MapBuilder::defineGazePts(vector &gazePts, const ShapeRoot& area, bool doScan) { - cout << "MapBuilder::defineGazePts\n"; - static const float meshSize=50; - switch (area->getType()) { - case lineDataType: { - const Shape& line = ShapeRootTypeConst(area,LineData); - if (doScan) { - ScanAlongLineRequest req(line->end1Pt(),line->end2Pt()); - scan(req); - } - else { - int numIntervals = (int) (line->getLength()/meshSize); - const EndPoint& ep1 = line->end1Pt(); - const EndPoint& ep2 = line->end2Pt(); - gazePts.push_back(ep1); - for (int i = 1; i < numIntervals; i++) - gazePts.push_back((ep1*i + ep2*(numIntervals-i))/numIntervals); - gazePts.push_back(ep2); - } - } - break; - case blobDataType: { - const Shape& blob = ShapeRootTypeConst(area,BlobData); - if (doScan) { - ScanAreaRequest req(blob->topLeft.coordY(),blob->bottomRight.coordY(), - blob->topLeft.coordX(),blob->bottomRight.coordX()); - scan(req); - } - else { - vector corners; - corners.push_back(blob->topLeft); - corners.push_back(blob->topRight); - corners.push_back(blob->bottomRight); - corners.push_back(blob->bottomLeft); - defineGazePts(gazePts, corners, meshSize); - } - } - break; - case pointDataType: - gazePts.push_back(area->getCentroid()); - break; - case polygonDataType: { - const Shape &poly = ShapeRootTypeConst(area,PolygonData); - const vector &verts = poly->getVertices(); - gazePts.insert(gazePts.end(),verts.begin(),verts.end()); - } - break; - default: - cerr << "ERRORR MapBuilder::defineGazePts: Only types of area supported are Blob, Line, Point, and Polygon\n"; - requestComplete(); - break; - }; -} - -void MapBuilder::defineGazePts(vector& gazePts, const vector& corners, float meshSize) { - if (corners.size() != 4) { - cout << "Region of interest has to have four corners" << endl; +void MapBuilder::defineGazePts() { + const ShapeRoot &sArea = curReq->searchArea; + if ( !sArea.isValid() ) return; - } - for (vector::const_iterator it = corners.begin(); - it != corners.end()-1; it++) { - if (find(it+1, corners.end(), *it) != corners.end()) { - cout << "At least two corners are identical" << endl; - return; - } - } - LineData ln1(groundShS, corners.at(0),corners.at(1)); - LineData ln2(groundShS, corners.at(0),corners.at(2)); - const bool edge0_next_to_edge1 = !ln1.intersectsLine(LineData(groundShS, corners.at(2),corners.at(3))); - const bool edge0_next_to_edge2 = !ln2.intersectsLine(LineData(groundShS, corners.at(1),corners.at(3))); - vector horizontal, vertical; - - // passed corners may not be in the right order, sort them in clockwise or counterclockwise direction - const Point p0(corners.at(0)); - const Point p1(edge0_next_to_edge1 ? corners.at(1) : corners.at(3)); - const Point p2(edge0_next_to_edge1 ? (edge0_next_to_edge2 ? corners.at(3) : corners.at(2)) : corners.at(1)); - const Point p3(edge0_next_to_edge2 ? corners.at(2) : corners.at(3)); + else if ( curReq->doScan == true ) + scanForGazePts(); + else + switch ( sArea->getType() ) { - // define horizontal lines - Point p1_minus_p0 = p1-p0; - Point p2_minus_p3 = p2-p3; - const int num_steps_x = - (int) (sqrt(max(p1_minus_p0.coordX()*p1_minus_p0.coordX() + p1_minus_p0.coordY()*p1_minus_p0.coordY(), - p2_minus_p3.coordX()*p2_minus_p3.coordX() + p2_minus_p3.coordY()*p2_minus_p3.coordY())) - / meshSize) + 1; - p1_minus_p0 /= (float) num_steps_x; - p2_minus_p3 /= (float) num_steps_x; - - for (int i = 0; i <= num_steps_x; i++) - horizontal.push_back(LineData(groundShS, p0+p1_minus_p0*i, p3+p2_minus_p3*i)); + case pointDataType: + curReq->gazePts.push_back(sArea->getCentroid()); + break; - // define vertical lines - Point p3_minus_p0 = p3-p0; - Point p2_minus_p1 = p2-p1; - const int num_steps_y = - (int) (sqrt(max(p3_minus_p0.coordX()*p3_minus_p0.coordX() + p3_minus_p0.coordY()*p3_minus_p0.coordY(), - p2_minus_p1.coordX()*p2_minus_p1.coordX() + p2_minus_p1.coordY()*p2_minus_p1.coordY())) - / meshSize) + 1; - p3_minus_p0 /= (float) num_steps_y; - p2_minus_p1 /= (float) num_steps_y; + case lineDataType: { + static const float meshSize=50; + const Shape& line = ShapeRootTypeConst(sArea,LineData); + if ( curReq->doScan == true ) + scanForGazePts(); + else { + int numIntervals = (int) (line->getLength()/meshSize); + const EndPoint& ep1 = line->end1Pt(); + const EndPoint& ep2 = line->end2Pt(); + curReq->gazePts.push_back(ep1); + for (int i = 1; i < numIntervals; i++) + curReq->gazePts.push_back((ep1*i + ep2*(numIntervals-i))/numIntervals); + curReq->gazePts.push_back(ep2); + } + } + break; - for (int i = 0; i <= num_steps_y; i++) - vertical.push_back(LineData(groundShS, p0+p3_minus_p0*i, p1+p2_minus_p1*i)); - - // define check points = intersection of horizontal and vertical lines - // cout << "checkList: " << endl; - for (vector::const_iterator H_it = horizontal.begin(); - H_it != horizontal.end(); H_it++) - for (vector::const_iterator V_it = vertical.begin(); - V_it != vertical.end(); V_it++) - gazePts.push_back(H_it->intersectionWithLine(*V_it)); + case polygonDataType: + if ( curReq->doScan == true ) + scanForGazePts(); + else { + const Shape &poly = ShapeRootTypeConst(sArea,PolygonData); + const vector &verts = poly->getVertices(); + curReq->gazePts.insert(curReq->gazePts.end(),verts.begin(),verts.end()); + } + break; + + default: + cerr << "ERROR MapBuilder::defineGazePts: Supported searchArea shapes are Point, Line, and Polygon\n"; + requestComplete(); + break; + } } + void MapBuilder::removeGazePts(vector &gazePts, const NEWMAT::Matrix& baseToCam) { - if ( ! dynamic_cast(curReq)->removePts ) - return; - int num_points_seen = 0; - for ( vector::iterator it = gazePts.begin(); - it != gazePts.end(); it++ ) { - if (isPointVisible(*it, baseToCam, maxDistSq)) { - num_points_seen++; - gazePts.erase(it--); + if (curReq->removePts) { + int num_points_seen = 0; + for ( vector::iterator it = gazePts.begin(); + it != gazePts.end(); it++ ) { + if ( isPointVisible(*it,baseToCam,maxDistSq) ) { + cout << "Removing already-visible gaze point " << *it << endl; + num_points_seen++; + gazePts.erase(it--); + } } + // cout << num_points_seen << " pre-defined gaze points seen in last image, " + // << gazePts.size() << " left\n"; } - // cout << num_points_seen << " pre-defined gaze points seen in last snap, " - // << gazePts.size() << " left\n"; } @@ -854,7 +861,6 @@ //================ Shape extraction ================ - void MapBuilder::getCameraShapes(const Sketch& camFrame) { camShS.clear(); getCamLines(camFrame, curReq->objectColors[lineDataType], curReq->occluderColors[lineDataType]); @@ -862,63 +868,74 @@ getCamPolygons(camFrame, curReq->objectColors[polygonDataType], curReq->occluderColors[polygonDataType]); getCamSpheres(camFrame, curReq->objectColors[sphereDataType], curReq->occluderColors[sphereDataType]); getCamWalls(camFrame, curReq->floorColor); - BlobData::extractBlobs(camFrame, curReq->objectColors[blobDataType]); + if ( curReq->numSamples == 1 && !curReq->searchArea.isValid() ) // for fast blob extraction from current camera frame + getCamBlobs(); + else + getCamBlobs(camFrame, curReq->objectColors[blobDataType], curReq->minBlobAreas, curReq->blobOrientations, curReq->assumedBlobHeights); + getCamTargets(camFrame, curReq->objectColors[targetDataType], curReq->occluderColors[targetDataType]); } -vector > MapBuilder::getCamLines(const Sketch &camFrame, const set& objectColors, - const set& occluderColors) const { - vector > lines; - if (objectColors.empty() ) - return lines; - cout << "*** Find the lines ***" << endl; +vector > MapBuilder::getCamLines(const Sketch &camFrame, const set& objectColors, + const set& occluderColors) const { + vector > linesReturned; + if ( objectColors.empty() ) + return linesReturned; + if ( verbosity & MBVshapeSearch ) + cout << "*** Find the lines ***" << endl; NEW_SKETCH_N(occluders,bool,visops::zeros(camFrame)); - for ( set::const_iterator it = occluderColors.begin(); + for ( set::const_iterator it = occluderColors.begin(); it != occluderColors.end(); it++ ) occluders |= visops::minArea(visops::colormask(camFrame,*it)); - for (set::const_iterator it = objectColors.begin(); + for (set::const_iterator it = objectColors.begin(); it != objectColors.end(); it++) { NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it)); NEW_SKETCH_N(cleancolor,bool,visops::minArea(colormask)); NEW_SKETCH_N(fatmask,bool,visops::fillin(cleancolor,1,2,8)); NEW_SKETCH_N(skel,bool,visops::skel(fatmask)); - vector > line_shapes = LineData::extractLines(skel,cleancolor|occluders); - lines.insert(lines.end(), line_shapes.begin(), line_shapes.end()); - cout << "Found " << line_shapes.size() << " lines." << endl; + vector > line_shapes(LineData::extractLines(skel,cleancolor|occluders)); + linesReturned.insert(linesReturned.end(), line_shapes.begin(), line_shapes.end()); + if ( verbosity & MBVshapesFound ) + cout << "Found " << line_shapes.size() << " " + << ProjectInterface::getColorName(*it) << " lines." << endl; } - return lines; + return linesReturned; } vector > MapBuilder::getCamEllipses(const Sketch &camFrame, - const set& objectColors, const set& ) const { + const set& objectColors, const set& ) const { vector > ellipses; if (objectColors.empty()) return ellipses; - cout << "*** Find the ellipses ***" << endl; - for (set::const_iterator it = objectColors.begin(); + if ( verbosity & MBVshapeSearch ) + cout << "*** Find the ellipses ***" << endl; + for (set::const_iterator it = objectColors.begin(); it !=objectColors.end(); it++) { NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it)); vector > ellipse_shapes = EllipseData::extractEllipses(colormask); ellipses.insert(ellipses.end(), ellipse_shapes.begin(),ellipse_shapes.begin()); - cout << "Found " << ellipse_shapes.size() << " ellipses." << endl; + if ( verbosity & MBVshapesFound ) + cout << "Found " << ellipse_shapes.size() << " " + << ProjectInterface::getColorName(*it) << " ellipses." << endl; } return ellipses; } vector > MapBuilder::getCamPolygons(const Sketch &camFrame, - const set& objectColors, const set& occluderColors) const { + const set& objectColors, const set& occluderColors) const { vector > polygons; if ( objectColors.empty() ) return polygons; - cout << "*** Find the polygons ***" << endl; + if ( verbosity & MBVshapeSearch ) + cout << "*** Find the polygons ***" << endl; NEW_SKETCH_N(occluders,bool,visops::zeros(camFrame)); - for ( set::const_iterator it = occluderColors.begin(); + for ( set::const_iterator it = occluderColors.begin(); it !=occluderColors.end(); it++ ) occluders |= visops::colormask(camFrame,*it); - for (set::const_iterator it = objectColors.begin(); + for (set::const_iterator it = objectColors.begin(); it !=objectColors.end(); it++) { NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it)); NEW_SKETCH_N(fatmask,bool,visops::fillin(colormask,1,2,8)); @@ -927,24 +944,27 @@ vector > polygon_lines = PolygonData::extractPolygonEdges(fatskel,fatmask|occluders); polygons.insert(polygons.end(), polygon_lines.begin(), polygon_lines.end()); - cout << "Found " << polygon_lines.size() << " lines." << endl; + if ( verbosity & MBVshapesFound ) + cout << "Found " << polygon_lines.size() << " lines." << endl; } return polygons; } vector > MapBuilder::getCamSpheres(const Sketch &camFrame, - const set& objectColors, const set& ) const { + const set& objectColors, const set& ) const { vector > spheres; if ( objectColors.empty() ) return spheres; - cout << "*** Find the spheres ***" << endl; - for (set::const_iterator it = objectColors.begin(); + if ( verbosity & MBVshapeSearch ) + cout << "*** Find the spheres ***" << endl; + for (set::const_iterator it = objectColors.begin(); it !=objectColors.end(); it++) { NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,*it)); - vector sphere_shapes = SphereData::extractSpheres(colormask); + vector > sphere_shapes = SphereData::extractSpheres(colormask); spheres.insert(spheres.end(), spheres.begin(), spheres.end()); - cout << "Found " << sphere_shapes.size() << " spheres." << endl; + if ( verbosity & MBVshapesFound ) + cout << "Found " << sphere_shapes.size() << " spheres." << endl; } return spheres; } @@ -953,7 +973,8 @@ MapBuilder::getCamWalls(const Sketch &camFrame, unsigned int floorColor) const { if (floorColor == 0) return vector >(); - cout << "*** Find the walls ***" << endl; + if ( verbosity & MBVshapeSearch ) + cout << "*** Find the walls ***" << endl; const int camFrame_offset = 8; NEW_SKETCH_N(colormask,bool,visops::colormask(camFrame,floorColor)); @@ -988,32 +1009,82 @@ (*it)->end2Pt().setValid(false); } - cout << "Found " << wall_bounds.size() << " wall boundary lines" << endl; + if ( verbosity & MBVshapesFound ) + cout << "Found " << wall_bounds.size() << " wall boundary lines" << endl; return wall_bounds; } +void MapBuilder::getCamBlobs(const Sketch& camFrame, + const set& colors, + const map& minBlobAreas, + const map& blobOrientations, + const map& assumedBlobHeights) { + if ( colors.empty() ) + return; + int const maxblobs = 50; + vector > result(BlobData::extractBlobs(camFrame, colors, minBlobAreas, blobOrientations, assumedBlobHeights, maxblobs)); + if ( verbosity & MBVshapesFound ) + cout << "Found " << result.size() << " blobs." << endl; +} + +// The code below grabs blobs directly from the region generator stream, instead of +// calling CMVision to do region extraction on camFrame void MapBuilder::getCamBlobs() { if ( curReq->objectColors[blobDataType].empty() ) return; - cout << "*** Find the blobs ***" << endl; - int minarea; - BlobData::BlobOrientation_t orient; - set& blobColors = curReq->objectColors[blobDataType]; - // unsigned int const numBlobColors = blobColors.size(); - // for ( unsigned int i = 0; i::const_iterator it = blobColors.begin(); + if ( verbosity & MBVshapeSearch ) + cout << "*** Find the blobs ***" << endl; + const set& blobColors = curReq->objectColors[blobDataType]; + for (set::const_iterator it = blobColors.begin(); it != blobColors.end(); it++) { - // minarea = ( i < (curReq->minBlobAreas).size() ) ? (curReq->minBlobAreas)[i] : 0; - // minarea = ( i < (curReq->minBlobAreas).size() ) ? 1 : 0; - // orient = ( i < curReq->blobOrientations.size() ) ? curReq->blobOrientations[i] : BlobData::groundplane; - // vector > blob_shapes(VRmixin::getBlobsFromRegionGenerator(blobColors[i],minarea,orient)); - minarea = (curReq->minBlobAreas.find(*it)==curReq->minBlobAreas.end()) ? + int const minarea = (curReq->minBlobAreas.find(*it)==curReq->minBlobAreas.end()) ? 0 : curReq->minBlobAreas[*it]; - orient = (curReq->blobOrientations.find(*it)==curReq->blobOrientations.end()) ? + BlobData::BlobOrientation_t const orient = (curReq->blobOrientations.find(*it)==curReq->blobOrientations.end()) ? BlobData::groundplane : curReq->blobOrientations[*it]; - vector > blob_shapes(VRmixin::getBlobsFromRegionGenerator(*it,minarea,orient)); - cout << "Found " << blob_shapes.size() << " blobs." << endl; + coordinate_t const height = (curReq->assumedBlobHeights.find(*it)==curReq->assumedBlobHeights.end()) ? + 0 : curReq->assumedBlobHeights[*it]; + vector > blob_shapes(VRmixin::getBlobsFromRegionGenerator(*it,minarea,orient,height)); + if ( verbosity & MBVshapesFound ) + cout << "Found " << blob_shapes.size() << " " << ProjectInterface::getColorName(*it) << " blobs." << endl; } } +vector > +MapBuilder::getCamTargets(const Sketch &camFrame, const set& objectColors, const set& occluderColors) const { + vector > targets; + if (objectColors.empty()) + return targets; + if ( verbosity & MBVshapeSearch ) + cout << "*** Find the targets ***" << endl; + + NEW_SKETCH_N(occluders,bool,visops::zeros(camFrame)); + for (set::const_iterator it = occluderColors.begin(); + it != occluderColors.end(); it++) + occluders |= visops::minArea(visops::colormask(camFrame,*it)); + + // assumes multiples of 3 for objectColors (stays on the last color otherwise) + for (set::const_iterator it = objectColors.begin(); + it != objectColors.end(); it++) { + NEW_SKETCH_N(front_colormask, bool, visops::colormask(camFrame,*it)); + it++; + if (it == objectColors.end()) { + it--; + } + NEW_SKETCH_N(back_colormask, bool, visops::colormask(camFrame,*it)); + it++; + if (it == objectColors.end()) { + it--; + } + NEW_SKETCH_N(right_colormask, bool, visops::colormask(camFrame,*it)); + Shape target = TargetData::extractLineTarget(front_colormask, back_colormask, right_colormask, occluders); + if (target.isValid()) { + targets.insert(targets.end(), target); + } + if ( verbosity & MBVshapesFound ) + cout << "Found " << (target.isValid() ? 1 : 0) << " targets." << endl; + } + + return targets; +} + } // namespace diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/MapBuilder.h ./DualCoding/MapBuilder.h --- ../Tekkotsu_3.0/DualCoding/MapBuilder.h 2006-10-02 18:50:38.000000000 -0400 +++ ./DualCoding/MapBuilder.h 2007-11-11 18:57:21.000000000 -0500 @@ -7,291 +7,240 @@ #include "Behaviors/BehaviorBase.h" #include "Shared/newmat/newmat.h" +#include "Point.h" + #include "BlobData.h" +#include "EllipseData.h" #include "LineData.h" -#include "Point.h" +#include "SphereData.h" +#include "TargetData.h" + #include "VRmixin.h" -#include "LookoutRequests.h" -#include "MapBuilderRequests.h" +#include "MapBuilderRequest.h" #include "SketchTypes.h" #include "ShapeSpace.h" +#include "PolygonData.h" + +class LookoutSketchEvent; // note: this is NOT in the DualCoding namespace namespace DualCoding { class SketchSpace; -class ScanRequest; class MapBuilder : public BehaviorBase { protected: SketchSpace &camSkS; ShapeSpace &camShS, &groundShS; + SketchSpace &localSkS; + ShapeSpace &localShS; + SketchSpace &worldSkS; + ShapeSpace &worldShS; - struct maps { - enum Space { local, world } space; - SketchSpace &SkS; - ShapeSpace &ShS; - vector gazePts; - vector baseToCamMats; - maps(Space _space, SketchSpace& _SkS) : - space(_space), SkS(_SkS), ShS(_SkS.getDualSpace()), gazePts(), baseToCamMats() {} - } local, world, *cur; - -public: const int xres, yres; //!< width and height of camera frame + NEWMAT::ColumnVector ground_plane; //!< ground plane to which shapes are projected + static bool retain; //!< if true, VRmixin::stopCrew will not clear MapBuilder structures + +public: + //! Control whether static structures (mapbuilder, sketchGUI sockets, etc.) are retained + static void setRetain(bool r) { retain = r; } + static bool isRetained() { return retain; } + + typedef unsigned int MapBuilderVerbosity_t; + static const MapBuilderVerbosity_t MBVstart = 1<<0; + static const MapBuilderVerbosity_t MBVevents = 1<<1; + static const MapBuilderVerbosity_t MBVexecute = 1<<2; + static const MapBuilderVerbosity_t MBVcomplete = 1<<3; + static const MapBuilderVerbosity_t MBVdefineGazePoints = 1<<4; + static const MapBuilderVerbosity_t MBVnextGazePoint = 1<<5; + static const MapBuilderVerbosity_t MBVshapeSearch = 1<<6; + static const MapBuilderVerbosity_t MBVshapesFound = 1<<7; + static const MapBuilderVerbosity_t MBVgroundPlane = 1<<8; + static const MapBuilderVerbosity_t MBVprojectionFailed = 1<<9; + static const MapBuilderVerbosity_t MBVimportShapes = 1<<10; + static const MapBuilderVerbosity_t MBVnotAdmissible = 1<<11; + static const MapBuilderVerbosity_t MBVshapeMatch = 1<<12; + static const MapBuilderVerbosity_t MBVshapesMerge = 1<<13; + static const MapBuilderVerbosity_t MBVshouldSee = 1<<14; + static const MapBuilderVerbosity_t MBVdeleteShape = 1<<15; + static const MapBuilderVerbosity_t MBVsetAgent = 1<<16; + static const MapBuilderVerbosity_t MBVbadGazePoint = 1<<17; + +private: + static MapBuilderVerbosity_t verbosity; +public: + static void setVerbosity(MapBuilderVerbosity_t v) { verbosity = v; } + protected: - Shape &theAgent; //!< agent in the world frame - //! trasformation matrices between local and world frames. + friend class Lookout; + + Shape &theAgent; //!< Agent in the world frame + //!@name Transformation matrices between local and world frames + //@{ NEWMAT::Matrix localToWorldMatrix, worldToLocalTranslateMatrix, worldToLocalRotateMatrix; + //@} - vector badGazePoints; //!< gaze points for which HeadPointerMC.lookAtPoint() returned false - bool agentAtOrigin; //!< whether or not agent is at origin and orientation is zero or two pi. + std::vector badGazePoints; //!< gaze points for which HeadPointerMC.lookAtPoint() returned false - queue requests; + std::queue requests; MapBuilderRequest *curReq; unsigned int idCounter; - unsigned int maxDistSq; //!< sqrt of current request's max distance parameter + float maxDistSq; //!< square of current request's max distance parameter unsigned int pointAtID, scanID; //!< ID's for lookout requests Point nextGazePoint; - - //! posts completion event and deletes current request, executes next request if there is one - void requestComplete(); - //! triggers action to execute the front one in requests queue + //! Triggers action to execute the request at the front of the queue void executeRequest(); //! calls exitTest of current request if there is one and returns the result bool requestExitTest(); + //! posts completion event and deletes current request, executes next request if there is one + void requestComplete(); public: MapBuilder(); //!< Constructor virtual ~MapBuilder() {} //!< Destructor virtual void DoStart(); virtual void DoStop(); - virtual void processEvent(const EventBase&); virtual std::string getDescription() const { return "MapBuilder"; } void printShS(ShapeSpace&) const; - unsigned int executeRequest(const MapBuilderRequest& req); + unsigned int executeRequest(const MapBuilderRequest&, unsigned int *req_id=NULL); // execute a MapBuilder request, and optionally store the request id in a variable; the id will be returned in any case - void processImage(const Sketch&, const NEWMAT::Matrix& camToBase, const NEWMAT::Matrix& baseToCam); + virtual void processEvent(const EventBase&); + void processImage(const LookoutSketchEvent&); - // returns if a ground shape should be seen in the current camera frame + // Returns true if a ground shape should be seen in the current camera frame static bool isPointVisible(const Point &pt, const NEWMAT::Matrix& baseToCam, float maxDistanceSq) ; static bool isLineVisible(const LineData& ln, const NEWMAT::Matrix& baseToCam); static bool isShapeVisible(const ShapeRoot &ground_shape, const NEWMAT::Matrix& baseToCam, float maxDistanceSq); - //!@ utility functions which may be used by MapBuilderRequests' exit condition and others + //! utility functions which may be used by MapBuilderRequest's exit condition and others + //@{ const Shape& getAgent() const { return theAgent; } // sets the agent location and heading - void setAgent(const Point &location, const AngTwoPi heading); + void setAgent(const Point &worldLocation, const AngTwoPi worldHeading); // updates the agent location and heading void moveAgent(coordinate_t const local_dx, coordinate_t const local_dy, AngTwoPi dtheta); + //@} - vector getShapes(const ShapeSpace& ShS, int minConf=2) const { - const vector allShapes = ShS.allShapes(); + std::vector getShapes(const ShapeSpace& ShS, int minConf=2) const { + const std::vector allShapes = ShS.allShapes(); if (&ShS == &camShS || &ShS == &groundShS || minConf <= 0) return allShapes; - vector nonNoiseShapes; - for (vector::const_iterator it = allShapes.begin(); + std::vector nonNoiseShapes; + for (std::vector::const_iterator it = allShapes.begin(); it != allShapes.end(); it++) if ((*it)->getConfidence() >= minConf) nonNoiseShapes.push_back(*it); return nonNoiseShapes; } - const vector& getGazePts() const { return cur->gazePts; } - static bool returnTrue() { return true; } - static bool returnFalse() { return false; } - //}@ + void importLocalToWorld(); - //!@ Shape extraction functions - vector > - getCamLines(const Sketch&, const set& objectColors, - const set& occluderColors) const ; - vector > - getCamPolygons(const Sketch&, const set& objectColors, - const set& occluderColors) const ; - vector > - getCamEllipses(const Sketch&, const set& objectColors, - const set& occluderColors) const ; - vector > - getCamSpheres(const Sketch&, const set& objectColors, - const set& occluderColors) const ; - vector > - getCamWalls(const Sketch&, unsigned int) const ; - vector > - getCamBlobs(const Sketch& sketch, const set& objectColors) const { - return BlobData::extractBlobs(sketch, objectColors); - } - //}@ + ShapeRoot importWorldToLocal(const ShapeRoot &worldShape); + template Shape importWorldToLocal(const Shape &worldShape); - static Shape formBoundingBox(coordinate_t left, coordinate_t right, - coordinate_t front, coordinate_t rear) { - return Shape - (new BlobData(VRmixin::groundShS, Point(front,left), Point(front,right), - Point(rear,right), Point(rear,left), fabs((left-right)*(front-rear)), - vector(), BlobData::groundplane, rgb())); - } +protected: + //!@name Shape extraction functions + //@{ + void getCameraShapes(const Sketch& camFrame); + + std::vector > + getCamLines(const Sketch&, const std::set& objectColors, + const std::set& occluderColors) const; + + std::vector > + getCamEllipses(const Sketch&, const std::set& objectColors, + const std::set& occluderColors) const; + + std::vector > + getCamPolygons(const Sketch&, const std::set& objectColors, + const std::set& occluderColors) const; + + std::vector > + getCamWalls(const Sketch&, unsigned int) const; + + std::vector > + getCamSpheres(const Sketch&, const std::set& objectColors, + const std::set& occluderColors) const; + + void getCamBlobs(const Sketch& camFrame, + const std::set& colors, + const std::map& minBlobAreas, + const std::map& blobOrientations, + const std::map& assumedBlobHeights); + void getCamBlobs(); + + std::vector > + getCamTargets(const Sketch &camFrame, const std::set& objectColors, + const std::set& occluderColors) const; + //@} - void importLocalToWorld(); - void importWorldToLocal(const ShapeRoot &shape); // matching shapes between two spaces. - static void matchSrcToDst(ShapeSpace &src, ShapeSpace &dst, set polygonEdgeColors=set(), + static void matchSrcToDst(ShapeSpace &src, ShapeSpace &dst, std::set polygonEdgeColors=std::set(), bool mergeSrc=true, bool mergeDst=true); -protected: - //! functions to make requests to lookout - void scan(ScanRequest& req); - void storeImageAt(const Point& pt); - void storeImage(); + //!@name Functions to make requests to the Lookout + //@{ + void storeImage() { storeImage(Point(),false); } + void storeImage(const Point& pt, bool havepoint=true); + void grabCameraImageAndGo(); + void scanForGazePts(); + //@} //! define gazePts either virtually or by scan - void defineGazePts(vector&, const ShapeRoot& area, bool doScan);// {} - void defineGazePts(vector& gazePts, const vector& corners, float meshSize); + void defineGazePts(); - void getCameraShapes(const Sketch& camFrame); - void getCamBlobs(); - void extendLocal(const NEWMAT::Matrix& baseToCam); void extendWorld(const NEWMAT::Matrix& baseToCam); //! decrement confidence of shapes which should have been seen according to the baseToCam matrix void removeNoise(ShapeSpace&, const NEWMAT::Matrix& baseToCam); //! erase gaze points which should have been seen according to the baseToCam matrix - void removeGazePts(vector&, const NEWMAT::Matrix& baseToCam); + void removeGazePts(std::vector&, const NEWMAT::Matrix& baseToCam); //! Returns true if it has set up a valid next gaze point in nextGazePoint bool determineNextGazePoint(); //! Returns true if there is a shape which needs be looked at again and is reachable; sets it up as nextGazePoint - bool determineNextGazePoint(const vector&); - // Returns true if an element of gazePt can be looked at; sets it up as nextGazePoint - bool determineNextGazePoint(vector &gazePts); + bool determineNextGazePoint(const std::vector&); + // Returns true if an element of gazePts can be looked at; sets it up as nextGazePoint + bool determineNextGazePoint(std::vector &gazePts); //! Starts robot moving to the next gaze point void moveToNextGazePoint(const bool manualOverride=false); + void doNextSearch(); + void doNextSearch2(); // operations in ground shape space bool isBadGazePoint(const Point&) const ; void projectToGround(const NEWMAT::Matrix& camToBase); void filterGroundShapes(const NEWMAT::Matrix& baseToCam); - // calculates ground place based on ground plane assumption type - void calculateGroundPlane(const MapBuilderRequest::GroundPlaneAssumption_t& gpa); + // calculates ground plane based on ground plane assumption type + void calculateGroundPlane(); private: MapBuilder(const MapBuilder&); //!< never call this MapBuilder& operator=(const MapBuilder&); //!< never call this }; -/* - -Functions of the MapBuilder subsystem: - -1. Given a series of camera views from the same body position (but -varying head positions), assemble them into a local world view. This -requires: - - a) Matching functions to establish correspondence between - groundspace objects and local worldspace objects. This is - tricky when lines have missing endpoints, or ellipses lie - partially offscreen. - - Matching algorithm: - do ellipses first -- they're simpler: just match color, center point - - Line matching: - pick a starting line (longest first) in groundShS - then find all colinear line segments in groundShS matching its rho/theta - do the same in localworldShS - now a smart algorithm can assemble line segments into a more - complex match than just 1-1 - but for starters can code just a 1-1 match - - Find nearby ellipses in ground and world space and cluster them together - before matching. This will allow us to handle piles of free game pieces. - - Make a list of ambiguous points that we would like to examine further - by taking another image, moving the head and maybe even the body. - - Idea: we have to deal with lots of ambiguity in matching camera frames - to the local world map, and may want to confirm some things with - additional camera frames if we suspect noise. But the world map should - be considered to be reliable so we shouldn't need the same kind of - expensive tentative matching there. However, we may want to allow for - "hints" of things to be put in the world map if we're not certain of our - data. For example, a far off ellipse may be of indeterminate size and - orientation, so put it in the world map as a hint-of-ellipse and let the - robot wander over there and get a close up view if it wants to resolve - whether this is a real ellipse or noise. - - Can use distance from the camera as a parameter to control how we - treat an object; small, far away objects should be treated as less reliable. - Can keep counts on how many frames an object was seen or not seen, and - delete objects as spurious if they were only seen once or twice and - then not seen in a while. - - Unmatched object: to know that a mobile object has disappeared, we - need to recognize that we should see it but we don't. How? For each - camera frame, project the four corners to ground, yielding a trapezoid. - As we build the local world map we have a collection of trapezoids. For - any object in the global map that isn't matched by something in the local, - we can check its coordinates to see if it falls within one of the - trapizeoids. If so, then we looked and didn't see it, so it's gone. - - b) Update functions that update local worldspace object descriptions - once correspondences have been determined. - -2. Given a local worldspace map and a global worldspace map, and -possibly an estimate of current position and orientation on the -latter, determine the robot's true position and orientation in the -world, and update the global worldspace map. This requires: - - a) A search method to find the best translation and rotation of - the local worldspace map to fit the global worldspace map. - A particle filter looks like a good candidate method. - - b) Update functions that update the global worldspace description - once correspondences have been determined. - - c) A mechanism for recognizing when the world has changed, e.g., - objects have been added, deleted, or moved. This requires: - - i) Designation of objects as fixed or mobile - - ii) Declaration of objects as unique, of a fixed number, - or multitudinous. Unique fixed objects are the best - landmarks. Multitudinous mobile objects may need to - be monitored more frequently. - -3. Given a request to self-localize, acquire the minimum necessary -number of local views to perform that function. This requires: - - a) Designation of objects to be used as beacons or key landmarks. - - b) Head motion constraints. - - c) Body motion constraints (possibly "no body motion allowed".) - -Ideally, rather than directly executing motion commands, the system -should pass its requests to look in certain directions to the -affordance generator, which will figure out what is feasible, e.g., if -we're up against a wall and can't turn in that direction, we may have -to take a step sideways first. Let the affordance system worry abou -that. - -4. Given a prototype global world map description, visually explore -the world and instantiate the prototype as an actual global world map. -For example, we might define a tic-tac-toe environment as containing -two sets of perpendicular lines defining the game board, plus a pool -of free pieces. We may not know the exact lengths of the lines, or -their orientation, or the location of the pool relative to the board; -these need to be determined by observation. We may not even know the -color of the lines, in which case we'll have to look for a contrast -difference, but that is too advanced to worry about at present. +template Shape MapBuilder::importWorldToLocal(const Shape &worldShape) { + ShapeRoot temp(localShS.importShape(worldShape)); + Shape localShape(ShapeRootType(temp,T)); + localShape->applyTransform(worldToLocalTranslateMatrix); + localShape->applyTransform(worldToLocalRotateMatrix); + return localShape; +} -*/ +//! Utility function for deleting queues of pointers to Lookout or MapBuilder requests +template void deleteAll(std::queue &q) { + while ( ! q.empty() ) { + delete q.front(); + q.pop(); + } +} } // namespace diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/MapBuilderRequest.h ./DualCoding/MapBuilderRequest.h --- ../Tekkotsu_3.0/DualCoding/MapBuilderRequest.h 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/MapBuilderRequest.h 2007-11-11 18:57:21.000000000 -0500 @@ -0,0 +1,118 @@ +//-*-c++-*- +#ifndef INCLUDED_MapBuilderRequest_h_ +#define INCLUDED_MapBuilderRequest_h_ + +#include + +#include "ShapeTypes.h" + +namespace DualCoding { + +class LookoutSearchRequest; + +class MapBuilderRequest { + friend class MapBuilder; + +public: + enum MapBuilderRequestType_t { + cameraMap, + groundMap, + localMap, + worldMap + }; + + MapBuilderRequestType_t requestType; //!< Type of map to build + std::map > objectColors; //!< For each object type, a set of object color indices + std::map > occluderColors; //!< For each object type, a set of occluder color indices + std::map minBlobAreas; //!< Minimum acceptable areas for blobs of specified colors, e.g., req.minBlobAreas[pink_index]=50 + std::map blobOrientations; //!< Default orientation for blobs of specified colors + std::map assumedBlobHeights; //!< Fixed heights for pillar or poster blobs of specified colors + unsigned int floorColor; + unsigned int motionSettleTime; //!< Time in msec to wait before taking measurements or throwing completion event after head reaches gazePt. + int numSamples; //!< Number of camera images to combine, for noise reduction + int sampleInterval; //!< Interval in msec between successive samples + float maxDist; //!< Ignore objects farther than this distance + bool clearShapes; //!< If true, clear the shape space at start of request + bool pursueShapes; //!< If true, generate new gaze points as shapes are recognized + bool manualHeadMotion; //!< If true, waits for !msg MoveHead before moving to next gaze point (for debugging) + bool rawY; //!< If true, leave an intensity (Y-channel) image in camera space for debugging + bool removePts; //!< If true, remove pending gaze points if they're visible in current image + bool doScan; //!< If true, do a continuous scan of the area to find interest points to be examined + AngPi dTheta; //!< Angular step for scanning + ShapeRoot searchArea; //!< The area to search, in egocentric coords + std::queue worldTargets; //!< Queue of search requests for world targets + void (*userCamProcessing)(); //!< User routine to call after cam space processing + void (*userGroundProcessing)(); //!< User routine to call after ground space processing + void (*userLocalProcessing)(); //!< User routine to call after local space processing + void (*userWorldProcessing)(); //!< User routine to call after world space processing + bool (*exitTest)(); //!< If true, terminate map building and post completion event + enum GroundPlaneAssumption_t { onStand, onLegs, custom } groundPlaneAssumption; + NEWMAT::ColumnVector customGroundPlane; //!< User-supplied ground plane +private: + std::vector gazePts; + std::vector baseToCamMats; + unsigned int requestID; //!< Set by mapbuilder when the request is added to its request queue + +public: + //! Constructor + MapBuilderRequest(MapBuilderRequestType_t reqtype=cameraMap) + : requestType(reqtype), objectColors(), occluderColors(), + minBlobAreas(), blobOrientations(), assumedBlobHeights(), floorColor(0), + motionSettleTime(1000), numSamples(1), sampleInterval(0), maxDist(1e10), + clearShapes(true), pursueShapes(false), manualHeadMotion(false), rawY(true), removePts(true), + doScan(false), dTheta(M_PI/18), + searchArea(), worldTargets(), + userCamProcessing(NULL), userGroundProcessing(NULL), + userLocalProcessing(NULL), userWorldProcessing(NULL), + exitTest(NULL), + groundPlaneAssumption(onLegs), customGroundPlane(4), + gazePts(), baseToCamMats(), + requestID(0) + {} + + //! Copy constructor + MapBuilderRequest(const MapBuilderRequest &req) + : requestType(req.requestType), + objectColors(req.objectColors), occluderColors(req.occluderColors), + minBlobAreas(req.minBlobAreas), blobOrientations(req.blobOrientations), assumedBlobHeights(req.assumedBlobHeights), + floorColor(req.floorColor), motionSettleTime(req.motionSettleTime), + numSamples(req.numSamples), sampleInterval(req.sampleInterval), + maxDist(req.maxDist), clearShapes(req.clearShapes), pursueShapes(req.pursueShapes), + manualHeadMotion(req.manualHeadMotion), rawY(req.rawY), removePts(req.removePts), + doScan(req.doScan), dTheta(req.dTheta), + searchArea(req.searchArea), worldTargets(req.worldTargets), + userCamProcessing(req.userCamProcessing), + userGroundProcessing(req.userGroundProcessing), + userLocalProcessing(req.userLocalProcessing), + userWorldProcessing(req.userWorldProcessing), + exitTest(req.exitTest), + groundPlaneAssumption(req.groundPlaneAssumption), + customGroundPlane(req.customGroundPlane), + gazePts(req.gazePts), baseToCamMats(req.baseToCamMats), + requestID(req.requestID) + {} + + virtual ~MapBuilderRequest() {} //!< Destructor + + MapBuilderRequestType_t getRequestType() const { return requestType; } + + void setCustomGroundPlane(float const angle, float const height) { + float const s = std::sin(-angle); + float const c = std::cos(-angle); + customGroundPlane = Kinematics::pack(s, 0, c, -height * c); + groundPlaneAssumption = custom; + } + + //! Returns true if the request can complete immediately (no head motion required and numSamples <= 1) + bool immediateRequest() const { + return !searchArea.isValid() && worldTargets.size() == 0 && !pursueShapes && !doScan && numSamples <= 1; + } + +private: + MapBuilderRequest& operator=(const MapBuilderRequest& req); + +}; + +} // namespace + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/MapBuilderRequests.h ./DualCoding/MapBuilderRequests.h --- ../Tekkotsu_3.0/DualCoding/MapBuilderRequests.h 2006-09-23 23:44:30.000000000 -0400 +++ ./DualCoding/MapBuilderRequests.h 1969-12-31 19:00:00.000000000 -0500 @@ -1,202 +0,0 @@ -//-*-c++-*- -#ifndef INCLUDED_MapBuilderRequests_h_ -#define INCLUDED_MapBuilderRequests_h_ - -#include "BlobData.h" -#include "ShapeTypes.h" -#include "VRmixin.h" - -namespace DualCoding { - -class MapBuilderRequest { - typedef map > colorMap; - friend class MapBuilder; - -public: - enum MapBuilderRequestType_t { - none, - takeSnap, - localMap, - worldMap - }; - - virtual MapBuilderRequestType_t getRequestType() const=0; - - virtual ~MapBuilderRequest() {} - - unsigned int requestID; // set by mapbuilder when added to request queue - colorMap objectColors, occluderColors; - map minBlobAreas; - map blobOrientations; - unsigned int floorColor; - unsigned int numImages; - enum GroundPlaneAssumption_t { onStand, onLegs } groundPlaneAssumption; - - //! Constructor - MapBuilderRequest(unsigned int num=1) - : requestID(0), objectColors(), occluderColors(), - minBlobAreas(), blobOrientations(), floorColor(0), - numImages(num), groundPlaneAssumption(onLegs) {} - - //! Copy constructor - MapBuilderRequest(const MapBuilderRequest& req) - : requestID(req.requestID), - objectColors(req.objectColors), occluderColors(req.occluderColors), - minBlobAreas(req.minBlobAreas), blobOrientations(req.blobOrientations), - floorColor(req.floorColor), numImages(req.numImages), - groundPlaneAssumption(req.groundPlaneAssumption) {} - -private: - MapBuilderRequest& operator=(const MapBuilderRequest& req); -}; - -class TakeSnapRequest : public MapBuilderRequest { -public: - virtual MapBuilderRequestType_t getRequestType() const { return takeSnap; } - TakeSnapRequest(unsigned int num=1) - : MapBuilderRequest(num) {} - TakeSnapRequest(const TakeSnapRequest& req) - : MapBuilderRequest(req) {} -}; - -class TakeSnapAtRequest : public TakeSnapRequest { -public: - TakeSnapAtRequest(const Point& pt, unsigned int num=1) - : TakeSnapRequest(num), gazePt(pt) {} - TakeSnapAtRequest(const TakeSnapAtRequest& req) - : TakeSnapRequest(req), gazePt(req.gazePt) {} - Point gazePt; -}; - - -class LocalMapRequest : public MapBuilderRequest { -public: - virtual MapBuilderRequestType_t getRequestType() const { return localMap; } - - //! Constructor with bounding box - LocalMapRequest(coordinate_t maxX, coordinate_t minX, coordinate_t maxY, - coordinate_t minY, unsigned int _maxDist, unsigned int num=1, - bool clear=true, bool _doScan=true) - : MapBuilderRequest(num), shs(VRmixin::localShS), - area(Shape - (new BlobData - (VRmixin::groundShS,Point(maxX,maxY),Point(maxX,minY), - Point(minX,maxY),Point(minX,minY),(maxX-minX)*(maxY-minY), - vector(),BlobData::groundplane,rgb(0,0,0)))), - maxDist(_maxDist), clearShapes(clear), pursueShapes(true), manualHeadMotion(false), - removePts(true), doScan(_doScan) {} - - //! Constructor with point target - LocalMapRequest(const Point& pt, unsigned int _maxDist, unsigned int num=1, - bool clear=true, bool _doScan=true) - : MapBuilderRequest(num), shs(VRmixin::localShS), - area(Shape(new PointData(VRmixin::groundShS,pt))), - maxDist(_maxDist), clearShapes(clear), pursueShapes(true), manualHeadMotion(false), - removePts(true), doScan(_doScan) {} - - //! Constructor with ShapeRoot to define scan region - LocalMapRequest(const ShapeRoot _area, unsigned int _maxDist, unsigned int num=1, - bool clear=true, bool _doScan=true) - : MapBuilderRequest(num), shs(VRmixin::localShS), - area(_area), maxDist(_maxDist), clearShapes(clear), pursueShapes(true), manualHeadMotion(false), - removePts(true), doScan(_doScan) {} - - //! Copy constructor - LocalMapRequest(const LocalMapRequest& req) - : MapBuilderRequest(req), shs(req.shs), area(req.area), maxDist(req.maxDist), - clearShapes(req.clearShapes), pursueShapes(req.pursueShapes), manualHeadMotion(req.manualHeadMotion), - removePts(req.removePts), doScan(req.doScan) {} - - virtual ~LocalMapRequest() {} - - ShapeSpace& shs; - ShapeRoot area; //!< The area to search, in egocentric coords - unsigned int maxDist; //!< Ignore objects farther than this distance - bool clearShapes; //!< If true, clear the shape space at start of request - bool pursueShapes; //!< If true, generate new gaze points as shapes are recognized - bool manualHeadMotion; //!< If true, waits for !msg MoveHead before moving to next gaze point (for debugging) - bool removePts; //!< If true, remove pending gaze points if they're visible in current image - bool doScan; //!< If true, do a continuous scan of the area to find interest points to be examined - -protected: - //! Constructor used by subclass WorldMapRequest - LocalMapRequest(ShapeSpace& _shs, const ShapeRoot& _area, unsigned int _maxDist, - unsigned int num=1, bool clear=true, bool _doScan=true) - : MapBuilderRequest(num), shs(_shs), area(_area), - maxDist(_maxDist), clearShapes(clear), pursueShapes(true), manualHeadMotion(false), - removePts(true), doScan(_doScan) {} - - //! Constructor used by subclass WorldMapRequest - LocalMapRequest(ShapeSpace& _shs, coordinate_t maxX, coordinate_t minX, - coordinate_t maxY, coordinate_t minY, unsigned int _maxDist, - unsigned int num=1, bool clear=true, bool _doScan=true) - : MapBuilderRequest(num), shs(_shs), - area(Shape - (new BlobData - (VRmixin::groundShS,Point(maxX,maxY),Point(maxX,minY), - Point(minX,maxY),Point(minX,minY),(maxX-minX)*(maxY-minY), - vector(),BlobData::groundplane,rgb(0,0,0)))), - maxDist(_maxDist), clearShapes(clear), pursueShapes(true), manualHeadMotion(false), - removePts(true), doScan(_doScan) {} -}; - -class LocalMapTestRequest : public LocalMapRequest { -public: - LocalMapTestRequest(bool (*func)(), coordinate_t maxX, coordinate_t minX, - coordinate_t maxY, coordinate_t minY, unsigned int _maxDist, - unsigned int num=1, bool clear=true, bool _doScan=true) - : LocalMapRequest(maxX,minX,maxY,minY,_maxDist,num,clear,_doScan), exitTest(func){} - - LocalMapTestRequest(const LocalMapTestRequest& req) - : LocalMapRequest(req), exitTest(req.exitTest) {} - - virtual ~LocalMapTestRequest() {} - - bool (*exitTest)(); - -private: - LocalMapTestRequest& operator=(const LocalMapTestRequest&); -}; - -class WorldMapRequest : public LocalMapRequest { -public: - virtual MapBuilderRequestType_t getRequestType() const { return worldMap; } - - //! Constructor using a shape to specify area - WorldMapRequest(const ShapeRoot& _area, unsigned int _maxDist, - unsigned int num=1, bool clear=true, bool _doScan=true) - : LocalMapRequest(VRmixin::worldShS,_area,_maxDist,num,clear,_doScan) {} - - //! Constructor using a bounding box - WorldMapRequest(coordinate_t maxX, coordinate_t minX, coordinate_t maxY, - coordinate_t minY, unsigned int _maxDist, - unsigned int num=1, bool clear=true, bool _doScan=true) - : LocalMapRequest(VRmixin::worldShS,maxX,minX,maxY,minY,_maxDist,num,clear,_doScan) {} - - //! Copy constructor - WorldMapRequest(const WorldMapRequest& req) : LocalMapRequest(req) {} - - virtual ~WorldMapRequest() {} -}; - -class WorldMapTestRequest : public WorldMapRequest { -public: - // virtual MapBuilderRequestType_t getRequestType() const { return worldMapTest; } - WorldMapTestRequest(bool (*func)(), const ShapeRoot& _area, unsigned int _maxDist, - unsigned int num=1, bool clear=true, bool _doScan=true) - : WorldMapRequest(_area,_maxDist,num,clear,_doScan), exitTest(func){} - WorldMapTestRequest(bool (*func)(), coordinate_t maxX, coordinate_t minX, - coordinate_t maxY, coordinate_t minY, unsigned int _maxDist, - unsigned int num=1, bool clear=true, bool _doScan=true) - : WorldMapRequest(maxX,minX,maxY,minY,_maxDist,num,clear,_doScan), exitTest(func){} - WorldMapTestRequest(const WorldMapTestRequest& req) - : WorldMapRequest(req), exitTest(req.exitTest) {} - virtual ~WorldMapTestRequest() {} - bool (*exitTest)(); -private: - WorldMapTestRequest& operator=(const WorldMapTestRequest&); -}; - -} // namespace - -#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/Measures.cc ./DualCoding/Measures.cc --- ../Tekkotsu_3.0/DualCoding/Measures.cc 2006-03-21 16:03:48.000000000 -0500 +++ ./DualCoding/Measures.cc 1969-12-31 19:00:00.000000000 -0500 @@ -1,72 +0,0 @@ -#include "Measures.h" - -namespace DualCoding { - -AngPi::AngPi(orientation_t const &v) : value(v) { - // Do cheap tests first; use fmod only if necessary. - if ( value < 0 ) - value += Pi; - else if ( value >= Pi) - value -= Pi; - else return; - // If we're still out of range, give up and call fmod. - if ( value < 0 || value >= Pi) { - value = fmod(value,Pi); - if ( value < 0 ) - value += Pi; - }; -} - - -AngPi angdist(AngPi const &arg1, AngPi const &arg2) { - AngPi diff = arg1.value - arg2.value; - if ( diff > Pi/2 ) - diff = Pi - diff; - return diff; -} - -AngTwoPi::AngTwoPi(direction_t const &v) : value(v) { - // Do cheap tests first; use fmod only if necessary. - if ( value < 0 ) - value += TwoPi; - else if ( value >= TwoPi) - value -= TwoPi; - else return; - // If we're still out of range, give up and call fmod. - if ( value < 0 || value >= TwoPi) { - value = fmod(value,TwoPi); - if ( value < 0 ) - value += TwoPi; - }; -} - -AngPi angdist(AngTwoPi const &arg1, AngTwoPi const &arg2) { - AngTwoPi diff = arg1.value - arg2.value; - if ( diff > Pi ) - diff = TwoPi - diff; - return AngPi(diff); -} - -AngSignPi::AngSignPi(direction_t const &v) : value(v) { - // Do cheap tests first; use fmod only if necessary. - if ( value < -Pi ) - value += TwoPi; - else if ( value > Pi) - value -= TwoPi; - else return; - // If we're still out of range, give up and call fmod. - if ( value < -Pi || value > Pi) { - value = fmod(value,TwoPi); - if (value < -Pi) - value += TwoPi; - }; -} - -AngPi angdist(AngSignPi const &arg1, AngSignPi const &arg2) { - AngSignPi diff = arg1.value - arg2.value; - if ( diff > Pi ) - diff = TwoPi - diff; - return AngPi(diff); -} - -} // namespace diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/Measures.h ./DualCoding/Measures.h --- ../Tekkotsu_3.0/DualCoding/Measures.h 2006-09-21 18:58:57.000000000 -0400 +++ ./DualCoding/Measures.h 1969-12-31 19:00:00.000000000 -0500 @@ -1,83 +0,0 @@ -#ifndef _MEASURES_H_ -#define _MEASURES_H_ - -#include - -namespace DualCoding { - -typedef float coordinate_t; //!< type used for positional coordinates -typedef float orientation_t; //!< type used for orientation values (0 to Pi) -typedef float direction_t; //!< type used for direction values (0 to 2*Pi) - -const direction_t Pi=M_PI; //!< shorthand for ::M_PI from math.h -const direction_t TwoPi=2*M_PI; //!< shorthand for 2*M_PI - -typedef coordinate_t Slope; //!< type used for ratio of coordinate offsets -const Slope BIG_SLOPE=5000.0; //!< slopes larger than this are considered vertical, or in other words, infinite slopes are rounded to this - -//! Circular arithmetic on angles between 0 and pi (180 degrees) -class AngPi { - public: - orientation_t value; - - AngPi(void) : value(0) {}; - AngPi(orientation_t const &v); - AngPi operator+(AngPi const &arg) const { return AngPi(value+arg.value); }; - AngPi operator-(AngPi const &arg) const { return AngPi(value-arg.value); }; - AngPi operator*(orientation_t const &arg) const { return AngPi(value*arg); }; - AngPi operator/(orientation_t const &arg) const { return AngPi(value/arg); }; - - AngPi& operator=(AngPi const &arg) { value = arg.value; return(*this); }; - AngPi& operator=(orientation_t const &arg) { value = AngPi(arg); return(*this); }; - - operator orientation_t() const { return value; }; -}; - -//! Angular distance: value is between 0 and pi -AngPi angdist(AngPi const &arg1, AngPi const &arg2); - -//! Circular arithmetic on angles between 0 and two pi (360 degrees) -class AngTwoPi { - public: - direction_t value; - - AngTwoPi(void) : value(0) {}; - AngTwoPi(direction_t const &v); - AngTwoPi operator+(AngTwoPi const &arg) const { return AngTwoPi(value+arg.value); }; - AngTwoPi operator-(AngTwoPi const &arg) const { return AngTwoPi(value-arg.value); }; - AngTwoPi operator*(direction_t const &arg) const { return AngTwoPi(value*arg); }; - AngTwoPi operator/(direction_t const &arg) const { return AngTwoPi(value/arg); }; - - AngTwoPi& operator=(AngTwoPi const &arg) { value = arg.value; return(*this); }; - AngTwoPi& operator=(direction_t const &arg) { value = AngTwoPi(arg); return(*this); }; - - operator direction_t() const { return value; }; -}; - -//! Angular distance: value is between 0 and pi -AngPi angdist(AngTwoPi const &arg1, AngTwoPi const &arg2); - -//! Circular arithmetic on angles between -pi and pi (360 degrees) -class AngSignPi { - public: - direction_t value; - - AngSignPi(void) : value(0) {}; - AngSignPi(direction_t const &v); - AngSignPi operator+(AngSignPi const &arg) const { return AngSignPi(value+arg.value); }; - AngSignPi operator-(AngSignPi const &arg) const { return AngSignPi(value-arg.value); }; - AngSignPi operator*(direction_t const &arg) const { return AngSignPi(value*arg); }; - AngSignPi operator/(direction_t const &arg) const { return AngSignPi(value/arg); }; - - AngSignPi& operator=(AngSignPi const &arg) { value = arg.value; return(*this); }; - AngSignPi& operator=(direction_t const &arg) { value = AngSignPi(arg); return(*this); }; - - operator direction_t() const { return value; }; -}; - -//! Angular distance: value is between 0 and pi -AngPi angdist(AngSignPi const &arg1, AngSignPi const &arg2); - -} // namespace - -#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_3.0/DualCoding/PFShapeLocalization.cc ./DualCoding/PFShapeLocalization.cc --- ../Tekkotsu_3.0/DualCoding/PFShapeLocalization.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/PFShapeLocalization.cc 2007-04-20 06:50:52.000000000 -0400 @@ -0,0 +1,180 @@ +//-*-c++-*- + +#include +#include + +#include "MapBuilder.h" +#include "PFShapeLocalization.h" +#include "ShapeLocalizationParticle.h" +#include "ShapeSpace.h" +#include "ShapeFuns.h" +#include "VRmixin.h" // for mapBuilder + +using namespace std; + +namespace DualCoding { + + ParticleShapeEvaluator::ParticleShapeEvaluator(ShapeSpace &localShS, ShapeSpace &worldShS) : + localLms(), worldLms(), + maxDist(0), + localMatches(), numMatches(0), localScores(), + particleViewX(), particleViewY(), particleViewX2(), particleViewY2() + { + PfRoot::loadLms(localShS.allShapes(), false, localLms); + PfRoot::loadLms(worldShS.allShapes(), true, worldLms); + if ( localLms.size()==0 || worldLms.size()==0 ) { + cout << "ParticleFilter::loadLms found " << localLms.size() << " local and " + << worldLms.size() << " world landmarks: can't localize!" << endl; + return; + } + unsigned int nlocal = localLms.size(); + localScores.resize(nlocal); + localMatches.resize(nlocal); + particleViewX.resize(nlocal); + particleViewY.resize(nlocal); + particleViewX2.resize(nlocal); + particleViewY2.resize(nlocal); + } + + void ParticleShapeEvaluator::evaluate(LocalizationParticle& part) { + // determine position of local space landmark in world given the current particle + float const cosT = cos(-part.theta); + float const sinT = sin(-part.theta); + float const negSinT = -sinT; + // constructor has already verified that localLms > 0 and worldLms > 0 + for ( unsigned int j=0; j < localLms.size(); j++ ) { + PfRoot &landmark = *(localLms[j]); + particleViewX[j] = landmark.x * cosT + landmark.y * sinT + part.x; + particleViewY[j] = landmark.x * negSinT + landmark.y * cosT + part.y; + if ( landmark.type == lineDataType ) { + const PfLine &line = static_cast(landmark); + particleViewX2[j] = line.x2 * cosT + line.y2 * sinT + part.x; + particleViewY2[j] = line.x2 * negSinT + line.y2 * cosT + part.y; + } + } + + // now compute score for the particle by finding matches between local landmarks and world landmarks + numMatches=0; + for ( unsigned int j = 0; jtype == worldLms[k]->type && + localLms[j]->color == worldLms[k]->color ) { + float const lx = particleViewX[j]; + float const ly = particleViewY[j]; + float const wx = worldLms[k]->x; + float const wy = worldLms[k]->y; + float tempDistsq; + switch ( localLms[j]->type ) { + case lineDataType: { + PfLine &localLine = *static_cast(localLms[j]); + PfLine &worldLine = *static_cast(worldLms[k]); + float tempDistsq1, tempDistsq2; + // If endpoints are valid, compare distance between endpoints. + // If not valid, measure perpendicular distance from the local endpoint + // to the world line segment, if the projection of the endpoint onto the + // segment occurs within the segment, not beyond it. Instead of calculating + // the projection we use a heuristic test: either the x or y endpoint value must + // lie within the range of the line segment. + if ( (localLine.valid1 && worldLine.valid1) || + !( lx >= min(worldLine.x,worldLine.x2) && + lx <= max(worldLine.x,worldLine.x2) || + ly >= min(worldLine.y,worldLine.y2) && + ly <= max(worldLine.y,worldLine.y2) ) ) + tempDistsq1 = (lx-wx)*(lx-wx) + (ly-wy)*(ly-wy); + else { + float const tempDist1 = distanceFromLine(lx,ly,worldLine); + tempDistsq1 = tempDist1 * tempDist1; + } + float const lx2 = particleViewX2[j]; + float const ly2 = particleViewY2[j]; + float const wx2 = worldLine.x2; + float const wy2 = worldLine.y2; + if ( (localLine.valid2 && worldLine.valid2) || + !( lx2 >= min(worldLine.x,worldLine.x2) && + lx2 <= max(worldLine.x,worldLine.x2) || + ly2 >= min(worldLine.y,worldLine.y2) && + ly2 <= max(worldLine.y,worldLine.y2) ) ) + tempDistsq2 = (lx2-wx2)*(lx2-wx2) + (ly2-wy2)*(ly2-wy2); + else { + float const tempDist2 = distanceFromLine(lx2,ly2,worldLine); + tempDistsq2 = tempDist2 * tempDist2; + } + AngPi const localOrient = localLine.orientation + part.theta; + AngPi const odiff = worldLine.orientation - localOrient; + float const odist = 500 * sin(odiff); + float const odistsq = odist * odist; + tempDistsq = tempDistsq1 + tempDistsq2 + odistsq; // plus orientation match term? + } + break; + case ellipseDataType: + case pointDataType: + case blobDataType: { + tempDistsq = (lx-wx)*(lx-wx) + (ly-wy)*(ly-wy); + break; + } + default: + std::cout << "ParticleFilter::computeMatchScore() can't match landmark type " + << localLms[j]->type << std::endl; + return; + } + if ( tempDistsq < distsq ) { + distsq = tempDistsq; + localMatches[j] = k; + } + } + } + + if ( localMatches[j] != -1 || !localLms[j]->mobile ) + localScores[numMatches++]=distsq; + } + } + + float ParticleShapeEvaluator::distanceFromLine(coordinate_t x0, coordinate_t y0, PfLine &wline) { + float const &x1 = wline.x; + float const &y1 = wline.y; + float const &x2 = wline.x2; + float const &y2 = wline.y2; + float const &length = wline.length; + float const result = fabs((x2-x1)*(y1-y0) - (x1-x0)*(y2-y1)) / length; + return result; + } + + void PFShapeLocalization::setAgent() const { + const PFShapeLocalization::particle_type& best = particles[bestIndex]; + VRmixin::mapBuilder.setAgent(Point(best.x,best.y,0,allocentr