diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/BehaviorBase.cc ./Behaviors/BehaviorBase.cc --- ../Tekkotsu_2.4.1/Behaviors/BehaviorBase.cc 2005-07-07 18:33:51.000000000 -0400 +++ ./Behaviors/BehaviorBase.cc 2006-09-27 17:15:52.000000000 -0400 @@ -1,20 +1,18 @@ #include "BehaviorBase.h" #include "Events/EventRouter.h" -std::set BehaviorBase::registry; - BehaviorBase::BehaviorBase(const std::string& name) : ReferenceCounter(), EventListener(), started(false), instanceName(name), className(name) { - registry.insert(this); + getRegistryInstance().insert(this); } BehaviorBase::BehaviorBase(const std::string& classname, const std::string& instancename) : ReferenceCounter(), EventListener(), started(false), instanceName(instancename), className(classname) { - registry.insert(this); + getRegistryInstance().insert(this); } @@ -22,7 +20,7 @@ : ReferenceCounter(b), EventListener(b), started(b.started), instanceName(b.instanceName), className(b.className) { - registry.insert(this); + getRegistryInstance().insert(this); } BehaviorBase& @@ -39,7 +37,7 @@ if(started) std::cerr << "Behavior " << getName() << " deleted while running: use 'RemoveReference', not 'delete'" << std::endl; erouter->removeListener(this); - registry.erase(this); + getRegistryInstance().erase(this); } void @@ -56,12 +54,16 @@ //std::cout << getName() << " stopped " << this << std::endl; if(started) { started=false; - erouter->removeListener(this); - erouter->removeTimer(this); + erouter->remove(this); RemoveReference(); } } +std::set& BehaviorBase::getRegistryInstance() { + static std::set registry; + return registry; +} + /*! @file * @brief Implements BehaviorBase from which all Behaviors should inherit * @author ejt (Creator) diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/BehaviorBase.h ./Behaviors/BehaviorBase.h --- ../Tekkotsu_2.4.1/Behaviors/BehaviorBase.h 2005-04-23 14:32:59.000000000 -0400 +++ ./Behaviors/BehaviorBase.h 2006-10-03 18:09:40.000000000 -0400 @@ -8,19 +8,24 @@ #include //! The basis from which all other Behaviors should inherit -/*! Makes use of ReferenceCounter so that behaviors can automatically delete themselves if - * wanted. - * - * Make sure your own DoStart and DoStop call BehaviorBase::DoStart (or Stop) to allow - * the auto-deletion from reference counting... otherwise you'll get memory leaks if you - * rely on the reference counting. +/*! + * For complex behaviors, it may be helpful to break aspects of the behaviors into independent 'states', and + * use a state machine formalism to control them. See StateNode and Transition for more information. * - * For an empty behavior boilerplate file to help you get started quickly, try - * project/templates/behavior.h: + * Quick-start boilerplate is included in the distribution: project/templates/behavior.h: * - * But it would probably still be a good idea to go through the "First Behavior" tutorial to get a better idea of - * what's going on. + * Tutorials: + * - Tekkotsu's "First Behavior" Tutorial + * - David Touretzky's "Behaviors" Chapter + * - CMU's Cognitive Robotics course slides + * + * REMEMBER: If/when you override DoStart() / DoStop(), make sure that your own implementation calls BehaviorBase's implementation to allow + * proper reference counting... otherwise you'll get memory leaks and other odd issues. (see boilerplate link above for example usage) + * + * Also, if you instantiate a behavior on the stack instead of the heap (this is very rarely done), remember to call + * SetAutoDelete(false) (provided from the ReferenceCounter base class) -- don't want it to try to free memory + * on the stack when the behavior is stopped! (The stack limits the allocation of the behavior + * to the current scope, which overrides the reference counting.) */ class BehaviorBase : public ReferenceCounter, public EventListener { public: @@ -74,9 +79,9 @@ //! Returns true if the behavior is currently running virtual bool isActive() const { return started; } - //! Allows read-only access to the set of currently instantiated behaviors + //! This read-only set allows us list all the currently instantiated behaviors /*! Not all of these behaviors are necessarily active, this is everything that has been allocated and not yet deallocated */ - static const std::set& getRegistry() { return registry; } + static const std::set& getRegistry() { return getRegistryInstance(); } // Just some debugging stuff in stasis /* virtual void AddReference() { @@ -91,6 +96,9 @@ */ protected: + //! static function to provide well-defined initialization order + static std::set& getRegistryInstance(); + //! constructor, @a name is used as both instance name and class name explicit BehaviorBase(const std::string& name); //! constructor, allows different initial values for class name and instance name @@ -103,7 +111,6 @@ bool started; //!< true when the behavior is active std::string instanceName; //!< holds the name of this instance of behavior const std::string className; //!< holds the type of the subclass of this behavior as a string - static std::set registry; //!< allows us to keep track of all the current behaviors }; /*! @file diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controller.cc ./Behaviors/Controller.cc --- ../Tekkotsu_2.4.1/Behaviors/Controller.cc 2005-06-01 01:47:44.000000000 -0400 +++ ./Behaviors/Controller.cc 2006-09-18 14:08:05.000000000 -0400 @@ -11,6 +11,9 @@ #include "Shared/ERS220Info.h" #include "Shared/ERS7Info.h" #include "Shared/string_util.h" +#ifndef PLATFORM_APERIOS +# include "local/sim/Simulator.h" +#endif #include Controller * Controller::theOneController=NULL; @@ -28,11 +31,11 @@ void Controller::DoStart() { BehaviorBase::DoStart(); - sndman->LoadFile(config->controller.select_snd); - sndman->LoadFile(config->controller.next_snd); - sndman->LoadFile(config->controller.prev_snd); - sndman->LoadFile(config->controller.read_snd); - sndman->LoadFile(config->controller.cancel_snd); + sndman->loadFile(config->controller.select_snd); + sndman->loadFile(config->controller.next_snd); + sndman->loadFile(config->controller.prev_snd); + sndman->loadFile(config->controller.read_snd); + sndman->loadFile(config->controller.cancel_snd); erouter->addListener(this,EventBase::estopEGID); // Turn on wireless gui_comm=wireless->socket(SocketNS::SOCK_STREAM, 2048, 32000); @@ -48,11 +51,11 @@ } void Controller::DoStop() { - sndman->ReleaseFile(config->controller.select_snd); - sndman->ReleaseFile(config->controller.next_snd); - sndman->ReleaseFile(config->controller.prev_snd); - sndman->ReleaseFile(config->controller.read_snd); - sndman->ReleaseFile(config->controller.cancel_snd); + sndman->releaseFile(config->controller.select_snd); + sndman->releaseFile(config->controller.next_snd); + sndman->releaseFile(config->controller.prev_snd); + sndman->releaseFile(config->controller.read_snd); + sndman->releaseFile(config->controller.cancel_snd); erouter->removeListener(this); reset(); motman->removeMotion(display); @@ -216,7 +219,7 @@ //pass a line at a time to the controller while(s.size()>0) { - unsigned int endline=s.find('\n'); + std::string::size_type endline=s.find('\n'); if(endline==std::string::npos) { incomplete+=s; return 0; @@ -247,7 +250,7 @@ //pass a line at a time to the controller while(s.size()>0) { - unsigned int endline=s.find('\n'); + std::string::size_type endline=s.find('\n'); if(endline==std::string::npos) { incomplete+=s; return 0; @@ -260,10 +263,18 @@ incomplete+=s.substr(0,endline); //is now complete - if(wireless->isConnected(theOneController->gui_comm->sock)) - erouter->postEvent(new TextMsgEvent(incomplete)); - else - theOneController->takeLine(incomplete); + switch(config->main.consoleMode) { + case Config::main_config::CONTROLLER: + theOneController->takeLine(incomplete); break; + case Config::main_config::TEXTMSG: + erouter->postEvent(TextMsgEvent(incomplete,0)); break; + case Config::main_config::AUTO: + if(wireless->isConnected(theOneController->gui_comm->sock)) + erouter->postEvent(TextMsgEvent(incomplete,0)); + else + theOneController->takeLine(incomplete); + break; + } incomplete.erase(); s=s.substr(endline+1); } @@ -365,7 +376,7 @@ setNext(cmdstack.top()->doCancel()); } else if(args[0]=="!select") { if (args.size() == 1) - setNext(cmdstack.top()->doSelect()); + setNext(cmdstack.top()->doSelect()); else { select(root, args[1].c_str()); refresh(); @@ -387,11 +398,66 @@ cmdstack.push(tmpstack.top()); tmpstack.pop(); } + } else if(args[0]=="!post") { + if(args.size()<4) { + serr->printf("Bad post command, need at least 3 arguments: generator source type [duration]\n"); + return; + } + //parse generator id -- could be a generator name or a numeric value + int egid=0; + for(;egidprintf("Bad event generator '%s'\n",args[1].c_str()); + return; + } + } + //parse source id -- numeric value, unless egid is buttonEGID, in which case we can look up a button name + //(if you want to add support for other symbolic source types, this is where to do it) + unsigned int source; + if(egid==EventBase::buttonEGID) { + source=0; + for(;sourceprintf("Invalid button name or index '%s'\n",args[2].c_str()); + return; + } + } + } else { + source=atoi(args[2].c_str()); + } + //parse type id -- numeric, name, or abbreviated name + int etid=0; + for(;etidprintf("Bad event type '%s'\n",args[3].c_str()); + return; + } + } + } + //duration field (optional, have to check args.size()) + int dur=0; + if(args.size()>4) + dur=atoi(args[4].c_str()); + //send event! + if(egid==EventBase::buttonEGID && isControlling) + erouter->removeTrapper(this); + erouter->postEvent((EventBase::EventGeneratorID_t)egid,source,(EventBase::EventTypeID_t)etid,dur); + if(egid==EventBase::buttonEGID && isControlling) + erouter->addTrapper(this,EventBase::buttonEGID); } else if(args[0]=="!msg") { if(offsets.size()>1) - erouter->postEvent(new TextMsgEvent(s.substr(offsets[1]))); + erouter->postEvent(TextMsgEvent(s.substr(offsets[1]),0)); else - erouter->postEvent(new TextMsgEvent("")); + erouter->postEvent(TextMsgEvent("",0)); } else if(args[0]=="!hello") { static unsigned int count=0; count++; @@ -417,8 +483,14 @@ } refresh(); } else if(args[0]=="!set") { - setConfig(s.substr(offsets[1]).c_str()); - } else + setConfig(s.substr(offsets[1]).c_str()); + } else if(args[0]=="!sim") { +#ifdef PLATFORM_APERIOS + serr->printf("!sim command invalid -- not running in simulator!\n"); +#else + Simulator::sendCommand(s.substr(offsets[1])); +#endif + } else setNext(cmdstack.top()->takeInput(s)); } } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controller.h ./Behaviors/Controller.h --- ../Tekkotsu_2.4.1/Behaviors/Controller.h 2005-08-04 00:37:55.000000000 -0400 +++ ./Behaviors/Controller.h 2005-09-01 14:53:06.000000000 -0400 @@ -22,8 +22,9 @@ * - '!next' - calls ControlBase::doNextItem() of the current control * - '!prev' - calls ControlBase::doPrevItem() of the current control * - '!select [item]' - calls ControlBase::doSelect() of the current control, unless item is specified, in which case it is searched for, starting at the root. - * - '!cancel' - calls ControlBase::doCancel() of the current control + * - '!cancel' - calls current ControlBase::doCancel(), indicates control should cease activity and return to parent (e.g. "Back" button) * - '!dump_stack' - requests a dump of the current stack of submenus (useful if the GUI (re)connects and thus current robot state is unknown) + * - '!post generator source type [duration]' - posts an event of your choosing; Generator should be an entry in EventBase::EventGeneratorNames (e.g. timerEGID) or numeric value; source should be a numeric value (unless generator is buttonEGID, in which case it could be an entry from #buttonNames); type can be a numeric value, EventBase::EventTypeNames (e.g. activate), or EventBase::EventTypeAbbr (e.g. A); duration, if specified, gives the value for EventBase::duration * - '!msg text' - sends text out as a TextMsgEvent; also note that any text entered on the console port while a GUI is also connected will also be sent as a TextMsgEvent, without needing the !input. * - '!root text' - calls ControlBase::takeInput(text) on the root control * - '!hello' - responds with 'hello\\ncount\\n' where count is the number of times '!hello' has been sent. Good for detecting first connection after boot vs. a reconnect. diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/BehaviorActivatorControl.h ./Behaviors/Controls/BehaviorActivatorControl.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/BehaviorActivatorControl.h 2003-09-25 11:26:10.000000000 -0400 +++ ./Behaviors/Controls/BehaviorActivatorControl.h 2006-09-16 02:28:06.000000000 -0400 @@ -8,7 +8,11 @@ class BehaviorActivatorControl : public NullControl { public: //! lets you tell it what action to perform - enum Mode_t { start, stop, toggle }; + enum Mode_t { + start, //!< Passed to constructor, indicates this control should start the behavior when activated + stop, //!< Passed to constructor, indicates this control should stop the behavior when activated + toggle //!< Passed to constructor, indicates this control should toggle the behavior when activated + }; //@{ //!constructors diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/BehaviorSwitchActivatorControl.h ./Behaviors/Controls/BehaviorSwitchActivatorControl.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/BehaviorSwitchActivatorControl.h 2003-09-25 11:26:10.000000000 -0400 +++ ./Behaviors/Controls/BehaviorSwitchActivatorControl.h 2006-09-16 02:28:06.000000000 -0400 @@ -9,7 +9,11 @@ class BehaviorSwitchActivatorControl : public ControlBase { public: //! lets you tell it what action to perform - enum Mode_t { start, stop, toggle }; + enum Mode_t { + start, //!< Passed to constructor, indicates this control should start the behavior when activated + stop, //!< Passed to constructor, indicates this control should stop the behavior when activated + toggle //!< Passed to constructor, indicates this control should toggle the behavior when activated + }; //!constructor BehaviorSwitchActivatorControl(const std::string& n, BehaviorSwitchControlBase* bscb, Mode_t m=toggle) : ControlBase(n), behswitch(bscb), mode(m) {} diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/BehaviorSwitchControl.h ./Behaviors/Controls/BehaviorSwitchControl.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/BehaviorSwitchControl.h 2005-06-29 18:02:58.000000000 -0400 +++ ./Behaviors/Controls/BehaviorSwitchControl.h 2006-09-01 14:21:20.000000000 -0400 @@ -7,6 +7,7 @@ #include "Shared/ReferenceCounter.h" #include "Shared/Factory.h" #include "Shared/debuget.h" +#include "Events/TextMsgEvent.h" //! Holds some utility classes and functions for BehaviorSwitchControl which shouldn't be stored in a templated class class BehaviorSwitchControlBase : public ControlBase { @@ -20,9 +21,10 @@ * Pass NULL instead of one of these to get checkbox-style. */ class BehaviorGroup : public ReferenceCounter { public: - BehaviorGroup() : curBehavior(NULL) { } //!< contructor + BehaviorGroup() : curBehavior(NULL), members() { } //!< contructor ~BehaviorGroup() { if(curBehavior!=NULL) curBehavior->DoStop(); } //!< destructor, will stop the current behavior if it was a one-shot BehaviorBase * curBehavior; //!< pointer to current behavior + std::set members; //!< set of members of the group private: BehaviorGroup(const BehaviorGroup&); //!< shouldn't be called BehaviorGroup operator=(const BehaviorGroup&); //!< shouldn't be called @@ -31,17 +33,41 @@ //! constructor BehaviorSwitchControlBase(const std::string& n, BehaviorBase* beh, BehaviorGroup* bg=NULL) : ControlBase(n), behgrp(bg), mybeh(beh) { - if(behgrp!=NULL) - behgrp->AddReference(); - if(mybeh!=NULL) + if(mybeh!=NULL) { mybeh->AddReference(); + mybeh->setName(n); + if(mybeh->isActive()) + mybeh->AddReference(); + } + if(behgrp!=NULL) { + behgrp->AddReference(); + behgrp->members.insert(this); + if(mybeh!=NULL && mybeh->isActive()) { + if(behgrp->curBehavior!=NULL) { + behgrp->curBehavior->DoStop(); + notifyGroupMembers(); + } + behgrp->curBehavior=mybeh; + } + } } - //! constructor + //! constructor, behavior must not be NULL BehaviorSwitchControlBase(BehaviorBase* beh, BehaviorGroup* bg=NULL) - : ControlBase(), behgrp(bg), mybeh(beh) { - if(behgrp!=NULL) - behgrp->AddReference(); + : ControlBase(beh->getName()), behgrp(bg), mybeh(beh) { mybeh->AddReference(); + if(mybeh->isActive()) + mybeh->AddReference(); + if(behgrp!=NULL) { + behgrp->AddReference(); + behgrp->members.insert(this); + if(mybeh!=NULL && mybeh->isActive()) { + if(behgrp->curBehavior!=NULL) { + behgrp->curBehavior->DoStop(); + notifyGroupMembers(); + } + behgrp->curBehavior=mybeh; + } + } } //! destructor @@ -50,6 +76,7 @@ if(mybeh!=NULL) stop(); if(behgrp!=NULL) { + behgrp->members.erase(this); behgrp->RemoveReference(); behgrp=NULL; } @@ -67,6 +94,15 @@ //! toggles the behavior virtual BehaviorSwitchControlBase* toggle() { if(isRunning()) stopother(); else { stopother(); startmine(); } return this; } + virtual ControlBase * takeInput(const std::string& msg) { + if(options.size()>0) + return ControlBase::takeInput(msg); + if(!isRunning()) + startmine(); + mybeh->processEvent(TextMsgEvent(msg,1)); + return NULL; + } + //! tells the current behavior (if there is one) to stop then loads its own /*! @return NULL unless there are submenus */ virtual ControlBase * activate(MotionManager::MC_ID display, Socket * gui) { @@ -89,35 +125,46 @@ return "Class "+mybeh->getClassName()+": "+mybeh->getDescription(); } + //! Returns true if the associated behavior is running + virtual bool isRunning() const { + if(mybeh==NULL) //not created or has been destroyed, definitely not running + return false; + // so, beh has been created (but may have been stopped by another in the group) + return mybeh->isActive(); //just check active flag (is valid object, we would have set it to NULL if we stopped it ourselves) + } + protected: //! Stops the "other" guy's behavior - if ::behgrp is NULL, stops ourselves virtual void stopother() { if(behgrp==NULL) { - if(mybeh->isActive()) + if(mybeh!=NULL && mybeh->isActive()) { mybeh->DoStop(); + behaviorStopped(); + } } else if(behgrp->curBehavior!=NULL) { - behgrp->curBehavior->DoStop(); + if(behgrp->curBehavior->isActive()) { + behgrp->curBehavior->DoStop(); + notifyGroupMembers(); + } behgrp->curBehavior=NULL; } } - + //! Starts our behavior virtual void startmine() { if(behgrp!=NULL) behgrp->curBehavior=mybeh; mybeh->DoStart(); } - - //! Returns true if the associated behavior is running - virtual bool isRunning() const { - if(mybeh==NULL) //not created or has been destroyed, definitely not running - return false; - // so, beh has been created (but may have been stopped by another in the group) - if(behgrp==NULL) //no group - return mybeh->isActive(); //just check active flag (is valid object, we would have set it to NULL if we stopped it ourselves) - // so, we're in a group, someone else could have stopped us - return (behgrp->curBehavior==mybeh); //all we can see is if the current behavior is ours. If it is, it'll be active + + //! updates other members in the group that the current behavior stopped -- do not call if behgrp is NULL + virtual void notifyGroupMembers() { + for(std::set::iterator it=behgrp->members.begin(); it!=behgrp->members.end(); ++it) + if((*it)->mybeh==behgrp->curBehavior) + (*it)->behaviorStopped(); } + //! called by notifyGroupMembers if #mybeh was destructed when stopped + virtual void behaviorStopped() {} BehaviorGroup * behgrp; //!< the behavior group this belongs to. Uses this to track the "current" behavior BehaviorBase* mybeh; //!< used to store the behavior. If retained and non-NULL, will be valid. However, if not retained, only valid if equals behgrp->curBehavior @@ -136,49 +183,34 @@ public: //! constructor, can use this to toggle a single behavior on and off BehaviorSwitchControl(const std::string& n, bool retain=false) - : BehaviorSwitchControlBase(n,NULL,NULL), retained(retain) + : BehaviorSwitchControlBase(n,NULL,NULL), retained(retain), startref(NULL) {} //! constructor, if you want to use an already constructed behavior BehaviorSwitchControl(B* beh, BehaviorGroup* bg=NULL) - : BehaviorSwitchControlBase(beh,bg), retained(true) + : BehaviorSwitchControlBase(beh,bg), retained(true), startref(NULL) {} //! constructor, if you want to use an already constructed behavior, but unretain it if it's stopped (if not retaining, will start @a beh if it's not already started) BehaviorSwitchControl(const std::string& n, B* beh, BehaviorGroup* bg=NULL, bool retain=false) - : BehaviorSwitchControlBase(n,beh,bg), retained(retain) + : BehaviorSwitchControlBase(n,beh,bg), retained(retain), startref(NULL) { if(!retained) { // have to make sure behavior is started to maintain invariants if(!mybeh->isActive()) { - //keep reference from superclass's constructor in case mybeh stops itself right away - mybeh->DoStart(); - bool stopped=!mybeh->isActive(); - mybeh->RemoveReference(); //cancels reference from BehaviorSwitchControlBase's constructor - if(stopped) - mybeh=NULL; - } else - mybeh->RemoveReference(); //cancels reference from BehaviorSwitchControlBase's constructor - } - if(behgrp!=NULL) { - if(mybeh->isActive()) { - if(behgrp->curBehavior!=NULL) - behgrp->curBehavior->DoStop(); - behgrp->curBehavior=mybeh; - } else if(!retained) { - if(behgrp->curBehavior!=NULL) - behgrp->curBehavior->DoStop(); - behgrp->curBehavior=NULL; + startmine(); } + mybeh->RemoveReference(); //cancels reference from BehaviorSwitchControlBase's constructor } } //! constructor, needs to know what group its in and whether to retain its behavior BehaviorSwitchControl(const std::string& n, BehaviorGroup* bg, bool retain=false) - : BehaviorSwitchControlBase(n,NULL,bg), retained(retain) + : BehaviorSwitchControlBase(n,NULL,bg), retained(retain), startref(NULL) {} //! destructor virtual ~BehaviorSwitchControl() { stop(); if(behgrp!=NULL) { + behgrp->members.erase(this); behgrp->RemoveReference(); behgrp=NULL; } @@ -200,27 +232,8 @@ return BehaviorSwitchControlBase::getDescription(); } - protected: - virtual void stopother() { - if(behgrp==NULL) { - if(mybeh!=NULL) { - if(mybeh->isActive()) { - mybeh->DoStop(); - if(!retained) - mybeh=NULL; - } else - ASSERT(retained,"null group, non-null not retained beh, not active, did you call inherited DoStart/DoStop in your Behavior?"); - } - } else if(behgrp->curBehavior!=NULL) { - behgrp->curBehavior->DoStop(); - if(behgrp->curBehavior==mybeh) - mybeh=NULL; - behgrp->curBehavior=NULL; - } - } - virtual void startmine() { if(!retained) { Al allocator; @@ -238,17 +251,20 @@ if(behgrp!=NULL) behgrp->curBehavior=mybeh; } - mybeh->AddReference(); //temporary reference in case mybeh stops itself right away + startref=mybeh; + startref->AddReference(); mybeh->DoStart(); - bool stopped=(!mybeh->isActive() && !retained); - mybeh->RemoveReference(); - if(stopped) { - if(behgrp!=NULL && behgrp->curBehavior==mybeh) - behgrp->curBehavior=NULL; - mybeh=NULL; - } } + //! adds a check to see if behavior has stopped itself -- if so, remove startref + virtual bool isRunning() const { + if(BehaviorSwitchControlBase::isRunning()) + return true; + else if(startref!=NULL) + const_cast*>(this)->stopother(); + return false; + } + //! Returns true if mybeh is pointing to a valid object virtual bool isValid() const { if(isRunning()) @@ -256,8 +272,19 @@ return retained; } -private: + virtual void behaviorStopped() { + if(!retained) + mybeh=NULL; + if(startref!=NULL) { + startref->RemoveReference(); + startref=NULL; + } + } + bool retained; //!< true if the behavior should be generated once and retained after DoStop. Otherwise, a new one is generated each time it is started + BehaviorBase * startref; //!< true if a reference was added (and still current) from calling DoStart + +private: BehaviorSwitchControl(const BehaviorSwitchControl&); //!< shouldn't call this BehaviorSwitchControl operator=(const BehaviorSwitchControl&); //!PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); return this; } for(unsigned int i=0;iPlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); if(hilights.size()>1) { options[cur]->activate(display_id,gui_comm); options[cur]->deactivate(); @@ -154,7 +154,7 @@ cur=(cur+1)%options.size(); hilights.clear(); hilights.push_back(cur); - sndman->PlayFile(config->controller.next_snd); + sndman->playFile(config->controller.next_snd); refresh(); // cout << "cur==" << cur << endl; return this; @@ -172,14 +172,14 @@ cur=(cur+options.size()-1)%options.size(); hilights.clear(); hilights.push_back(cur); - sndman->PlayFile(config->controller.prev_snd); + sndman->playFile(config->controller.prev_snd); refresh(); // cout << "cur==" << cur << endl; return this; } ControlBase * ControlBase::doCancel() { - sndman->PlayFile(config->controller.cancel_snd); + sndman->playFile(config->controller.cancel_snd); return NULL; } @@ -190,7 +190,7 @@ MMAccessor display(display_id); display.mc()->cset(FaceLEDMask,.5); } - sndman->PlayFile(config->controller.read_snd); + sndman->playFile(config->controller.read_snd); //Just do one of the other two if(gui_comm==NULL || !wireless->isConnected(gui_comm->sock)) { @@ -373,9 +373,9 @@ float newavg=hilightsAvg(); if(avg!=-1 || newavg!=-1) { if(avg<=newavg) - sndman->PlayFile(config->controller.next_snd); + sndman->playFile(config->controller.next_snd); else - sndman->PlayFile(config->controller.prev_snd); + sndman->playFile(config->controller.prev_snd); } refresh(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/EventLogger.cc ./Behaviors/Controls/EventLogger.cc --- ../Tekkotsu_2.4.1/Behaviors/Controls/EventLogger.cc 2005-08-07 00:11:03.000000000 -0400 +++ ./Behaviors/Controls/EventLogger.cc 2006-09-18 14:07:54.000000000 -0400 @@ -8,7 +8,9 @@ #include #include "Sound/SoundManager.h" #include "Vision/FilterBankGenerator.h" +#include "Vision/JPEGGenerator.h" #include "Shared/Base64.h" +#include "Behaviors/StateNode.h" #include #include @@ -16,8 +18,13 @@ Socket* EventLogger::logSocket=NULL; unsigned int EventLogger::logSocketRefCount=0; int EventLogger::port=10080; +EventLogger * EventLogger::theOne=NULL; -EventLogger::EventLogger() : ControlBase("Event Logger","Allows you to see/log all of the un-trapped events as they are generated"), logfilePath(), logfile(), verbosity(0) { +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() { for(unsigned int i=0; isocket(SocketNS::SOCK_STREAM,1024,1<<15); wireless->setDaemon(logSocket); + wireless->setReceiver(logSocket, callback); wireless->listen(logSocket,port); } logSocketRefCount++; } EventLogger::~EventLogger() { + expected.clear(); + while(!queuedEvents.empty()) + queuedEvents.pop(); clearSlots(); if(--logSocketRefCount==0) { wireless->setDaemon(logSocket,false); wireless->close(logSocket); logSocket=NULL; } + if(theOne==this) + theOne=NULL; } ControlBase* EventLogger::doSelect() { @@ -72,7 +86,7 @@ ans=options[cur]; } } - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); } if(ans==this) refresh(); @@ -94,7 +108,7 @@ xmlNode * cur = xmlNewNode(NULL,(const xmlChar*)""); xmlSetProp(cur,(const xmlChar*)"type",(const xmlChar*)"log"); xmlNode * desc = xmlNewNode(NULL,(const xmlChar*)"param"); - event.SaveXML(cur); + event.saveXML(cur); xmlAddChild(cur,desc); xmlSetProp(desc,(const xmlChar*)"name",(const xmlChar*)"description"); xmlSetProp(desc,(const xmlChar*)"value",(const xmlChar*)event.getDescription(true,3).c_str()); @@ -116,21 +130,31 @@ void EventLogger::logImage(FilterBankGenerator& fbg, unsigned int layer, unsigned int channel, const BehaviorBase* source/*=NULL*/) { if(logSocket!=NULL && wireless->isConnected(logSocket->sock)) { - fbg.selectSaveImage(layer,channel); - unsigned int len=fbg.getBinSize(); - char * binbuf=new char[len]; - fbg.SaveBuffer(binbuf,len); + + 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()); - snprintf(binbuf,len,"%d",get_time()); - xmlSetProp(cur,(const xmlChar*)"time",(const xmlChar*)binbuf); - delete [] binbuf; - xmlNodeSetContent(cur,(const xmlChar*)b64buf.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); @@ -230,6 +254,216 @@ } } + +void EventLogger::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); + 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()); + + 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()); + } + const std::vector& outgoing=(*it)->getDestinations(); + for(unsigned int i=0; iprintf("%s\n",outgoing[i]->getName().c_str()); + } + indent(depth+1); + logSocket->printf("\n"); + } + + indent(depth); + logSocket->printf("\n"); + } +} + +bool EventLogger::isListening(const StateNode* n) { + while(n!=NULL) { + if(listen.find(n->getName())!=listen.end()) + return true; + n=n->getParent(); + } + return false; +} + +void EventLogger::indent(unsigned int level) { + for(unsigned int i=0; iprintf(" "); +} + +const StateNode * EventLogger::find(const std::string& sname) { + const registry_t& registry=BehaviorBase::getRegistry(); + for(registry_t::const_iterator it=registry.begin(); it!=registry.end(); it++) { + const StateNode * cur=dynamic_cast(*it); + if(cur!=NULL && cur->getName()==sname) + return cur; + } + //serr->printf("WARNING: EventLogger Could not find StateNode named `%s'\n",sname.c_str()); + return NULL; +} + +void EventLogger::runCommand(const std::string& s) { + if(s==std::string("list")) { + const registry_t& registry=BehaviorBase::getRegistry(); + unsigned int numstate=0; + for(registry_t::const_iterator it=registry.begin(); it!=registry.end(); it++) { + const StateNode * cur=dynamic_cast(*it); + if(cur!=NULL) + numstate++; + } + logSocket->printf("%d\n",numstate); + for(registry_t::const_iterator it=registry.begin(); it!=registry.end(); it++) { + const StateNode * cur=dynamic_cast(*it); + if(cur!=NULL) + logSocket->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: EventLogger could not find \"%s\" for spidering\n",s.substr(7).c_str()); + logSocket->printf("\n"); + } else { + logSocket->printf("\n"); + spider(n); + logSocket->printf("\n"); + } + + } else if(s.find("listen ")==0) { + if(listen.size()==0) { + erouter->addListener(&smProcess,EventBase::stateMachineEGID); + erouter->addListener(&smProcess,EventBase::stateTransitionEGID); + } + listen.insert(s.substr(7)); + + } else if(s.find("ignore ")==0) { + listen.erase(s.substr(7)); + if(listen.size()==0) + erouter->removeListener(&smProcess); + + } else if(s=="clear") { + listen.clear(); + erouter->removeListener(&smProcess); + + } else { + serr->printf("EventLogger::runCommand() - bad message: '%s'\n",s.c_str()); + } +} + +// The command packet reassembly mechanism +int EventLogger::callback(char *buf, int bytes) { + if(EventLogger::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; +} + +void EventLogger::processStateMachineEvent(const EventBase& event) { + if(!wireless->isConnected(logSocket->sock) || listen.size()==0) + return; + + if(event.getGeneratorID()==EventBase::stateTransitionEGID) { + bool care=false; + const Transition * trans = reinterpret_cast(event.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(event); + } 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); + } + } + + } else if(event.getGeneratorID()==EventBase::stateMachineEGID) { + if(event.getTypeID()==EventBase::statusETID) + 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); + } + } + } + + } else { + serr->printf("WARNING: Unknown event %s (%s)\n",event.getName().c_str(),event.getDescription().c_str()); + } +} + + + + + /*! @file * @brief Describes EventLogger, which allows logging of events to the console or a file * @author ejt (Creator) diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/EventLogger.h ./Behaviors/Controls/EventLogger.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/EventLogger.h 2005-06-01 01:47:44.000000000 -0400 +++ ./Behaviors/Controls/EventLogger.h 2006-09-16 02:28:06.000000000 -0400 @@ -5,15 +5,19 @@ #include "ControlBase.h" #include "Events/EventListener.h" #include +#include +#include class FilterBankGenerator; class BehaviorBase; +class StateNode; //! allows logging of events to the console or a file class EventLogger : public ControlBase, public EventListener { public: //!constructor EventLogger(); + //!destructor virtual ~EventLogger(); //!opens a custom (embedded) menu to toggle individual EGIDs @@ -42,7 +46,21 @@ //! request that the desktop side take a picture with the webcam (if available) static void logWebcam(const BehaviorBase* source=NULL); + static int callback(char *buf, int bytes); //!< called by wireless when there's new data + protected: + static EventLogger * theOne; //!< the instance which will handle network communication + + //! a separate processEvent to distinguish between events requested for logging and events requested by a remote monitor + class StateMachineListener : public EventListener { + //! forwards any events received to EventLogger::theOne's EventLogger::processStateMachineEvent() + /*! EventLogger::runCommand() is responsible for maintaining which events this is listening to */ + virtual void processEvent(const EventBase& event) { + EventLogger::theOne->processStateMachineEvent(event); + } + }; + static class StateMachineListener smProcess; //!< handles state machine transitions if the Storyboard GUI (or other remote monitor) is listening for state machine events + virtual void clearSlots(); //!sets the status char of slot @a i to @a c @@ -51,6 +69,26 @@ //!checks to see if logfilePath differs from the StringInputControl's value and switches it if it is void checkLogFile(); + //! 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); + + //! parses commands sent from callback() + void runCommand(const std::string& s); + + //!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); + + //!if there is a remote monitor listening for state machine transitions, this will send them over + /*!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); + //!address of the logfile, if any (empty string is no logfile) std::string logfilePath; @@ -68,6 +106,17 @@ //!controls the level of verbosity - currently 0 through 2 unsigned int verbosity; + + 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 }; /*! @file diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/FreeMemReportControl.cc ./Behaviors/Controls/FreeMemReportControl.cc --- ../Tekkotsu_2.4.1/Behaviors/Controls/FreeMemReportControl.cc 2005-02-05 02:27:26.000000000 -0500 +++ ./Behaviors/Controls/FreeMemReportControl.cc 2006-09-25 19:26:56.000000000 -0400 @@ -37,7 +37,8 @@ //! reports size of free memory - if this is below low_mem, also generates a warning void FreeMemReportControl::report() { size_t freemem=freeMem(); - sout->printf("%lu bytes free\n",(unsigned long)freemem); + sout->printf("%lu bytes free (%+ld)\n",(unsigned long)freemem,(long)(freemem-lastReport)); + lastReport=freemem; if(freememprintf("WARNING: Low memory: %lu\n",(unsigned long)freemem); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/FreeMemReportControl.h ./Behaviors/Controls/FreeMemReportControl.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/FreeMemReportControl.h 2004-11-10 20:45:35.000000000 -0500 +++ ./Behaviors/Controls/FreeMemReportControl.h 2006-09-25 19:26:56.000000000 -0400 @@ -13,9 +13,9 @@ public: //!@name Contructors/Destructors //!contructor - FreeMemReportControl() : BehaviorBase("FreeMemReportControl"), ControlBase("Free Memory Report","Reports size of free memory, and monitors for low memory warning"), report_freq(-1U), low_mem(256), monitor_freq(1000), isWarning(false) {init();} - FreeMemReportControl(const std::string& n) : BehaviorBase("FreeMemReportControl"), ControlBase(n,"Reports size of free memory, and monitors for low memory warning"), report_freq(-1U), low_mem(256), monitor_freq(1000),isWarning(false) {init();} - FreeMemReportControl(const std::string& n, const std::string& d) : BehaviorBase("FreeMemReportControl"), ControlBase(n,d), report_freq(-1U), low_mem(256), monitor_freq(1000),isWarning(false) {init();} + FreeMemReportControl() : BehaviorBase("FreeMemReportControl"), ControlBase("Free Memory Report","Reports size of free memory, and monitors for low memory warning"), report_freq(-1U), low_mem(256), monitor_freq(1000), isWarning(false), lastReport() {init();} + FreeMemReportControl(const std::string& n) : BehaviorBase("FreeMemReportControl"), ControlBase(n,"Reports size of free memory, and monitors for low memory warning"), report_freq(-1U), low_mem(256), monitor_freq(1000),isWarning(false), lastReport() {init();} + FreeMemReportControl(const std::string& n, const std::string& d) : BehaviorBase("FreeMemReportControl"), ControlBase(n,d), report_freq(-1U), low_mem(256), monitor_freq(1000),isWarning(false), lastReport() {init();} virtual ~FreeMemReportControl() { SetAutoDelete(false); DoStop(); clearSlots(); } //!< destructor //@} @@ -53,12 +53,14 @@ pushSlot(new ValueEditControl("Report Frequency","Controls how often to generate free memory reports (in milliseconds)","Please enter milliseconds to wait between reports (-1 to stop)",&report_freq)); pushSlot(new ValueEditControl("Low Memory Threshold","Controls when to start warning about low memory (in KB)","Please enter the new low memory warning threshold (in KB)",&low_mem)); pushSlot(new ValueEditControl("Monitor Frequency","Controls how often to check for low memory (in milliseconds)","Please enter milliseconds to wait between low memory checks (-1 to stop)",&monitor_freq)); + lastReport=freeMem(); } int report_freq; //!< how often to report memory size (in milliseconds - negative turns off, 0 is as often as possible) unsigned int low_mem; //!< threshold to trigger low memory warning (in kilobytes) unsigned int monitor_freq; //!< how often to check for low memory (in milliseconds - -1U turns off, 0 is as often as possible) bool isWarning; //!< true we already know we're below threshold + size_t lastReport; //!< free memory at last report so we can report the difference }; /*! @file diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/LoadPostureControl.h ./Behaviors/Controls/LoadPostureControl.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/LoadPostureControl.h 2005-06-06 19:05:51.000000000 -0400 +++ ./Behaviors/Controls/LoadPostureControl.h 2006-09-18 14:07:54.000000000 -0400 @@ -23,6 +23,7 @@ setFilter("*.pos"); } + //! destructor virtual ~LoadPostureControl() { erouter->removeListener(this); motman->removeMotion(ledid); @@ -53,7 +54,7 @@ runFile(); } else { //we have to wait for the estop to be turned off - sndman->PlayFile("donkey.wav"); + sndman->playFile("donkey.wav"); SharedObject led; led->cset(FaceLEDMask,0); led->cycle(BotLLEDMask,1000,3,0,0); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/LoadWalkControl.h ./Behaviors/Controls/LoadWalkControl.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/LoadWalkControl.h 2005-06-06 19:05:51.000000000 -0400 +++ ./Behaviors/Controls/LoadWalkControl.h 2006-09-09 00:32:17.000000000 -0400 @@ -37,8 +37,9 @@ if(walk==NULL) serr->printf("Invalid walk for loading\n"); else { - walk->LoadFile(f.c_str()); - motman->checkinMotion(id); + walk->loadFile(f.c_str()); + if(id!=MotionManager::invalid_MC_ID) + motman->checkinMotion(id); } return NULL; } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/NetworkStatusControl.h ./Behaviors/Controls/NetworkStatusControl.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/NetworkStatusControl.h 2005-08-07 00:11:03.000000000 -0400 +++ ./Behaviors/Controls/NetworkStatusControl.h 2005-10-25 17:25:33.000000000 -0400 @@ -43,11 +43,11 @@ pushSlot(new NullControl(tmp)); snprintf(tmp,TMP_SIZE,"noise: %d",msg.statistics.noise); pushSlot(new NullControl(tmp)); - snprintf(tmp,TMP_SIZE,"invalidIDCount: %d",msg.statistics.invalidIDCount); + snprintf(tmp,TMP_SIZE,"invalidIDCount: %u",(unsigned int)msg.statistics.invalidIDCount); pushSlot(new NullControl(tmp)); - snprintf(tmp,TMP_SIZE,"invalidEncCount: %d",msg.statistics.invalidEncCount); + snprintf(tmp,TMP_SIZE,"invalidEncCount: %u",(unsigned int)msg.statistics.invalidEncCount); pushSlot(new NullControl(tmp)); - snprintf(tmp,TMP_SIZE,"invalidMiscCount: %d",msg.statistics.invalidMiscCount); + snprintf(tmp,TMP_SIZE,"invalidMiscCount: %u",(unsigned int)msg.statistics.invalidMiscCount); pushSlot(new NullControl(tmp)); MMAccessor leds_acc(display_id); leds_acc->displayPercent(msg.statistics.signal/100.0,LedEngine::major,LedEngine::major); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/PlaySoundControl.h ./Behaviors/Controls/PlaySoundControl.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/PlaySoundControl.h 2005-06-06 19:05:51.000000000 -0400 +++ ./Behaviors/Controls/PlaySoundControl.h 2006-09-18 14:07:54.000000000 -0400 @@ -20,9 +20,9 @@ protected: //!does the actual loading of the MotionSequence virtual ControlBase* selectedFile(const std::string& f) { - sndman->StopPlay(); + sndman->stopPlay(); if(sndman) - sndman->PlayFile(f.c_str()); + sndman->playFile(f.c_str()); return this; } }; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/PostureEditor.cc ./Behaviors/Controls/PostureEditor.cc --- ../Tekkotsu_2.4.1/Behaviors/Controls/PostureEditor.cc 2005-08-02 18:24:20.000000000 -0400 +++ ./Behaviors/Controls/PostureEditor.cc 2006-09-09 00:32:18.000000000 -0400 @@ -64,13 +64,14 @@ PostureEditor::refresh() { //cout << "refresh" << endl; if(isEStopped()) { + processEvent(EventBase(EventBase::timerEGID,1,EventBase::statusETID,0)); erouter->addTimer(this,0,500); options[0]=disabledLoadPose; } else { options[0]=loadPose; } if(loadPose->getLastInput().size()>0) { - pose.LoadFile(loadPose->getLastInput().c_str()); + pose.loadFile(loadPose->getLastInput().c_str()); updatePose(moveTime); loadPose->clearLastInput(); } else if(savePose->getLastInput().size()>0) { @@ -78,7 +79,7 @@ std::string filename=savePose->getLastInput(); if(filename.find(".")==std::string::npos) filename+=".pos"; - pose.SaveFile(config->motion.makePath(filename).c_str()); + pose.saveFile(config->motion.makePath(filename).c_str()); savePose->takeInput(""); } else { updatePose(moveTime/2); @@ -128,7 +129,8 @@ pose(i).value=state->outputs[i]; for(unsigned int i=LEDOffset+NumLEDs; ioutputs[i]; - refresh(); + if(e.getSourceID()==0) // source==1 indicates it's a forged event sent from refresh -- don't inf. recurse + refresh(); } else { serr->printf("WARNING: PostureEditor unexpected event: %s\n",e.getName().c_str()); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/ProfilerCheckControl.h ./Behaviors/Controls/ProfilerCheckControl.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/ProfilerCheckControl.h 2004-11-11 15:34:59.000000000 -0500 +++ ./Behaviors/Controls/ProfilerCheckControl.h 2006-07-13 13:25:50.000000000 -0400 @@ -3,7 +3,7 @@ #define INCLUDED_ProfilerCheckControl_h_ #include "ControlBase.h" -#include "Shared/WorldState.h" +#include "Shared/Profiler.h" //! causes the WorldState::mainProfile and WorldState::motionProfile to display reports to #sout class ProfilerCheckControl : public ControlBase { @@ -13,8 +13,9 @@ //! Prints a report to sout virtual ControlBase * activate(MotionManager::MC_ID, Socket *) { - sout->printf("~~~ Main: ~~~\n%s",state->mainProfile.report().c_str()); - sout->printf("~~~ Motion: ~~~\n%s",state->motionProfile.report().c_str()); + sout->printf("~~~ Main: ~~~\n%s\n",mainProfiler==NULL?"Main profile unavailable":mainProfiler->report().c_str()); + sout->printf("~~~ Motion: ~~~\n%s\n",motionProfiler==NULL?"Motion profile unavailable":motionProfiler->report().c_str()); + sout->printf("~~~ Sound: ~~~\n%s\n",soundProfiler==NULL?"Sound profile unavailable":soundProfiler->report().c_str()); return NULL; } }; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/RunSequenceControl.h ./Behaviors/Controls/RunSequenceControl.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/RunSequenceControl.h 2005-06-06 19:05:51.000000000 -0400 +++ ./Behaviors/Controls/RunSequenceControl.h 2006-09-18 14:07:54.000000000 -0400 @@ -34,6 +34,7 @@ setFilter("*.mot"); } + //! destructor virtual ~RunSequenceControl() { erouter->removeListener(this); motman->removeMotion(ledid); @@ -63,7 +64,7 @@ runFile(); } else { //we have to wait for the estop to be turned off - sndman->PlayFile("donkey.wav"); + sndman->playFile("donkey.wav"); SharedObject led; led->cset(FaceLEDMask,0); led->cycle(BotLLEDMask,1000,3,0,0); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/SavePostureControl.h ./Behaviors/Controls/SavePostureControl.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/SavePostureControl.h 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Controls/SavePostureControl.h 2006-09-09 00:32:18.000000000 -0400 @@ -22,7 +22,7 @@ PostureEngine post; post.takeSnapshot(); post.setWeights(1); - post.SaveFile(filename.c_str()); + post.saveFile(filename.c_str()); } return StringInputControl::takeInput(msg); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/SaveWalkControl.h ./Behaviors/Controls/SaveWalkControl.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/SaveWalkControl.h 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Controls/SaveWalkControl.h 2006-09-09 00:32:18.000000000 -0400 @@ -28,8 +28,9 @@ if(walk==NULL) serr->printf("Invalid walk for saving\n"); else { - walk->SaveFile(filename.c_str()); - motman->checkinMotion(id); + walk->saveFile(filename.c_str()); + if(id!=MotionManager::invalid_MC_ID) + motman->checkinMotion(id); } } return StringInputControl::takeInput(msg); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/SensorObserverControl.cc ./Behaviors/Controls/SensorObserverControl.cc --- ../Tekkotsu_2.4.1/Behaviors/Controls/SensorObserverControl.cc 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Controls/SensorObserverControl.cc 2006-10-02 18:11:38.000000000 -0400 @@ -62,7 +62,7 @@ ans=options[cur]; } } - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); if(wasListening!=(numListeners>0)) { if(numListeners>0) erouter->addListener(this,EventBase::sensorEGID,SensorSourceID::UpdatedSID); @@ -191,11 +191,11 @@ ControlBase::refresh(); } void SensorObserverControl::RTViewControl::pause() { - erouter->removeListener(this); + erouter->removeTimer(this); ControlBase::pause(); } void SensorObserverControl::RTViewControl::deactivate() { - erouter->removeListener(this); + erouter->removeTimer(this); ControlBase::deactivate(); } /*! The change doesn't get picked up until next call to refresh() */ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/ShutdownControl.cc ./Behaviors/Controls/ShutdownControl.cc --- ../Tekkotsu_2.4.1/Behaviors/Controls/ShutdownControl.cc 2005-02-02 13:20:27.000000000 -0500 +++ ./Behaviors/Controls/ShutdownControl.cc 2005-08-31 17:55:12.000000000 -0400 @@ -1,12 +1,17 @@ #include "ShutdownControl.h" #ifdef PLATFORM_APERIOS # include +#else +# include "local/sim/SharedGlobals.h" #endif ControlBase * ShutdownControl::doSelect() { #ifdef PLATFORM_APERIOS OBootCondition bc(0); OPENR::Shutdown(bc); +#else + if(globals!=NULL) + globals->signalShutdown(); #endif return NULL; } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/SimulatorAdvanceFrameControl.h ./Behaviors/Controls/SimulatorAdvanceFrameControl.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/SimulatorAdvanceFrameControl.h 2005-06-23 18:37:28.000000000 -0400 +++ ./Behaviors/Controls/SimulatorAdvanceFrameControl.h 2006-08-30 10:26:44.000000000 -0400 @@ -6,7 +6,7 @@ #ifdef PLATFORM_APERIOS # warning SimulatorAdvanceFrameControl is only useful when running in simulation! #else -# include "local/sim/Main.h" +# include "local/sim/Simulator.h" #endif //! Requests the next camera frame and sensor data, for use when running in simulation @@ -32,23 +32,25 @@ #ifndef PLATFORM_APERIOS virtual ControlBase * activate(MotionManager::MC_ID disp_id, Socket * gui) { - Main::advanceVision(); - Main::advanceSensor(); + Simulator::sendCommand("advance"); return NullControl::activate(disp_id,gui); } virtual std::string getName() const { - if(Main::canManuallyAdvance()) + if(canManuallyAdvance()) return NullControl::getName(); return "[Auto-Advancing]"; } virtual std::string getDescription() const { - if(Main::canManuallyAdvance()) + if(canManuallyAdvance()) return NullControl::getDescription(); return "Cannot manually advance when in realtime mode, or when AdvanceOnAccess is enabled"; } +protected: + bool canManuallyAdvance() const { return true; } + #endif }; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/WalkCalibration.cc ./Behaviors/Controls/WalkCalibration.cc --- ../Tekkotsu_2.4.1/Behaviors/Controls/WalkCalibration.cc 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Controls/WalkCalibration.cc 2006-09-18 14:07:54.000000000 -0400 @@ -220,12 +220,12 @@ if(hilights.size()==0) return this; if(options[hilights.front()]==measure) { - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); setupChoose(); refresh(); return this; } else if(options[hilights.front()]==clear) { - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); setupClear(); refresh(); return this; @@ -235,10 +235,10 @@ } else if(st==CLEAR) { if(hilights.size()==0) return this; - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); if(hilights.front()!=0) { sout->printf("Clearing data...\n"); - sndman->PlayFile(config->controller.cancel_snd); + sndman->playFile(config->controller.cancel_snd); for(int i=0; i(hilights.front()-2); - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); setupReady(); refresh(); return this; } - sndman->PlayFile(config->controller.cancel_snd); + sndman->playFile(config->controller.cancel_snd); setupRoot(); refresh(); return this; } else if(st==READY) { if(hilights.size()==0 || options[hilights.front()]->getName()=="Go!") { - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); setupMoving(); refresh(); return this; @@ -271,12 +271,12 @@ if(options[hilights.front()]==polar || options[hilights.front()]==rect) { return ControlBase::doSelect(); } - sndman->PlayFile(config->controller.cancel_snd); + sndman->playFile(config->controller.cancel_snd); setupChoose(); refresh(); return this; } else { - sndman->PlayFile(config->controller.cancel_snd); + sndman->playFile(config->controller.cancel_snd); setupChoose(); refresh(); return this; @@ -287,7 +287,7 @@ if(st==MOVING) { stopTime=e.getTimeStamp(); sout->printf("Ran for %g seconds\n",(stopTime-startTime)/1000.f); - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); setupReading1(); refresh(); if(curType!=5) @@ -310,7 +310,7 @@ err("Invalid input: "+msg); return doReadStdIn(std::string("Enter ")+getFirstMeasure(curType)); } - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); setupReading2(); refresh(); if(*end!='\0') @@ -325,7 +325,7 @@ return doReadStdIn(std::string("Enter ")+getSecondMeasure(curType)); } addSample(); - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); setupReady(); refresh(); return this; @@ -873,7 +873,7 @@ errmsg.push_back(str); serr->printf("%s\n",str.c_str()); Controller::loadGUI("org.tekkotsu.mon.ControllerErr","msg",0,errmsg); - sndman->PlayFile(config->controller.error_snd); + sndman->playFile(config->controller.error_snd); return; } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/WalkCalibration.h ./Behaviors/Controls/WalkCalibration.h --- ../Tekkotsu_2.4.1/Behaviors/Controls/WalkCalibration.h 2005-02-02 13:47:23.000000000 -0500 +++ ./Behaviors/Controls/WalkCalibration.h 2006-09-16 16:11:49.000000000 -0400 @@ -44,25 +44,25 @@ protected: enum { - ROOT, - CHOOSE, - READY, - MOVING, - READING_1, - READING_2, - CLEAR, + ROOT, //!< indicates the root menu is currently displayed (save/load data sets...) + CHOOSE, //!< indicates the sample type selection is displayed + READY, //!< waiting for user to indicate 0 point + MOVING, //!< recording, waiting for the motion stop event + READING_1, //!< waiting for user to supply the first measurement coordinate + READING_2, //!< waiting for user to supply the second measurement coordinate + CLEAR, //!< clear data confirmation menu } st; //!< the currently active state //! allows representation of the current sample type enum dataSource { - fs, - fr, - sr, - br, - bs, - r, - NUM_SRC - } curType; + fs, //!< forward-sideways + fr, //!< forward-rotate + sr, //!< sideways-rotate + br, //!< backward-rotate + bs, //!< backward-sideways + r, //!< pure rotation + NUM_SRC //!< number of data types + } curType; //!< the currently selected type of data being recorded static void loadData(const std::string& name, std::vector& data); //!< does the work of loading data sets static void saveData(const std::string& name, const std::vector& data); //!< does the work of saving data sets diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Controls/WaypointWalkControl.cc ./Behaviors/Controls/WaypointWalkControl.cc --- ../Tekkotsu_2.4.1/Behaviors/Controls/WaypointWalkControl.cc 2005-06-06 19:05:51.000000000 -0400 +++ ./Behaviors/Controls/WaypointWalkControl.cc 2006-09-18 14:07:54.000000000 -0400 @@ -120,23 +120,23 @@ startstopCtl->setDescription("Halt locomotion"); MMAccessor(walk_id)->go(); } - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); return curctl; } else if(curctl==loopCtl) { MMAccessor(walk_id)->setIsLooping(!loopCtl->getStatus()); - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); return curctl; } else if(curctl==addEgoWPCtl) { MMAccessor(walk_id)->addEgocentricWaypoint(0,0,false,true,.1); - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); return curctl; } else if(curctl==addOffWPCtl) { MMAccessor(walk_id)->addOffsetWaypoint(0,0,false,true,.1); - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); return curctl; } else if(curctl==addAbsWPCtl) { MMAccessor(walk_id)->addAbsoluteWaypoint(0,0,false,true,.1); - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); return curctl; } } @@ -177,20 +177,20 @@ if(curctl==up) { WaypointWalkMC::WaypointList_t& list=MMAccessor(walk_id)->getWaypointList(); list.swap(list.prev(waypoint_id),waypoint_id); - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); return NULL; } else if(curctl==down) { WaypointWalkMC::WaypointList_t& list=MMAccessor(walk_id)->getWaypointList(); list.swap(waypoint_id,list.next(waypoint_id)); - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); return NULL; } else if(curctl==del) { MMAccessor(walk_id)->getWaypointList().erase(waypoint_id); - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); return NULL; } else if(curctl==set) { MMAccessor(walk_id)->setTargetWaypoint(waypoint_id); - sndman->PlayFile(config->controller.select_snd); + sndman->playFile(config->controller.select_snd); return NULL; } } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/AutoGetupBehavior.h ./Behaviors/Demos/AutoGetupBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Demos/AutoGetupBehavior.h 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Demos/AutoGetupBehavior.h 1969-12-31 19:00:00.000000000 -0500 @@ -1,83 +0,0 @@ -//-*-c++-*- -#ifndef INCLUDED_AutoGetupBehavior_h_ -#define INCLUDED_AutoGetupBehavior_h_ - -#include "Behaviors/BehaviorBase.h" -#include "Shared/WorldState.h" -#include "Events/EventRouter.h" -#include "IPC/SharedObject.h" -#include "Motion/MotionManager.h" -#include "Motion/MotionSequenceMC.h" -#include "Shared/Config.h" -#include "Sound/SoundManager.h" - -//! a little background behavior to keep the robot on its feet -class AutoGetupBehavior : public BehaviorBase { -public: - //! constructor - AutoGetupBehavior() : BehaviorBase("AutoGetupBehavior"), back(0), side(0), gamma(.9), sensitivity(.85*.85), waiting(false) {} - //! destructor - virtual ~AutoGetupBehavior() {} - - //! Listens for the SensorSourceID::UpdatedSID - virtual void DoStart() { - BehaviorBase::DoStart(); - erouter->addListener(this,EventBase::sensorEGID,SensorSourceID::UpdatedSID); - } - //! Stops listening for events - virtual void DoStop() { - erouter->removeListener(this); - BehaviorBase::DoStop(); - } - //! Run appropriate motion script if the robot falls over - virtual void processEvent(const EventBase &event) { - if(event.getGeneratorID()==EventBase::motmanEGID) { - //previous attempt at getting up has completed - cout << "Getup complete" << endl; - erouter->removeListener(this,EventBase::motmanEGID); - waiting=false; - return; - } - back=back*gamma+(1-gamma)*state->sensors[BAccelOffset]; - 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; - 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) - if(fabs(back)motion.makePath("gu_side.mot"); - else if(back<0) - gu=config->motion.makePath("gu_back.mot"); - else - gu=config->motion.makePath("gu_front.mot"); - SharedObject getup(gu.c_str()); - MotionManager::MC_ID id=motman->addPrunableMotion(getup,MotionManager::kHighPriority); - erouter->addListener(this,EventBase::motmanEGID,id,EventBase::deactivateETID); - waiting=true; - } - } - static std::string getClassDescription() { return "Monitors gravity's influence on the accelerometers - if it seems the robot has fallen over, it runs appropriate getup script"; } - virtual std::string getDescription() const { return getClassDescription(); } - -protected: - float back; //!< exponential average of backwards accel - float side; //!< exponential average of sideways accel - float gamma; //!< default 0.9, gamma parameter for exponential average of above - float sensitivity; //!< default 0.85*0.85, squared threshold to consider having fallen over, use values 0-1 - bool waiting; //!< true while we're waiting to hear from completion of MotionSequence, won't try again until this is cleared -}; - -/*! @file - * @brief Defines AutoGetupBehavior, a little background behavior to keep the robot on its feet - * @author ejt (Creator) - * - * $Author: ejt $ - * $Name: HEAD $ - * $Revision: 1.1 $ - * $State: Exp $ - * $Date: 2006/10/04 04:21:12 $ - */ - -#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/BanditMachine.h ./Behaviors/Demos/BanditMachine.h --- ../Tekkotsu_2.4.1/Behaviors/Demos/BanditMachine.h 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Demos/BanditMachine.h 2006-09-18 14:07:57.000000000 -0400 @@ -6,6 +6,7 @@ #include "Behaviors/Demos/StareAtBallBehavior.h" #include "IPC/SharedObject.h" #include "Motion/PostureMC.h" +#include "Motion/MMAccessor.h" #include "Motion/MotionSequenceMC.h" #include "Motion/LedMC.h" #include "Behaviors/Transitions/TimeOutTrans.h" @@ -13,200 +14,201 @@ #include "Behaviors/Nodes/OutputNode.h" #include "Sound/SoundManager.h" #include "Shared/ProjectInterface.h" +#include "Shared/WorldState.h" #include "Behaviors/Demos/karmedbandit.h" //! Plays K-armed bandit class BanditMachine : public StateNode { public: - //!constructor - BanditMachine() - : StateNode("BanditMachine","BanditMachine"), stare(NULL), start(NULL), liedown(MotionManager::invalid_MC_ID), bandit(2) - { - stare=new StareAtBallBehavior(); - stare->AddReference(); - } - //!constructor - BanditMachine(const char* n) - : StateNode("BanditMachine",n), stare(), start(NULL), liedown(MotionManager::invalid_MC_ID), bandit(2) - { - stare=new StareAtBallBehavior(); - stare->AddReference(); - } - //!destructor - virtual ~BanditMachine() { - stare->RemoveReference(); - } + //!constructor + BanditMachine() + : StateNode("BanditMachine","BanditMachine"), stare(NULL), start(NULL), liedown(MotionManager::invalid_MC_ID), bandit(2) + { + stare=new StareAtBallBehavior(); + stare->AddReference(); + } + //!constructor + BanditMachine(const char* n) + : StateNode("BanditMachine",n), stare(), start(NULL), liedown(MotionManager::invalid_MC_ID), bandit(2) + { + stare=new StareAtBallBehavior(); + stare->AddReference(); + } + //!destructor + virtual ~BanditMachine() { + stare->RemoveReference(); + } - static std::string getClassDescription() { return "Plays k-armed bandit with a computer"; } - virtual std::string getDescription() const { return getClassDescription(); } + static std::string getClassDescription() { return "Plays k-armed bandit with a computer"; } + virtual std::string getDescription() const { return getClassDescription(); } - virtual void setup() { - StateNode *wait=start=addNode(new WaitNode("Wait",bandit)); - StateNode *left=addNode(new PressNode("Left",LFrLegOffset+KneeOffset)); - StateNode *right=addNode(new PressNode("Right",RFrLegOffset+KneeOffset)); - 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)); - wait->addTransition(new TimeOutTrans(decide,2000)); - left->addTransition(new TimeOutTrans(recoverl,1500)); - right->addTransition(new TimeOutTrans(recoverr,1500)); - // recover->addTransition(new TimeOutTrans(decide,500)); - StateNode::setup(); - } + virtual void setup() { + StateNode *wait=start=addNode(new WaitNode("Wait",bandit)); + StateNode *left=addNode(new PressNode("Left",LFrLegOffset+KneeOffset)); + StateNode *right=addNode(new PressNode("Right",RFrLegOffset+KneeOffset)); + 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)); + wait->addTransition(new TimeOutTrans(decide,2000)); + left->addTransition(new TimeOutTrans(recoverl,1500)); + right->addTransition(new TimeOutTrans(recoverr,1500)); + // recover->addTransition(new TimeOutTrans(decide,500)); + StateNode::setup(); + } - virtual void DoStart() { - StateNode::DoStart(); - stare->DoStart(); - start->DoStart(); - SharedObject lie("liedown.pos"); - lie->setOutputCmd(LFrLegOffset+RotatorOffset,.77); - lie->setOutputCmd(RFrLegOffset+RotatorOffset,.73); - lie->setOutputCmd(LFrLegOffset+KneeOffset,.6); - lie->setOutputCmd(RFrLegOffset+KneeOffset,.6); - liedown=motman->addPrunableMotion(lie); - } + virtual void DoStart() { + StateNode::DoStart(); + stare->DoStart(); + start->DoStart(); + SharedObject lie("liedown.pos"); + lie->setOutputCmd(LFrLegOffset+RotatorOffset,.77); + lie->setOutputCmd(RFrLegOffset+RotatorOffset,.73); + lie->setOutputCmd(LFrLegOffset+KneeOffset,.6); + lie->setOutputCmd(RFrLegOffset+KneeOffset,.6); + liedown=motman->addPrunableMotion(lie); + } - virtual void DoStop() { - motman->removeMotion(liedown); - stare->DoStop(); - StateNode::DoStop(); - } + virtual void DoStop() { + motman->removeMotion(liedown); + stare->DoStop(); + StateNode::DoStop(); + } protected: - //! This node is used to move a paw down using a MotionSequenceMC - class PressNode : public StateNode { - public: - //! constructor - /*! @param n name of the node - * @param idx the joint index of the paw to move - */ - PressNode(const char* n, unsigned int idx) : StateNode("PressNode",n), press_id(MotionManager::invalid_MC_ID), index(idx) { - SharedObject press; - press->setTime(0); - press->setOutputCmd(idx,.6); - press->setTime(1); - press->setOutputCmd(idx,.6); - press->setTime(200); - press->setOutputCmd(idx,.3); - press->setTime(1500); - press->setOutputCmd(idx,outputRanges[idx][MinRange]); - press_id=motman->addPersistentMotion(press,MotionManager::kStdPriority+1); - } - //!destructor - virtual ~PressNode() { - motman->removeMotion(press_id); - } - virtual void DoStart() { - StateNode::DoStart(); - MMAccessor press(press_id); - press->play(); - press->setOutputCmd(index,.6); - // press->setSpeed(1); - } - virtual void DoStop() { - MMAccessor press(press_id); - // press->setSpeed(-1); - press->pause(); - press->setTime(0); - StateNode::DoStop(); - } - protected: - MotionManager::MC_ID press_id; //!< the MC_ID of the MotionSequenceMC being used to do the press - unsigned int index; //!< the joint index of the paw to move - }; + //! This node is used to move a paw down using a MotionSequenceMC + class PressNode : public StateNode { + public: + //! constructor + /*! @param n name of the node + * @param idx the joint index of the paw to move + */ + PressNode(const char* n, unsigned int idx) : StateNode("PressNode",n), press_id(MotionManager::invalid_MC_ID), index(idx) { + SharedObject press; + press->setTime(0); + press->setOutputCmd(idx,.6); + press->setTime(1); + press->setOutputCmd(idx,.6); + press->setTime(200); + press->setOutputCmd(idx,.3); + press->setTime(1500); + press->setOutputCmd(idx,outputRanges[idx][MinRange]); + press_id=motman->addPersistentMotion(press,MotionManager::kStdPriority+1); + } + //!destructor + virtual ~PressNode() { + motman->removeMotion(press_id); + } + virtual void DoStart() { + StateNode::DoStart(); + MMAccessor press(press_id); + press->play(); + press->setOutputCmd(index,.6); + // press->setSpeed(1); + } + virtual void DoStop() { + MMAccessor press(press_id); + // press->setSpeed(-1); + press->pause(); + press->setTime(0); + StateNode::DoStop(); + } + protected: + MotionManager::MC_ID press_id; //!< the MC_ID of the MotionSequenceMC being used to do the press + unsigned int index; //!< the joint index of the paw to move + }; - //! uses one of the algorithms in karmedbandit.h to decide which paw to press next - class DecideNode : public StateNode { - public: - //! constructor - /*! @param n name of the node - * @param bandito the decision making algorithm to use (look in karmedbandit.h) - * @param left the PressNode to go to if the left paw is chosen - * @param right the PressNode to go to if the right paw is chosen - */ - DecideNode(const char* n, karmedbanditExp3_1& bandito, StateNode* left, StateNode* right) - : StateNode("DecideNode",n), b(bandito), l(left), r(right) - {} - virtual void DoStart() { - StateNode::DoStart(); - AddReference(); - DoStop(); - if(b.decide()==0) { - std::cout << "Left... " << std::flush; - l->DoStart(); - } else { - std::cout << "Right... " << std::flush; - r->DoStart(); - } - RemoveReference(); - } - protected: - karmedbanditExp3_1& b; //!< the class implementing the k-armed bandit algorithm - StateNode* l; //!< the node to go to if the left paw is chosen - StateNode* r; //!< the node to go to if the right paw is chosen - private: - DecideNode(const DecideNode& node); //!< don't call this - DecideNode operator=(const DecideNode& node); //!< don't call this - }; + //! uses one of the algorithms in karmedbandit.h to decide which paw to press next + class DecideNode : public StateNode { + public: + //! constructor + /*! @param n name of the node + * @param bandito the decision making algorithm to use (look in karmedbandit.h) + * @param left the PressNode to go to if the left paw is chosen + * @param right the PressNode to go to if the right paw is chosen + */ + DecideNode(const char* n, karmedbanditExp3_1& bandito, StateNode* left, StateNode* right) + : StateNode("DecideNode",n), b(bandito), l(left), r(right) + {} + virtual void DoStart() { + StateNode::DoStart(); + AddReference(); + DoStop(); + if(b.decide()==0) { + std::cout << "Left... " << std::flush; + l->DoStart(); + } else { + std::cout << "Right... " << std::flush; + r->DoStart(); + } + RemoveReference(); + } + protected: + karmedbanditExp3_1& b; //!< the class implementing the k-armed bandit algorithm + StateNode* l; //!< the node to go to if the left paw is chosen + StateNode* r; //!< the node to go to if the right paw is chosen + private: + DecideNode(const DecideNode& node); //!< don't call this + DecideNode operator=(const DecideNode& node); //!< don't call this + }; - //! Waits to see if a reward is received, lights up LEDs to let the user know - class WaitNode : public StateNode { - public: - //! constructor - /* @param n name to use for the node - * @param bandito the class to pass the reward to (if it comes) - */ - WaitNode(const char* n, karmedbanditExp3_1& bandito) - : StateNode("WaitNode",n), b(bandito), reward(false), leds_id(MotionManager::invalid_MC_ID) - { - leds_id=motman->addPersistentMotion(SharedObject()); - } - //! destructor - virtual ~WaitNode() { - motman->removeMotion(leds_id); - } - virtual void DoStart() { - StateNode::DoStart(); - erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID); - erouter->addTimer(this,0,1000,false); - MMAccessor leds(leds_id); - leds->cflash(BotLLEDMask+BotRLEDMask,1,1000); - } - virtual void DoStop() { - erouter->removeListener(this); - b.reward(reward); - cout << endl; - reward=false; - StateNode::DoStop(); - } - virtual void processEvent(const EventBase& event) { - if(event.getGeneratorID()==EventBase::timerEGID) { - sndman->PlayFile("whimper.wav"); - } else { - sndman->PlayFile("yipper.wav"); - reward=true; - MMAccessor leds(leds_id); - leds->cflash(MidLLEDMask+MidRLEDMask,1,100); - } - erouter->removeListener(this); - } - protected: - karmedbanditExp3_1& b; //!< the class implimenting a k-armed bandit algorithm to pass the reward back to - bool reward; //!< true if a reward was received - MotionManager::MC_ID leds_id; //!< MC_ID of a LedMC - }; + //! Waits to see if a reward is received, lights up LEDs to let the user know + class WaitNode : public StateNode { + public: + //! constructor + /* @param n name to use for the node + * @param bandito the class to pass the reward to (if it comes) + */ + WaitNode(const char* n, karmedbanditExp3_1& bandito) + : StateNode("WaitNode",n), b(bandito), reward(false), leds_id(MotionManager::invalid_MC_ID) + { + leds_id=motman->addPersistentMotion(SharedObject()); + } + //! destructor + virtual ~WaitNode() { + motman->removeMotion(leds_id); + } + virtual void DoStart() { + StateNode::DoStart(); + erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID); + erouter->addTimer(this,0,1000,false); + MMAccessor leds(leds_id); + leds->cflash(BotLLEDMask+BotRLEDMask,1,1000); + } + virtual void DoStop() { + erouter->removeListener(this); + b.reward(reward); + cout << endl; + reward=false; + StateNode::DoStop(); + } + virtual void processEvent(const EventBase& event) { + if(event.getGeneratorID()==EventBase::timerEGID) { + sndman->playFile("whimper.wav"); + } else { + sndman->playFile("yipper.wav"); + reward=true; + MMAccessor leds(leds_id); + leds->cflash(MidLLEDMask+MidRLEDMask,1,100); + } + erouter->removeListener(this); + } + protected: + karmedbanditExp3_1& b; //!< the class implimenting a k-armed bandit algorithm to pass the reward back to + bool reward; //!< true if a reward was received + MotionManager::MC_ID leds_id; //!< MC_ID of a LedMC + }; - StareAtBallBehavior* stare; //!< active as long as we're in this state so it keeps an eye on the ball - StateNode* start; //!< used to start off by lying down before we start pressing buttons - MotionManager::MC_ID liedown; //!< a MotionSequence which will move the dog into a lying down posture - karmedbanditExp3_1 bandit; //!< algorithm to use in the k-armed bandit problem + StareAtBallBehavior* stare; //!< active as long as we're in this state so it keeps an eye on the ball + StateNode* start; //!< used to start off by lying down before we start pressing buttons + MotionManager::MC_ID liedown; //!< a MotionSequence which will move the dog into a lying down posture + karmedbanditExp3_1 bandit; //!< algorithm to use in the k-armed bandit problem private: - BanditMachine(const BanditMachine& node); //!< don't call this - BanditMachine operator=(const BanditMachine& node); //!< don't call this + BanditMachine(const BanditMachine& node); //!< don't call this + BanditMachine operator=(const BanditMachine& node); //!< don't call this }; /*! @file diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/BatteryMonitorBehavior.h ./Behaviors/Demos/BatteryMonitorBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Demos/BatteryMonitorBehavior.h 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Demos/BatteryMonitorBehavior.h 1969-12-31 19:00:00.000000000 -0500 @@ -1,165 +0,0 @@ -//-*-c++-*- -#ifndef INCLUDED_BatteryMonitorBehavior_h_ -#define INCLUDED_BatteryMonitorBehavior_h_ - -#include "Behaviors/BehaviorBase.h" -#include "Shared/debuget.h" -#include "Shared/WorldState.h" -#include "Events/EventRouter.h" -#include "IPC/SharedObject.h" -#include "Motion/MotionManager.h" -#include "Motion/PostureMC.h" -#include "Motion/LedMC.h" -#include "Shared/ERS210Info.h" -#include "Shared/ERS220Info.h" -#include "Shared/ERS7Info.h" -#include "Motion/MMAccessor.h" - -//! A background behavior which will monitor the power level and flip the ears when appropriate on a 210, or blink the headlight if a 220 -/*! Think of this as a simple example class. For exercise, try using a MotionSequenceMC instead - * of switching the ears back manually using a PostureMC */ -class BatteryMonitorBehavior : public BehaviorBase { -public: - static const unsigned int max_t=10000; //!< max time between ear flips when at "high power" mark - static const unsigned int high_power_p=20; //!< percent of 100 which is point at which to begin warning - static const unsigned int no_power_p=14; //!< percent of 100 at which power will fail (approximate!) - - //! constructor - BatteryMonitorBehavior() : BehaviorBase("BatteryMonitorBehavior"), pose(NULL), pose_id(MotionManager::invalid_MC_ID), led_id(MotionManager::invalid_MC_ID) {} - //! destructor - virtual ~BatteryMonitorBehavior() {} - - //! Listens for the PowerSourceID::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); - //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)); - } - //! Stops listening for events - virtual void DoStop() { - if(pose!=NULL) - stopWarning(); - erouter->removeListener(this); - BehaviorBase::DoStop(); - } - //! Adds a BatteryMonitorMC to motman if power goes low - virtual void processEvent(const EventBase &event) { - if(event.getGeneratorID()==EventBase::powerEGID) { - //just check for low power status - bool shouldwarn=shouldWarn(); - if(pose!=NULL && !shouldwarn) - stopWarning(); - else if(pose==NULL && shouldwarn) - startWarning(); - } else { - ASSERTRET(event.getGeneratorID()==EventBase::timerEGID,"Unrequested event "<setPriority(led_id,MotionManager::kEmergencyPriority+1); - MMAccessor led(led_id); - led->displayPercent(state->sensors[PowerRemainOffset],LedEngine::major,LedEngine::major); - } else - motman->setPriority(led_id,MotionManager::kIgnoredPriority); - erouter->addTimer(this,1,128+flipdelay,false); - } else { - motman->setPriority(led_id,MotionManager::kEmergencyPriority+1); - MMAccessor led(led_id); - led->displayPercent(state->sensors[PowerRemainOffset],LedEngine::major,LedEngine::major); - erouter->addTimer(this,2,128,false); - } - } break; - case 2: { // release ear until next flap, hide LEDs display - ASSERTRET(pose!=NULL,"Extra timer 1"); - setFlipper(false); - motman->setPriority(led_id,MotionManager::kIgnoredPriority); - erouter->addTimer(this,1,calcFlipDelay(),false); - } break; - default: - ASSERTRET(false,"Unrequested timer " << event.getName()); - break; - } - } - } - static std::string getClassDescription() { return "Reports the current battery status, and starts flicks the ears to warn when it gets too low"; } - 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]; } - -protected: - //! adds a pose and a timer to get the ears flipping - void startWarning() { - serr->printf("LOW BATTERY\n"); - pose_id=motman->addPersistentMotion(SharedObject(),MotionManager::kEmergencyPriority+1); - pose=(PostureMC*)motman->peekMotion(pose_id); - SharedObject led; - led->displayPercent(state->sensors[PowerRemainOffset],LedEngine::major,LedEngine::major); - led_id=motman->addPersistentMotion(led,MotionManager::kEmergencyPriority+1); - setFlipper(true); - erouter->addTimer(this,2,128,false); - } - //! removes pose, in case battery magically charges - void stopWarning() { - serr->printf("BATTERY GOOD\n"); - motman->removeMotion(pose_id); - motman->removeMotion(led_id); - led_id=pose_id=MotionManager::invalid_MC_ID; - pose=NULL; - erouter->removeTimer(this,1); - erouter->removeTimer(this,2); - } - //! makes the ears flip more rapidly as power declines. Flips back and forth once every 15 seconds at 15%, down to flipping constantly at 5%. - unsigned int calcFlipDelay() { - const float high_power=high_power_p/100.0; - const float no_power=no_power_p/100.0; - float cur_power=state->sensors[PowerRemainOffset]; - if(cur_powerrobotDesign & 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(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 - MotionManager::MC_ID led_id; //!< id of LedMC if we are currently warning, MotionManager::invalid_MC_ID otherwise - -private: - BatteryMonitorBehavior(const BatteryMonitorBehavior&); //!< don't copy behaviors - BatteryMonitorBehavior operator=(const BatteryMonitorBehavior&); //!< don't assign behaviors -}; - -/*! @file - * @brief Defines BatteryMonitorBehavior, a background behavior to trigger BatteryMonitorMC to warn when the power is low - * @author ejt (Creator) - * - * $Author: ejt $ - * $Name: HEAD $ - * $Revision: 1.1 $ - * $State: Exp $ - * $Date: 2006/10/04 04:21:12 $ - */ - -#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/CameraBehavior.cc ./Behaviors/Demos/CameraBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Demos/CameraBehavior.cc 2005-08-05 17:55:43.000000000 -0400 +++ ./Behaviors/Demos/CameraBehavior.cc 2006-09-18 14:07:57.000000000 -0400 @@ -16,6 +16,7 @@ #include "Vision/RawCameraGenerator.h" #include "Vision/InterleavedYUVGenerator.h" #include "Vision/JPEGGenerator.h" +#include "Vision/PNGGenerator.h" #include #include @@ -33,7 +34,7 @@ camera_click.setSourceID(ERS7Info::HeadButOffset); } initIndex(); - sndman->LoadFile("camera.wav"); + sndman->loadFile("camera.wav"); ledID=motman->addPersistentMotion(SharedObject()); erouter->addListener(this,camera_click); erouter->addListener(this,EventBase::textmsgEGID); @@ -41,7 +42,7 @@ void CameraBehavior::DoStop() { erouter->removeListener(this); - sndman->ReleaseFile("camera.wav"); + sndman->releaseFile("camera.wav"); motman->removeMotion(ledID); BehaviorBase::DoStop(); } @@ -49,7 +50,7 @@ /*! The format used depends on the current config settings. If JPEG * is the current choice, then a JPEG file will be written. - * Otherwise, RawCameraGenerator::SaveFile() will be called. + * Otherwise, RawCameraGenerator::saveFile() will be called. */ void CameraBehavior::processEvent(const EventBase& e) { @@ -78,19 +79,19 @@ if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_COLOR) { FilterBankGenerator * gen=ProjectInterface::defInterleavedYUVGenerator; // just an alias for readability gen->selectSaveImage(ProjectInterface::doubleLayer,InterleavedYUVGenerator::CHAN_YUV); - unsigned int len=gen->SaveFileStream(f); + unsigned int len=gen->saveFileStream(f); if(len==0) { serr->printf("Error saving file\n"); - sndman->PlayFile(config->controller.error_snd); + sndman->playFile(config->controller.error_snd); return; } } else if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_SINGLE_CHANNEL) { FilterBankGenerator * gen=ProjectInterface::defRawCameraGenerator; // just an alias for readability gen->selectSaveImage(ProjectInterface::doubleLayer,config->vision.rawcam_channel); - unsigned int len=gen->SaveFileStream(f); + unsigned int len=gen->saveFileStream(f); if(len==0) { serr->printf("Error saving file\n"); - sndman->PlayFile(config->controller.error_snd); + sndman->playFile(config->controller.error_snd); return; } } @@ -122,7 +123,7 @@ unsigned int writ=fwrite(imgbuf,jpeg->getImageSize(ProjectInterface::doubleLayer,chan),1,f); if(writ==0) { serr->printf("Error saving file\n"); - sndman->PlayFile(config->controller.error_snd); + sndman->playFile(config->controller.error_snd); return; } @@ -131,9 +132,37 @@ jpeg->setQuality(tmp_q); } - } - + } else if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_PNG) { + //save a JPEG 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) + png=dynamic_cast(ProjectInterface::defColorPNGGenerator); + else if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_SINGLE_CHANNEL) { + png=dynamic_cast(ProjectInterface::defGrayscalePNGGenerator); + chan=config->vision.rawcam_channel; + } + if(png!=NULL) { + // open file + FILE * f=openNextFile(".png"); + if(f==NULL) //error message already displayed in openNextFile() + return; + + //! write actual image data + unsigned char * imgbuf=png->getImage(ProjectInterface::doubleLayer,chan); + unsigned int writ=fwrite(imgbuf,png->getImageSize(ProjectInterface::doubleLayer,chan),1,f); + if(writ==0) { + serr->printf("Error saving file\n"); + sndman->playFile(config->controller.error_snd); + return; + } + + // close file + fclose(f); + } + } + { MMAccessor leds(ledID); leds->clear(); @@ -151,10 +180,10 @@ FILE * f=fopen(getNextName(ext).c_str(),"w+"); if(f==NULL) { serr->printf("Error opening file\n"); - sndman->PlayFile(config->controller.error_snd); + sndman->playFile(config->controller.error_snd); return NULL; } - sndman->PlayFile("camera.wav"); + sndman->playFile("camera.wav"); return f; } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/DrawVisObjBoundBehavior.h ./Behaviors/Demos/DrawVisObjBoundBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Demos/DrawVisObjBoundBehavior.h 2005-08-04 17:09:26.000000000 -0400 +++ ./Behaviors/Demos/DrawVisObjBoundBehavior.h 2006-06-30 12:37:03.000000000 -0400 @@ -52,9 +52,12 @@ chan=RawCameraGenerator::CHAN_Y; color=255; } else if(e.getGeneratorID()==EventBase::visSegmentEGID) { - layer=fbe.getNumLayers()-1-config->vision.rlecam_skip; - chan=config->vision.rlecam_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; + return; } //do the drawing @@ -65,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.rlecam_compression==Config::vision_config::COMPRESS_RLE) + if(config->vision.segcam_compression==Config::vision_config::COMPRESS_RLE) ProjectInterface::defRLEGenerator->invalidateCaches(); drawn++; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/ExploreMachine.cc ./Behaviors/Demos/ExploreMachine.cc --- ../Tekkotsu_2.4.1/Behaviors/Demos/ExploreMachine.cc 2004-12-03 19:10:38.000000000 -0500 +++ ./Behaviors/Demos/ExploreMachine.cc 2005-12-15 13:51:35.000000000 -0500 @@ -27,9 +27,9 @@ WalkNode * move=NULL; addNode(move=new WalkNode(getName()+"::move",150,0,0)); - move->setWalkID(walkid); + move->setMC(walkid); start=addNode(turn=new WalkNode(getName()+"::turn",0,0,0.5f)); - turn->setWalkID(walkid); + turn->setMC(walkid); move->addTransition(new SmoothCompareTrans(turn,&state->sensors[IRDistOffset],CompareTrans::LT,350,EventBase(EventBase::sensorEGID,SensorSourceID::UpdatedSID,EventBase::statusETID),.7)); turn->addTransition(new TimeOutTrans(move,2000)); @@ -42,7 +42,7 @@ StateNode::DoStart(); start->DoStart(); //erouter->addListener(this,EventBase::sensorEGID,SensorSourceID::UpdatedSID); - erouter->addListener(this,EventBase::stateMachineEGID,(unsigned int)turn,EventBase::activateETID); + erouter->addListener(this,EventBase::stateMachineEGID,(size_t)turn,EventBase::activateETID); } void ExploreMachine::DoStop() { diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/ExploreMachine.h ./Behaviors/Demos/ExploreMachine.h --- ../Tekkotsu_2.4.1/Behaviors/Demos/ExploreMachine.h 2005-01-24 17:23:46.000000000 -0500 +++ ./Behaviors/Demos/ExploreMachine.h 2005-12-15 13:51:36.000000000 -0500 @@ -3,6 +3,7 @@ #define INCLUDED_ExploreMachine_h_ #include "Behaviors/StateNode.h" +#include "Behaviors/Nodes/WalkNode.h" #include "Motion/MotionManager.h" //! A state machine for exploring an environment (or searching for an object) @@ -34,7 +35,7 @@ protected: StateNode * start; //!< the node to begin within on DoStart() (turn) - class WalkNode * turn; //!< walk node to use when turning + WalkNode * turn; //!< walk node to use when turning MotionManager::MC_ID walkid; //!< we want to share a walk between turning and walking nodes private: diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/FlashIPAddrBehavior.cc ./Behaviors/Demos/FlashIPAddrBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Demos/FlashIPAddrBehavior.cc 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Demos/FlashIPAddrBehavior.cc 1969-12-31 19:00:00.000000000 -0500 @@ -1,187 +0,0 @@ -#include "FlashIPAddrBehavior.h" - -#include "Events/EventRouter.h" -#include "Motion/MMAccessor.h" -#include "Motion/LedEngine.h" -#include "Shared/WorldState.h" -#include "Shared/Config.h" -#include "Sound/SoundManager.h" -#include "Wireless/Wireless.h" - -void FlashIPAddrBehavior::DoStart() { - BehaviorBase::DoStart(); // do this first - if(config->behaviors.flash_on_start) { - setupSequence(); - loadSounds(); - ms_id = motman->addPrunableMotion(ms,MotionManager::kEmergencyPriority+1); - erouter->addListener(this,EventBase::motmanEGID,ms_id,EventBase::deactivateETID); - } - erouter->addListener(this,EventBase::buttonEGID,button1); - erouter->addListener(this,EventBase::buttonEGID,button2); -} - -void FlashIPAddrBehavior::DoStop() { - erouter->removeListener(this); - motman->removeMotion(ms_id); - ms_id=MotionManager::invalid_MC_ID; - releaseSounds(); - BehaviorBase::DoStop(); // do this last -} - -void FlashIPAddrBehavior::processEvent(const EventBase& e) { - if(e.getGeneratorID()==EventBase::timerEGID) { - - if(e.getSourceID()==ACTIVATE_TIMER) { - //buttons have been held down long enough, time to run display - if(ms_id!=MotionManager::invalid_MC_ID) { - //there's already one running, have to check it out to clear it - MMAccessor ms_acc(ms_id); - setupSequence(); - } else - setupSequence(); - loadSounds(); - ms_id = motman->addPrunableMotion(ms); - erouter->addListener(this,EventBase::motmanEGID,ms_id,EventBase::deactivateETID); - - } else { //its time to play a digit sound file - //the source id was set to correspond to an element of the sounds vector - if(e.getSourceID()>=sounds.size()) - serr->printf("ERROR: %s received invalid timer event %s\n",getName().c_str(),e.getName().c_str()); - else { - sndman->PlayFile(sounds[e.getSourceID()]); - if(e.getSourceID()==sounds.size()-1) - releaseSounds(); - } - - } - - } 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 - if(e.getTypeID()==EventBase::activateETID) { - if(state->buttons[button1] && state->buttons[button2]) - erouter->addTimer(this,ACTIVATE_TIMER,2000,false); - } else if(e.getTypeID()==EventBase::deactivateETID) - erouter->removeTimer(this,ACTIVATE_TIMER); - - } else if(e.getGeneratorID()==EventBase::motmanEGID) { - // display has completed, mark it as such - if(e.getSourceID()!=ms_id) - serr->printf("WARNING: %s received event %s, doesn't match ms_id (%d)\n",getName().c_str(),e.getName().c_str(),ms_id); - ms_id=MotionManager::invalid_MC_ID; - erouter->removeListener(this,EventBase::motmanEGID); - - } -} - -void FlashIPAddrBehavior::loadSounds() { - for(unsigned int i=0; iLoadFile(sounds[i]); -} - -void FlashIPAddrBehavior::releaseSounds() { - for(unsigned int i=0; iReleaseFile(sounds[i]); - sounds.clear(); -} - -void FlashIPAddrBehavior::setupSequence() { - const unsigned int DISP_TIME=600; - const unsigned int GROUP_TIME=500; - const unsigned int DOT_TIME=400; - const unsigned int FADE_TIME=1; - const unsigned int BLANK_TIME=100-FADE_TIME*2; - erouter->removeTimer(this); - ms->clear(); - releaseSounds(); - unsigned int a=wireless->getIPAddress(); - unsigned int n=config->behaviors.flash_bytes; - if(n>4) - n=4; - LedEngine disp; - for(unsigned int i=n-1; i!=-1U; i--) { - unsigned int byte=(a>>(i*8))&0xFF; - unsigned int digits=1; - if(byte>=10) - digits++; - if(byte>=100) - digits++; - //cout << "byte " << i << " is " << byte << " -- " << digits << " digits" << endl; - //cout << "Setting LEDs: "; - for(unsigned int d=0; daddTimer(this,sounds.size(),ms->getTime()+delay,false); - sounds.push_back(soundfile); - for(unsigned int j=0; j(LEDOffset+j))) - //cout << j << ' '; - ms->setOutputCmd(LEDOffset+j,disp.getValue(static_cast(LEDOffset+j))); - } - ms->advanceTime(DISP_TIME); - for(unsigned int j=0; jsetOutputCmd(LEDOffset+j,disp.getValue(static_cast(LEDOffset+j))); - ms->advanceTime(FADE_TIME); - for(unsigned int j=0; jsetOutputCmd(LEDOffset+j,0); - ms->advanceTime(BLANK_TIME); - if(d==digits-1) - ms->advanceTime(GROUP_TIME); - for(unsigned int j=0; jsetOutputCmd(LEDOffset+j,0); - ms->advanceTime(FADE_TIME); - } - //cout << endl; - if(i!=0) { - LEDBitMask_t dot=1<robotDesign&WorldState::ERS210Mask) { - dot=LedEngine::ERS210numMasks[10]; - } else if(state->robotDesign&WorldState::ERS220Mask) { - dot=LedEngine::ERS220numMasks[10]; - } else if(state->robotDesign&WorldState::ERS7Mask) { - dot=LedEngine::ERS7numMasks[10]; - } - erouter->addTimer(this,sounds.size(),ms->getTime()+delay,false); - sounds.push_back("numbers/dot.wav"); - for(unsigned int j=0; jsetOutputCmd(LEDOffset+j,(dot>>j)&1); - ms->advanceTime(DOT_TIME); - for(unsigned int j=0; jsetOutputCmd(LEDOffset+j,(dot>>j)&1); - ms->advanceTime(FADE_TIME); - for(unsigned int j=0; jsetOutputCmd(LEDOffset+j,0); - ms->advanceTime(BLANK_TIME); - for(unsigned int j=0; jsetOutputCmd(LEDOffset+j,0); - ms->advanceTime(FADE_TIME); - } - } - ms->play(); -} - - -/*! @file - * @brief Implements FlashIPAddrBehavior, which displays IP address by flashing a series of numbers on the LED face panel - * @author ejt (Creator) - * - * $Author: ejt $ - * $Name: HEAD $ - * $Revision: 1.1 $ - * $State: Exp $ - * $Date: 2006/10/04 04:21:12 $ - */ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/FlashIPAddrBehavior.h ./Behaviors/Demos/FlashIPAddrBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Demos/FlashIPAddrBehavior.h 2005-08-07 00:11:03.000000000 -0400 +++ ./Behaviors/Demos/FlashIPAddrBehavior.h 1969-12-31 19:00:00.000000000 -0500 @@ -1,67 +0,0 @@ -//-*-c++-*- -#ifndef INCLUDED_FlashIPAddrBehavior_h_ -#define INCLUDED_FlashIPAddrBehavior_h_ - -#include "Behaviors/BehaviorBase.h" -#include "Motion/MotionManager.h" -#include "Motion/MotionSequenceMC.h" - -//! Displays IP address by speaking the digits and flashing a series of numbers on the LED face panel -/*! Will only run the display on DoStart() if the flash_on_start - * config variable is set. Otherwise you will need to hold down the - * buttons specified by #button1 and #button2 to trigger the display. - * Note that if the e-stop is active it will intercept the button - * events, so turn off e-stop first. */ -class FlashIPAddrBehavior : public BehaviorBase { -public: - //! constructor - FlashIPAddrBehavior() - : BehaviorBase("FlashIPAddrBehavior"), sounds(), ms(), ms_id(MotionManager::invalid_MC_ID) - {} - - virtual void DoStart(); //!< if the Config::behavior_config::flash_on_start flag is set, will setup and run - virtual void DoStop(); //!< halts any display which may be in progress - - //! Receives button events, timers, and motman manager pruning notifications - 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 "; - pre+=buttonNames[button1]; - pre+=" and "; - pre+=buttonNames[button2]; - pre+=" to trigger any time while running"; - return pre; - } - virtual std::string getDescription() const { return getClassDescription(); } - -protected: - typedef XLargeMotionSequenceMC MSMC_t; //!< used to flash the LEDs to report the IP address - - void loadSounds(); //!< loads the numeric sounds into memory - void releaseSounds(); //!< releases the numeric sounds - void setupSequence(); //!< construct the motion sequence for flashing leds, request timers to play corresponding sound file - - 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 - - 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) - static const unsigned int delay=64; //!< time (in milliseconds) to expect #ms to be delayed before it actually starts - - SharedObject ms; //!< motion sequence used to control the LEDs - MotionManager::MC_ID ms_id; //!< id number of #ms -}; - -/*! @file - * @brief Describes FlashIPAddrBehavior, which displays IP address by flashing a series of numbers on the LED face panel - * @author ejt (Creator) - * - * $Author: ejt $ - * $Name: HEAD $ - * $Revision: 1.1 $ - * $State: Exp $ - * $Date: 2006/10/04 04:21:12 $ - */ - -#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/FollowHeadBehavior.cc ./Behaviors/Demos/FollowHeadBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Demos/FollowHeadBehavior.cc 2004-11-10 20:45:36.000000000 -0500 +++ ./Behaviors/Demos/FollowHeadBehavior.cc 2006-09-05 16:30:21.000000000 -0400 @@ -53,7 +53,7 @@ } else if(e==head_release) { cout << "release" << endl; motman->addPrunableMotion(SharedObject(HeadOffset,HeadOffset+NumHeadJoints,0)); - erouter->addListener(this,clock); + erouter->addTimer(this,clock); } else if(e==head_lock) { cout << "lock" << endl; @@ -61,7 +61,7 @@ for(unsigned int i=HeadOffset; isetOutput(NULL,i,state->outputs[i]); //doing this prevents the head from jerking back when you released it to where it was before you pressed the button cout << state->outputs[HeadOffset+TiltOffset]/M_PI*180 << ' ' << state->outputs[HeadOffset+PanOffset]/M_PI*180 << ' ' << state->outputs[HeadOffset+RollOffset]/M_PI*180 << endl; - erouter->removeListener(this,clock); + erouter->removeTimer(this,clock.getSourceID()); } else { ASSERT(false,"unprocessed event " << e.getName() << endl); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/GroundPlaneBehavior.h ./Behaviors/Demos/GroundPlaneBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Demos/GroundPlaneBehavior.h 2004-12-22 20:47:06.000000000 -0500 +++ ./Behaviors/Demos/GroundPlaneBehavior.h 2006-03-03 18:08:39.000000000 -0500 @@ -61,7 +61,7 @@ } else if(e==head_release) { motman->addPrunableMotion(SharedObject(HeadOffset,HeadOffset+NumHeadJoints,0)); - erouter->addListener(this,clock); + erouter->addTimer(this,clock); processEvent(clock); } else if(e==head_lock) { motman->addPrunableMotion(SharedObject(HeadOffset,HeadOffset+NumHeadJoints,1)); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/LEDCounterBehavior.h ./Behaviors/Demos/LEDCounterBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Demos/LEDCounterBehavior.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Demos/LEDCounterBehavior.h 2006-02-02 15:08:12.000000000 -0500 @@ -0,0 +1,65 @@ +//-*-c++-*- +#ifndef INCLUDED_LEDCounterBehavior_h_ +#define INCLUDED_LEDCounterBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Events/EventRouter.h" +#include "Motion/LedMC.h" +#include "Motion/PostureMC.h" +#include "Motion/MMAccessor.h" + +//! Counts with LEDs on back button presses +class LEDCounterBehavior : public BehaviorBase { +public: + LEDCounterBehavior() + : BehaviorBase("LEDCounterBehavior"), + inc(EventBase::buttonEGID,FrontBackButOffset,EventBase::activateETID), + dec(EventBase::buttonEGID,RearBackButOffset,EventBase::activateETID), + sw(EventBase::buttonEGID,MiddleBackButOffset,EventBase::activateETID), + style(LedEngine::onedigit), cnt(0), led_id(MotionManager::invalid_MC_ID) {} + virtual void DoStart() { + BehaviorBase::DoStart(); + erouter->addListener(this,inc); + erouter->addListener(this,dec); + erouter->addListener(this,sw); + led_id=motman->addPersistentMotion(SharedObject()); + motman->addPrunableMotion(SharedObject("stand.pos")); + } + virtual void DoStop() { + motman->removeMotion(led_id); + erouter->removeListener(this); + BehaviorBase::DoStop(); + } + virtual void processEvent(const EventBase& e) { + if(e==inc) { + MMAccessor(led_id)->displayNumber(++cnt,style); + } else if(e==dec) { + MMAccessor(led_id)->displayNumber(--cnt,style); + } else if(e==sw) { + style = (style==LedEngine::onedigit ? LedEngine::twodigit : LedEngine::onedigit); + MMAccessor(led_id)->displayNumber(cnt,style); + } + } + + static std::string getClassDescription() { return "Counts with LEDs on back button presses"; } + virtual std::string getDescription() const { return getClassDescription(); } + +protected: + EventBase inc,dec,sw; + LedEngine::numStyle_t style; + int cnt; + MotionManager::MC_ID led_id; +}; + +/*! @file + * @brief Defines LEDCounterBehavior, which counts with LEDs on back button presses + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: HEAD $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2006/10/04 04:21:12 $ + */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/LogTestMachine.h ./Behaviors/Demos/LogTestMachine.h --- ../Tekkotsu_2.4.1/Behaviors/Demos/LogTestMachine.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Demos/LogTestMachine.h 2006-09-18 14:07:57.000000000 -0400 @@ -0,0 +1,147 @@ +//-*-c++-*- +#ifndef INCLUDED_LogTestMachine_h_ +#define INCLUDED_LogTestMachine_h_ + +#include "Behaviors/StateNode.h" +#include "Behaviors/Nodes/LedNode.h" +#include "Behaviors/Controls/EventLogger.h" +#include "Shared/ProjectInterface.h" +#include "Behaviors/Transitions/TextMsgTrans.h" +#include "Behaviors/Transitions/NullTrans.h" +#include "Vision/JPEGGenerator.h" + +//! a class for testing the image logging facilities +class ImageLogTestNode : public LedNode { +public: + //!constructor + ImageLogTestNode(std::string name, int n) + : LedNode("ImageLogTestNode",name) + { + getMC()->displayNumber(n,LedEngine::onedigit); + } + virtual void DoStart() { + LedNode::DoStart(); + EventLogger::logImage(*ProjectInterface::defColorJPEGGenerator,ProjectInterface::fullLayer,0,this); + sndman->playFile("camera.wav"); + } +}; + +//! a class for testing the text message logging facilities +class MessageLogTestNode : public LedNode { +public: + //!constructor + MessageLogTestNode(std::string name, int n) + : LedNode("MessageLogTestNode",name) + { + getMC()->displayNumber(n,LedEngine::onedigit); + } + virtual void DoStart() { + LedNode::DoStart(); + EventLogger::logMessage("Hello World!",this); //icon and placement arguments also available + } +}; + +//! a class for testing the external camera request facilities +class WebcamLogTestNode : public LedNode { +public: + //!constructor + WebcamLogTestNode(std::string name, int n) + : LedNode("WebcamLogTestNode",name) + { + getMC()->displayNumber(n,LedEngine::onedigit); + } + virtual void DoStart() { + LedNode::DoStart(); + EventLogger::logWebcam(this); + } +}; + + +//! tests different methods of the state machine viewer logging facilities +class LogTestMachine : public StateNode { + + // **************************** + // ******* CONSTRUCTORS ******* + // **************************** +public: + //! default constructor, use type name as instance name + LogTestMachine() + : StateNode("LogTestMachine","LogTestMachine"), start(NULL) + {} + + //! constructor, take an instance name + LogTestMachine(const std::string& nm) + : StateNode("LogTestMachine",nm), start(NULL) + {} + +protected: + //! constructor for subclasses (which would need to provide a different class name) + LogTestMachine(const std::string &class_name, const std::string &node_name) + : StateNode(class_name,node_name), start(NULL) + {} + + + // **************************** + // ********* METHODS ********** + // **************************** +public: + virtual void setup() { + StateNode::setup(); + + //setup sub-nodes + start=addNode(new StateNode("Waiting")); + ImageLogTestNode * img_node=new ImageLogTestNode("Image",1); addNode(img_node); + MessageLogTestNode * msg_node=new MessageLogTestNode("Message",2); addNode(msg_node); + WebcamLogTestNode * webcam_node=new WebcamLogTestNode("Webcam",3); addNode(webcam_node); + + //link with transitions + start->addTransition(new TextMsgTrans(img_node,"image")); + start->addTransition(new TextMsgTrans(msg_node,"message")); + start->addTransition(new TextMsgTrans(webcam_node,"webcam")); + img_node->addTransition(new NullTrans(start)); + msg_node->addTransition(new NullTrans(start)); + webcam_node->addTransition(new NullTrans(start)); + } + + virtual void DoStart() { + StateNode::DoStart(); // do this first (required) + start->DoStart(); + } + + static std::string getClassDescription() { return "Allows testing of various EventLogger facilities via text message events"; } + virtual std::string getDescription() const { return getClassDescription(); } + + + // **************************** + // ********* MEMBERS ********** + // **************************** +protected: + StateNode * start; //!< the subnode to begin within on DoStart() + + + // **************************** + // ********** OTHER *********** + // **************************** +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) + LogTestMachine(const LogTestMachine&); //!< don't call (copy constructor) + LogTestMachine& operator=(const LogTestMachine&); //!< don't call (assignment operator) +}; + + +/*! @file + * @brief Defines LogTestMachine, which allows testing of various EventLogger facilities via text message events + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: HEAD $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2006/10/04 04:21:12 $ + */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/LookAtPointBehavior.h ./Behaviors/Demos/LookAtPointBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Demos/LookAtPointBehavior.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Demos/LookAtPointBehavior.h 2006-09-22 12:59:27.000000000 -0400 @@ -0,0 +1,91 @@ +//-*-c++-*- +#ifndef INCLUDED_LookAtPointBehavior_h_ +#define INCLUDED_LookAtPointBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Motion/HeadPointerMC.h" +#include "Motion/Kinematics.h" +#include "Motion/MotionManager.h" +#include "Shared/newmat/newmat.h" + +//! A simple behavior to see how well LookAtPoint (inverse kinematics) is working +//! You may want to uncomment cout's in HeadPointer::LooAtPoint before running Behavior +class LookAtPointBehavior : public BehaviorBase { + public: + + LookAtPointBehavior() : + BehaviorBase("LookAtPointBehavior"), + pointer_id(MotionManager::invalid_MC_ID), gazePt(500,0,0) { srand(clock()); } + + virtual void DoStart() { + cout << "LookAtPointBehavior:DoStart()\n"; + BehaviorBase::DoStart(); + pointer_id = motman->addPersistentMotion(SharedObject()); + erouter->addListener(this, EventBase::motmanEGID, pointer_id, EventBase::statusETID); + erouter->addTimer(this, 0, 1000, false); + } + virtual void DoStop() { + motman->removeMotion(pointer_id); + erouter->removeListener(this); + BehaviorBase::DoStop(); + } + + virtual void processEvent(const EventBase& e) { + switch (e.getGeneratorID()) { + case EventBase::motmanEGID: + if (e.getSourceID() == pointer_id) { + erouter->addTimer(this, 0, 1000, false); // wait 1 sec for joints to stablize + } + break; + case EventBase::timerEGID: + if (e.getSourceID() == 0) { + NEWMAT::ColumnVector down=Kinematics::pack(state->sensors[BAccelOffset], + -state->sensors[LAccelOffset], + state->sensors[DAccelOffset]); + NEWMAT::ColumnVector p=kine->calculateGroundPlane(down); + + NEWMAT::ColumnVector ray(4); ray(1)=ray(2)=0; ray(3)=ray(4)=1; + NEWMAT::ColumnVector hit; + hit=kine->projectToPlane(CameraFrameOffset,ray,BaseFrameOffset,p,BaseFrameOffset); + + NEWMAT::ColumnVector groundPt = Kinematics::pack(gazePt.coordX(),gazePt.coordY(),gazePt.coordZ()); + NEWMAT::ColumnVector camPt = (kine->baseToJoint(CameraFrameOffset))*groundPt; + groundPt = kine->projectToPlane(CameraFrameOffset,camPt,BaseFrameOffset,p,BaseFrameOffset); + float theta = acos(camPt(3)/sqrt(camPt(1)*camPt(1)+camPt(2)*camPt(2)+camPt(3)*camPt(3))); + cout << "Result:\n Cam Center projected to GroundPlane: " << Point(hit(1),hit(2),hit(3)) << endl; + cout << " Gaze Point projected to GroundPlane: " << Point(groundPt(1),groundPt(2),groundPt(3)) << endl; + cout << " Gaze Point in CameraPlane: " << Point(camPt(1),camPt(2),camPt(3)) << endl; + cout << " theta: " << mathutils::rad2deg(theta) << " degrees\n"; + + gazePt.setCoords(rand()%1000-200, rand()%1000-500, rand()%400-200); //set next gaze point + erouter->addTimer(this, 1, 4000, false); + } + else { + cout << "\n\n\nLookAtPoint: " << gazePt << endl; + bool b = MMAccessor(pointer_id)->lookAtPoint(gazePt.coordX(), gazePt.coordY(), gazePt.coordZ()); + cout << " => " << (b ? "Reachable" : "Unreachable") << endl; + } + break; + default: + cout << "LookAtPointBehavior::ProcessEvent(): unknown event\n"; + break; + }; + } + +protected: + MotionManager::MC_ID pointer_id; + Point gazePt; +}; + +/*! @file +* @brief Defines LookAtPointBehavior, moves the head through a series of gaze points and reports if each is reachable to test inverse kinematic's lookAtPoint +* @author dst (Creator) +* +* $Author: ejt $ +* $Name: HEAD $ +* $Revision: 1.1 $ +* $State: Exp $ +* $Date: 2006/10/04 04:21:12 $ +*/ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/PaceTargetsMachine.cc ./Behaviors/Demos/PaceTargetsMachine.cc --- ../Tekkotsu_2.4.1/Behaviors/Demos/PaceTargetsMachine.cc 2005-08-03 15:11:58.000000000 -0400 +++ ./Behaviors/Demos/PaceTargetsMachine.cc 2006-09-18 14:07:57.000000000 -0400 @@ -60,10 +60,10 @@ tmptrans->setSound("barkmed.wav"); //preload the sounds so we don't pause on tranisitions - sndman->LoadFile("cutey.wav"); - sndman->LoadFile("barkmed.wav"); - sndman->LoadFile("whimper.wav"); - sndman->LoadFile("fart.wav"); + sndman->loadFile("cutey.wav"); + sndman->loadFile("barkmed.wav"); + sndman->loadFile("whimper.wav"); + sndman->loadFile("fart.wav"); //starts out exploring start=explGrp; @@ -76,10 +76,10 @@ void PaceTargetsMachine::teardown() { //release the sounds - sndman->ReleaseFile("cutey.wav"); - sndman->ReleaseFile("barkmed.wav"); - sndman->ReleaseFile("whimper.wav"); - sndman->ReleaseFile("fart.wav"); + sndman->releaseFile("cutey.wav"); + sndman->releaseFile("barkmed.wav"); + sndman->releaseFile("whimper.wav"); + sndman->releaseFile("fart.wav"); StateNode::teardown(); } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/RegionTestBehavior.h ./Behaviors/Demos/RegionTestBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Demos/RegionTestBehavior.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Demos/RegionTestBehavior.h 2006-02-07 14:45:00.000000000 -0500 @@ -0,0 +1,53 @@ +//-*-c++-*- +#ifndef INCLUDED_RegionTestBehavior_h_ +#define INCLUDED_RegionTestBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Events/EventRouter.h" +#include "Shared/ProjectInterface.h" +#include "Vision/RegionGenerator.h" +#include "Vision/SegmentedColorGenerator.h" + +//! Outputs some basic region statistics regarding the current frame to the console (and then stops again) +class RegionTestBehavior : public BehaviorBase { +public: + //! constructor + RegionTestBehavior() : BehaviorBase("RegionTest") {} + + virtual void DoStart() { + BehaviorBase::DoStart(); //always do first + + //long line: get the regions from the default region generator //have to cast it to the type actually returned by the generator in question (see documentation) RegionGenerator::region_stats * regions=(RegionGenerator::region_stats*)ProjectInterface::defRegionGenerator->getImage(ProjectInterface::fullLayer,0); + unsigned int num_colors=ProjectInterface::defSegmentedColorGenerator->getNumColors(); + + //do some processing on the regions for(unsigned int i=0; i0 && regions[i].list!=NULL) { //really only need to check one + //region list is sorted by size, so first entry (if list isn't empty) is the largest: + std::cout << "\tlargest region has area " << regions[i].list->area << std::endl; + //but you might prefer the total size of all regions of that color: std::cout << "\ttotal area is " << regions[i].total_area << std::endl; + } } + + //just do it once for the current frame at launch, now stop again + DoStop(); + } + + static std::string getClassDescription() { return "Outputs some basic region statistics regarding the current frame to the console (and then stops again)"; } + virtual std::string getDescription() const { return getClassDescription(); } + +}; + +/*! @file + * @brief Defines RegionTestBehavior, which outputs some basic region statistics to the console + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: HEAD $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2006/10/04 04:21:12 $ + */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/SoundTestBehavior.h ./Behaviors/Demos/SoundTestBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Demos/SoundTestBehavior.h 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Demos/SoundTestBehavior.h 2006-09-18 14:07:57.000000000 -0400 @@ -43,18 +43,18 @@ virtual void DoStart() { BehaviorBase::DoStart(); erouter->addListener(this,EventBase::buttonEGID); - sndman->LoadFile("yap.wav"); - sndman->LoadFile("howl.wav"); - sndman->LoadFile("whimper.wav"); + sndman->loadFile("yap.wav"); + sndman->loadFile("howl.wav"); + sndman->loadFile("whimper.wav"); } //! Release sounds we loaded in DoStart() virtual void DoStop() { BehaviorBase::DoStop(); erouter->removeListener(this); - sndman->ReleaseFile("howl.wav"); - sndman->ReleaseFile("yap.wav"); - sndman->ReleaseFile("whimper.wav"); + sndman->releaseFile("howl.wav"); + sndman->releaseFile("yap.wav"); + sndman->releaseFile("whimper.wav"); } //! Play the sound corresponding to the button @@ -75,7 +75,7 @@ curplay=SoundManager::invalid_Play_ID; endtime=0; } else if(pauseWhileChin) - sndman->ResumePlay(curplay); + sndman->resumePlay(curplay); } //! returns name to system @@ -89,20 +89,20 @@ // Just play the sound // This is probably all you need how to do unless you want to get fancy - sndman->PlayFile(name); + sndman->playFile(name); } else { // Enqueue the sound - mainly useful if you have a set of sounds and want to play a song with them if(curplay==SoundManager::invalid_Play_ID || !pauseWhileChin && get_time()>=endtime) { //start a new chain, either this is the first or we already finished playing the chain - curplay=sndman->PlayFile(name); + curplay=sndman->playFile(name); if(pauseWhileChin) - sndman->PausePlay(curplay); + sndman->pausePlay(curplay); } else //add to existing chain - sndman->ChainFile(curplay,name); - endtime=sndman->GetRemainTime(curplay)+get_time()-SoundBufferTime; - //-SoundBufferTime to guarrantee ID validity, see SoundManager::GetRemainTime() documentation + sndman->chainFile(curplay,name); + endtime=sndman->getRemainTime(curplay)+get_time()-SoundBufferTime; + //-SoundBufferTime to guarrantee ID validity, see SoundManager::getRemainTime() documentation } } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/StareAtBallBehavior.cc ./Behaviors/Demos/StareAtBallBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Demos/StareAtBallBehavior.cc 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Demos/StareAtBallBehavior.cc 2006-09-26 17:47:00.000000000 -0400 @@ -3,6 +3,7 @@ #include "Events/VisionObjectEvent.h" #include "Shared/WorldState.h" #include "Motion/HeadPointerMC.h" +#include "Motion/MMAccessor.h" #include "Shared/ProjectInterface.h" //! Converts degrees to radians @@ -22,33 +23,29 @@ //this could be cleaned up event-wise (only use a timer when out of view) void StareAtBallBehavior::processEvent(const EventBase& event) { - /*if(event.getSourceID()==ProjectInterface::visBlueBallSID) { - cout << "BLUE" << endl; - } else if(event.getSourceID()==ProjectInterface::visPinkBallSID) { - cout << "PINK" << endl; - } - */ - - static float horiz=0,vert=0; + float horiz=0,vert=0; if(event.getGeneratorID()==EventBase::visObjEGID && event.getTypeID()==EventBase::statusETID) { - horiz=static_cast(&event)->getCenterX(); - vert=static_cast(&event)->getCenterY(); + const VisionObjectEvent& objev=static_cast(event); + horiz=objev.getCenterX(); + vert=objev.getCenterY(); } //cout << horiz << ' ' << vert << endl; - // Very simple visual servoing control -- move the head in the direction of the target + // 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; - HeadPointerMC * headpointer= (HeadPointerMC*)motman->checkoutMotion(headpointer_id); -#ifdef TGT_ERS7 - //on an ers-7, we want to set the nod joint to look up, 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); -#endif - motman->checkinMotion(headpointer_id); + + // now request access to the headpointer we added in DoStart and set the joint angles + MMAccessor headpointer(headpointer_id); + if(state->robotDesign & WorldState::ERS7Mask) { + //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); + } } /*! @file diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/TestBehaviors.cc ./Behaviors/Demos/TestBehaviors.cc --- ../Tekkotsu_2.4.1/Behaviors/Demos/TestBehaviors.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Demos/TestBehaviors.cc 2006-09-18 14:07:57.000000000 -0400 @@ -0,0 +1,53 @@ +#include "TestBehaviors.h" +#include "Vision/PNGGenerator.h" +#include "Sound/SoundManager.h" +#include + +//better to put this here instead of the header +using namespace std; + +void SaveImagePyramidBehavior::saveImage(unsigned int layer, unsigned int chan, const std::string& name) { + const unsigned int MAXPATH=128; + char fname[MAXPATH]; + snprintf(fname,MAXPATH,name.c_str(),layer); + FILE * f=fopen(fname,"w+"); + if(f==NULL) { + int err=errno; + serr->printf("Error opening file %s: %s\n",fname,strerror(err)); + sndman->playFile(config->controller.error_snd); + return; + } + PNGGenerator * gen=dynamic_cast(ProjectInterface::defGrayscalePNGGenerator); + unsigned char * img=gen->getImage(layer,chan); + if(!gen->getImageCached(layer,chan)) { + serr->printf("A PNG encoding error occurred whiel saving file %s\n",fname); + sndman->playFile(config->controller.error_snd); + //return; + } + unsigned int size=gen->getImageSize(layer,chan); + unsigned int writ=fwrite(img,size,1,f); + if(writ==0) { + int err=errno; + serr->printf("Error saving file %s: %s\n",fname,strerror(err)); + sndman->playFile(config->controller.error_snd); + return; + } + if(fclose(f)!=0) { + int err=errno; + serr->printf("Error closing file %s: %s\n",fname,strerror(err)); + sndman->playFile(config->controller.error_snd); + return; + } + sout->printf("Saved %s\n",fname); +} + +/*! @file + * @brief + * @author Ethan Tira-Thompson (ejt) (Creator) + * + * $Author: ejt $ + * $Name: HEAD $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2006/10/04 04:21:12 $ + */ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/TestBehaviors.h ./Behaviors/Demos/TestBehaviors.h --- ../Tekkotsu_2.4.1/Behaviors/Demos/TestBehaviors.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Demos/TestBehaviors.h 2006-09-05 15:40:07.000000000 -0400 @@ -0,0 +1,107 @@ +//-*-c++-*- +#ifndef INCLUDED_TestBehaviors_h_ +#define INCLUDED_TestBehaviors_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Events/EventRouter.h" +#include "Events/TextMsgEvent.h" +#include "Motion/MotionManager.h" +#include "Motion/PostureMC.h" +#include "Shared/ProjectInterface.h" +#include "Vision/RawCameraGenerator.h" +#include "Shared/Config.h" + +//! Adds a MotionCommand and then immediately removes it again +class InstantMotionTestBehavior : public BehaviorBase { +public: + InstantMotionTestBehavior() : BehaviorBase("InstantMotionTestBehavior") {} //!< constructor + virtual void DoStart() { BehaviorBase::DoStart(); motman->removeMotion(motman->addPersistentMotion(SharedObject())); DoStop(); } + static std::string getClassDescription() { return "Adds a MotionCommand and then immediately removes it again"; } + virtual std::string getDescription() const { return getClassDescription(); } +}; + +//! When started, busy loops for 3 seconds and then stops +class BusyLoopTestBehavior : public BehaviorBase { +public: + BusyLoopTestBehavior() : BehaviorBase("BusyLoopTestBehavior") {} //!< constructor + virtual void DoStart() { BehaviorBase::DoStart(); unsigned int t=get_time(); while(get_time()-t<3000) {} DoStop(); } + static std::string getClassDescription() { return "When started, busy loops for 3 seconds and then stops"; } + virtual std::string getDescription() const { return getClassDescription(); } +}; + +//! Adds a MotionCommand which will busy loop for 3 seconds on its first update, and then autoprune +class BusyMCTestBehavior : public BehaviorBase { + //! on first updateOutputs, blocks for 3 seconds, then autoprunes on second update + class BusyMC : public MotionCommand { + bool hasRun; + public: + BusyMC() : MotionCommand(), hasRun(false) {} //!< constructor + virtual int updateOutputs() {unsigned int t=get_time(); while(get_time()-t<3000) {} hasRun=true; return 0; } + virtual int isDirty() { return true; } + virtual int isAlive() { return !hasRun; } + }; +public: + BusyMCTestBehavior() : BehaviorBase("BusyMCTestBehavior") {} //!< constructor + virtual void DoStart() { BehaviorBase::DoStart(); motman->addPrunableMotion(SharedObject()); DoStop(); } + static std::string getClassDescription() { return "Adds a MotionCommand which will busy loop for 3 seconds on its first update, and then autoprune"; } + virtual std::string getDescription() const { return getClassDescription(); } +}; + +//! Stops itself after a second via timer event +class SuicidalBehavior : public BehaviorBase { +public: + SuicidalBehavior() : BehaviorBase("SuicidalBehavior") {} //!< constructor + ~SuicidalBehavior() { std::cout << getName() << " destructed" << 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"; } + virtual std::string getDescription() const { return getClassDescription(); } +}; + +//! Echos any text message events received to the console (doesn't add a listener, relies on direct BehaviorSwitchControl passing) +class EchoTextBehavior : public BehaviorBase { +public: + EchoTextBehavior() : BehaviorBase("EchoTextBehavior") {} //!< constructor + virtual void processEvent(const EventBase& event) { if(const TextMsgEvent* tev=dynamic_cast(&event)) std::cout << tev->getText() << std::endl; else std::cout << getName() << " got a non-TextMsgEvent" << std::endl; } + static std::string getClassDescription() { return "Echos any text message events received to the console (doesn't add a listener, relies on direct BehaviorSwitchControl passing)"; } + virtual std::string getDescription() const { return getClassDescription(); } +}; + +//! Saves all images currently provided by the hardware to a series of PNG files on the memory stick +/*! Handy for examining the different channels/resolution levels provided by the system, all based on the same input */ +class SaveImagePyramidBehavior : public BehaviorBase { +public: + SaveImagePyramidBehavior() : BehaviorBase("SaveImagePyramidBehavior") {} //!< constructor + virtual void DoStart() { BehaviorBase::DoStart(); erouter->addListener(this,EventBase::textmsgEGID); } + virtual void processEvent(const EventBase& event) { + if(const TextMsgEvent* tev=dynamic_cast(&event)) { + std::string prefix=config->portPath(tev->getText().substr(0,4)); + saveImage(ProjectInterface::doubleLayer,RawCameraGenerator::CHAN_Y,prefix+"%dy.png"); + for(unsigned int layer=ProjectInterface::fullLayer; layer>=ProjectInterface::quarterLayer; layer--) { + saveImage(layer,RawCameraGenerator::CHAN_Y,prefix+"%dy.png"); + saveImage(layer,RawCameraGenerator::CHAN_U,prefix+"%du.png"); + saveImage(layer,RawCameraGenerator::CHAN_V,prefix+"%dv.png"); + saveImage(layer,RawCameraGenerator::CHAN_Y_DX,prefix+"%ddx.png"); + saveImage(layer,RawCameraGenerator::CHAN_Y_DY,prefix+"%ddy.png"); + saveImage(layer,RawCameraGenerator::CHAN_Y_DXDY,prefix+"%ddxy.png"); + } + } else std::cout << getName() << " got a non-TextMsgEvent" << std::endl; + } + static std::string getClassDescription() { return "Saves all images currently provided by the hardware to a series of PNG files on the memory stick"; } + virtual std::string getDescription() const { return getClassDescription(); } +protected: + void saveImage(unsigned int layer, unsigned int chan, const std::string& name); +}; + +/*! @file + * @brief A collection of small test behaviors (some would call them unit tests) + * @author Ethan Tira-Thompson (ejt) (Creator) + * + * $Author: ejt $ + * $Name: HEAD $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2006/10/04 04:21:12 $ + */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/WallTestBehavior.cc ./Behaviors/Demos/WallTestBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Demos/WallTestBehavior.cc 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Demos/WallTestBehavior.cc 2006-07-13 13:25:50.000000000 -0400 @@ -7,6 +7,7 @@ #include "Shared/Config.h" #include "Shared/WorldState.h" #include "Shared/newmat/newmatio.h" +#include "Shared/TimeET.h" #include void diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Demos/WorldStateVelDaemon.h ./Behaviors/Demos/WorldStateVelDaemon.h --- ../Tekkotsu_2.4.1/Behaviors/Demos/WorldStateVelDaemon.h 2005-06-29 18:00:03.000000000 -0400 +++ ./Behaviors/Demos/WorldStateVelDaemon.h 1969-12-31 19:00:00.000000000 -0500 @@ -1,95 +0,0 @@ -//-*-c++-*- -#ifndef INCLUDED_WorldStateVelDaemon_h_ -#define INCLUDED_WorldStateVelDaemon_h_ - -#include "Behaviors/BehaviorBase.h" -#include "Events/EventRouter.h" -#include "Events/LocomotionEvent.h" -#include "Shared/WorldState.h" -#include "Events/EventTrapper.h" - -//! Listens for LocomotionEvents and updates the velocity fields of WorldState -/*! If we get multiple ways of locomoting, this would be a good place - * to manage them to determine the actual final velocity. - * - * Right now it'll correctly handle one (or more i suppose) e-stops - * with a single other locomotor. But if there's two active - * locomotors, I dunno how to handle that. - */ -class WorldStateVelDaemon : public BehaviorBase, public EventTrapper { -public: - //! constructor - WorldStateVelDaemon() : BehaviorBase("WorldStateVelDaemon"), estopTime(1), old_x(0), old_y(0), old_a(0) {} - - virtual void DoStart() { - BehaviorBase::DoStart(); // do this first - erouter->addTrapper(this,EventBase::locomotionEGID); - erouter->addListener(this,EventBase::estopEGID); - } - - virtual void DoStop() { - erouter->removeListener(this); - erouter->removeTrapper(this); - BehaviorBase::DoStop(); // do this last - } - - //! traps locomotion events - will filter them out if currently in EStop - virtual bool trapEvent(const EventBase& e) { - const LocomotionEvent& le=dynamic_cast(e); - old_x=le.x; - old_y=le.y; - old_a=le.a; - if(!estopTime) { - state->vel_x=le.x; - state->vel_y=le.y; - state->vel_a=le.a; - state->vel_time=le.getTimeStamp(); - return false; - } - return true; - } - - virtual void processEvent(const EventBase& e) { - if(e.getTypeID()==EventBase::deactivateETID) { - if(estopTime) { - estopTime=0; - LocomotionEvent *le = new LocomotionEvent(EventBase::locomotionEGID,e.getSourceID(),EventBase::statusETID,e.getTimeStamp()-state->vel_time); - le->setXYA(old_x,old_y,old_a); - erouter->postEvent(le); - } - } else { - if(!estopTime) { - float older_x=old_x; - float older_y=old_y; - float older_a=old_a; - erouter->postEvent(new LocomotionEvent(EventBase::locomotionEGID,e.getSourceID(),EventBase::statusETID,e.getTimeStamp()-state->vel_time)); - estopTime=e.getTimeStamp(); - old_x=older_x; - old_y=older_y; - old_a=older_a; - } - } - } - - static std::string getClassDescription() { return "Keeps the WorldState's velocity fields up to date"; } - virtual std::string getDescription() const { return getClassDescription(); } - -protected: - unsigned int estopTime; //!< time estop activation was received - float old_x; //!< current velocity of underlying locomotor - float old_y; //!< current velocity of underlying locomotor - float old_a; //!< current velocity of underlying locomotor -}; - -/*! @file - * @brief Defines WorldStateVelDaemon, which listens for LocomotionEvents and updates the velocity fields of WorldState - * @author ejt (Creator) - * - * $Author: ejt $ - * $Name: HEAD $ - * $Revision: 1.1 $ - * $State: Exp $ - * $Date: 2006/10/04 04:21:12 $ - */ - -#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/Aibo3DControllerBehavior.cc ./Behaviors/Mon/Aibo3DControllerBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Mon/Aibo3DControllerBehavior.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Mon/Aibo3DControllerBehavior.cc 2006-08-03 18:58:39.000000000 -0400 @@ -0,0 +1,35 @@ +#include "Aibo3DControllerBehavior.h" +#include "Behaviors/Controls/BehaviorSwitchControl.h" + +BehaviorSwitchControlBase* Aibo3DControllerBehavior::stateSerializerControl=NULL; + +void Aibo3DControllerBehavior::DoStart() { + // Behavior startup + BehaviorBase::DoStart(); + + launchedSerializer=false; + if(stateSerializerControl!=NULL) { + if(!stateSerializerControl->isRunning()) { + stateSerializerControl->start(); + launchedSerializer=true; + } + // open gui + /* std::vector tmp; + tmp.push_back("Aibo3D Load Instructions"); + tmp.push_back("To load Aibo3D, you will need to install java3d\nand then run Tekkotsu/tools/aibo3d/"); + tmp.back()+=getGUIType(); + Controller::loadGUI("ControllerMsg","LoadAibo3d",getPort(),tmp);*/ + } + + Controller::loadGUI(getGUIType(),getGUIType(),getPort()); +} + +void Aibo3DControllerBehavior::DoStop() { + Controller::closeGUI(getGUIType()); + if(launchedSerializer && stateSerializerControl!=NULL) { + stateSerializerControl->stop(); + } + // Total behavior stop + BehaviorBase::DoStop(); +} + diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/Aibo3DControllerBehavior.h ./Behaviors/Mon/Aibo3DControllerBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Mon/Aibo3DControllerBehavior.h 2004-11-10 20:45:36.000000000 -0500 +++ ./Behaviors/Mon/Aibo3DControllerBehavior.h 2006-09-16 02:28:07.000000000 -0400 @@ -13,119 +13,46 @@ #include "Behaviors/Controller.h" #include "Shared/WorldState.h" -//! gets input from the GUI -int aibo3dcontrollercmd_callback(char *buf, int bytes); -class Aibo3DControllerBehavior; -//! so aibo3dcontrollercmd_callback knows where to send the input from the GUI -Aibo3DControllerBehavior *aibo3dControllerBehavior = NULL; +class BehaviorSwitchControlBase; +class Aibo3DControllerBehavior; - //! Listens to aibo3d control commands coming in from the command port. class Aibo3DControllerBehavior : public BehaviorBase { - protected: - MotionManager::MC_ID rcontrol_id; //!< remote controller motion command's id - - //! The input command stream socket - Socket *cmdsock; - - float val[NumPIDJoints]; //!< the value to use for each of the PID joints - char *fbuf; //!< alias to val - unsigned int pos; //!< a counter to know when we've gotten 4 frames - private: Aibo3DControllerBehavior(const Aibo3DControllerBehavior&); //!< don't call Aibo3DControllerBehavior operator=(const Aibo3DControllerBehavior&); //!< don't call - + //! so we can start the serializer behavior if it's not already running + static BehaviorSwitchControlBase* stateSerializerControl; + //! if true, indicates we launched the WorldState serializer, so we should stop it again if we stop + bool launchedSerializer; + public: //! constructor - Aibo3DControllerBehavior() : - BehaviorBase("Aibo3DControllerBehavior"), - rcontrol_id(MotionManager::invalid_MC_ID), - cmdsock(NULL), - fbuf((char*)val), pos(0) - { aibo3dControllerBehavior = this; } + Aibo3DControllerBehavior() : BehaviorBase("Aibo3DControllerBehavior"),launchedSerializer(false) { } //! destructor - virtual ~Aibo3DControllerBehavior() { aibo3dControllerBehavior = NULL; } - - //! processes input from the GUI - int registerData(char *buf, int bytes) { - int read=0; - while (readcheckoutMotion(rcontrol_id); - for (unsigned int i=0; icmds[i]=val[i]; - rcontrol->setDirty(); - motman->checkinMotion(rcontrol_id); - } + virtual void DoStart(); + virtual void DoStop(); - virtual void DoStart() { - // Behavior startup - BehaviorBase::DoStart(); - for(unsigned int i=0; ioutputs[i]; - // Enable remote control stream - rcontrol_id = motman->addPersistentMotion(SharedObject()); - updateRC(); - // Turn on wireless - cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 2048, 2048); - wireless->setReceiver(cmdsock->sock, aibo3dcontrollercmd_callback); - wireless->setDaemon(cmdsock,true); - wireless->listen(cmdsock->sock, config->main.aibo3d_port); - // open gui - /* std::vector tmp; - tmp.push_back("Aibo3D Load Instructions"); - tmp.push_back("To load Aibo3D, you will need to install java3d\nand then run Tekkotsu/tools/aibo3d/"); - tmp.back()+=getGUIType(); - Controller::loadGUI("ControllerMsg","LoadAibo3d",getPort(),tmp);*/ - Controller::loadGUI(getGUIType(),getGUIType(),getPort()); - } - - virtual void DoStop() { - Controller::closeGUI(getGUIType()); - // Close socket; turn wireless off - wireless->setDaemon(cmdsock,false); - wireless->close(cmdsock); - // Disable remote control - motman->removeMotion(rcontrol_id); - // Total behavior stop - BehaviorBase::DoStop(); - } - //! returns string corresponding to the Java GUI which should be launched - virtual std::string getGUIType() const { return "org.tekkotsu.aibo3d.Aibo3DPick"; } + virtual std::string getGUIType() const { return "org.tekkotsu.aibo3d.Aibo3D"; } //! returns port number the Java GUI should connect to - virtual unsigned int getPort() const { return config->main.aibo3d_port; } + virtual unsigned int getPort() const { return config->main.wsjoints_port; } static std::string getClassDescription() { - char tmp[20]; - sprintf(tmp,"%d",config->main.aibo3d_port); - return std::string("Listens to aibo3d control commands coming in from port ")+tmp; + return "Launches a WorldStateSerializer and asks gui to load Aibo3D GUI"; + //char tmp[20]; + //sprintf(tmp,"%d",config->main.aibo3d_port); + //return std::string("Listens to aibo3d control commands coming in from port ")+tmp; } virtual std::string getDescription() const { return getClassDescription(); } + //! sets the BehaviorSwitchControlBase which should be used to activate the serialization of WorldState data for the Aibo3D client to read + static void setSerializerControl(BehaviorSwitchControlBase* ctrl) { stateSerializerControl=ctrl; } }; - -int aibo3dcontrollercmd_callback(char *buf, int bytes) { - if (aibo3dControllerBehavior!=NULL) - return aibo3dControllerBehavior->registerData(buf, bytes); - return 0; -} - /*! @file * @brief Defines Aibo3DControllerBehavior, which listens to commands from the Aibo3D gui and shows current state * @author alokl (Creator) diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/CameraStreamBehavior.cc ./Behaviors/Mon/CameraStreamBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Mon/CameraStreamBehavior.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Mon/CameraStreamBehavior.cc 2006-09-09 00:32:27.000000000 -0400 @@ -0,0 +1,103 @@ +#include "CameraStreamBehavior.h" +#include "Wireless/Socket.h" +#include "Motion/PostureEngine.h" +#include "Events/EventRouter.h" +#include "Shared/LoadSave.h" +#include "Shared/Config.h" +#include "Shared/WorldState.h" + +#if DEBUG +void CameraStreamBehavior::processEvent(const EventBase& e) { + ASSERTRET(e.getGeneratorID()==EventBase::sensorEGID,"unexpected event"); +#else +void CameraStreamBehavior::processEvent(const EventBase& /*e*/) { +#endif + sendSensors(); +} + +int CameraStreamBehavior::receiveData(char* data, unsigned int len) { + std::string s(data,len); + //cout << "Console Received: " << s << endl; + + static std::string incomplete; + + //pass a line at a time to the controller + while(s.size()>0) { + std::string::size_type endline=s.find('\n'); + if(endline==std::string::npos) { + incomplete+=s; + return 0; + } + + //strip a \r\n or a \n + if(endline>0 && s[endline-1]=='\r') + incomplete+=s.substr(0,endline-1); + else + incomplete+=s.substr(0,endline); + + //is now complete + if(incomplete=="refreshSensors") { + sendSensors(); + } else if(incomplete=="startSensors") { + if(sensorListeners++ == 0) + erouter->addListener(this,EventBase::sensorEGID); + } else if(incomplete=="stopSensors") { + if(sensorListeners==0) + serr->printf("WARNING: %s sensor listener underflow",getName().c_str()); + else if(--sensorListeners == 0) + erouter->removeListener(this,EventBase::sensorEGID); + } + incomplete.erase(); + s=s.substr(endline+1); + } + return 0; +} + +void CameraStreamBehavior::sendSensors() { + if ((state->lastSensorUpdateTime - lastProcessedTime) < config->main.worldState_interval) // not enough time has gone by + return; + lastProcessedTime = state->lastSensorUpdateTime; + + ASSERT(LoadSave::stringpad==sizeof(unsigned int)+sizeof(char),"LoadSave::encode(string) format has changed?"); + PostureEngine pose; + pose.takeSnapshot(); + pose.setWeights(1); + pose.setSaveFormat(true,state); + unsigned int len=pose.getBinSize()+LoadSave::stringpad; + byte* buf=curSocket->getWriteBuffer(len); + if(buf==NULL) { + serr->printf("Unable to serialize sensor data for camera image -- network overflow"); + return; + } + unsigned int used; + if((used=pose.saveBuffer((char*)buf+sizeof(unsigned int),len-LoadSave::stringpad))==0) { + cerr << "An error occured during sensor serialization" << endl; + curSocket->write(0); + return; + } + if(used!=len-LoadSave::stringpad-1) + std::cout << "Warning: used==" << used << " len==" << len << std::endl; + //add the LoadSave fields (prepend length, append '\0') + len=LoadSave::encode(used,reinterpret_cast(buf),LoadSave::getSerializedSize(used)); + if(len==0) { + cerr << "An error occured during serialization of buffer length" << endl; + curSocket->write(0); + return; + } + buf[used+sizeof(used)]='\0'; + curSocket->write(used+LoadSave::stringpad); + //std::cout << "Sent sensors " << used << std::endl; +} + + + +/*! @file + * @brief Defines CameraStreamBehavior, which is the base class for camera streaming communication classes, handles upstream communication + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: HEAD $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2006/10/04 04:21:12 $ + */ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/CameraStreamBehavior.h ./Behaviors/Mon/CameraStreamBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Mon/CameraStreamBehavior.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Mon/CameraStreamBehavior.h 2006-03-03 10:33:05.000000000 -0500 @@ -0,0 +1,68 @@ +//-*-c++-*- +#ifndef INCLUDED_CameraStreamBehavior_h_ +#define INCLUDED_CameraStreamBehavior_h_ + +#include "Behaviors/BehaviorBase.h" + +class Socket; + +//! Base class for camera streaming communication classes, handles upstream communication +/*! This class isn't meant to be run directly -- it just provides common functionality for its subclasses. */ +class CameraStreamBehavior : public BehaviorBase { +public: + + virtual void processEvent(const EventBase& e); + + static std::string getClassDescription() { return "Base class for camera streaming communication classes, handles upstream communication"; } + virtual std::string getDescription() const { return getClassDescription(); } + + int receiveData(char* data, unsigned int len); //!< called when new data is available (currently, only to toggle sensor sending) + + void sendSensors(); //!< causes current sensor values to be sent through #curSocket (along with video data) + +protected: + //! constructor, protected because you're not intended to instantiate this directly + /*! @param name the name of the instance and the class + * @param s the subclass's socket, a reference is stored so CameraStreamBehavior will always have access to the current socket */ + CameraStreamBehavior(const std::string& name, Socket*& s) + : BehaviorBase(name), curSocket(s), sensorListeners(0), lastProcessedTime(0) + {} + //! constructor, protected because you're not intended to instantiate this directly + /*! @param classname the name of the class type + * @param name the name of the instance + * @param s the subclass's socket, a reference is stored so CameraStreamBehavior will always have access to the current socket */ + CameraStreamBehavior(const std::string& classname, const std::string& name, Socket*& s) + : BehaviorBase(classname,name), curSocket(s), sensorListeners(0), lastProcessedTime(0) + {} + + //! the socket over which to send updates + Socket*& curSocket; + + //! number of times startSensors has been sent, minus number of times stopSensors has been sent + unsigned int sensorListeners; + + //! timestamp of last sensor update sent + unsigned int lastProcessedTime; + +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) + CameraStreamBehavior(const CameraStreamBehavior&); //!< don't call (copy constructor) + CameraStreamBehavior& operator=(const CameraStreamBehavior&); //!< don't call (assignment operator) +}; + +/*! @file + * @brief Defines CameraStreamBehavior, which is the base class for camera streaming communication classes, handles upstream communication + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: HEAD $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2006/10/04 04:21:12 $ + */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/EchoBehavior.h ./Behaviors/Mon/EchoBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Mon/EchoBehavior.h 2005-08-07 00:11:03.000000000 -0400 +++ ./Behaviors/Mon/EchoBehavior.h 2006-09-16 02:28:07.000000000 -0400 @@ -9,7 +9,7 @@ class EchoBehavior : public BehaviorBase { public: static EchoBehavior * theOne; //!< the singleton object (only one of these objects can be active at a time or they would conflict over ports) - static unsigned short port; //! the port to listen on for incoming UDP and TCP connections + static unsigned short port; //!< the port to listen on for incoming UDP and TCP connections static int server_callbackT(char *buf, int bytes); //!< called by wireless when there's new data static int client_callbackT(char *buf, int bytes); //!< called by wireless when there's new data static int server_callbackU(char *buf, int bytes); //!< called by wireless when there's new data @@ -27,6 +27,7 @@ route[i][i]=true; } } + //! destructor ~EchoBehavior() { theOne=NULL; } virtual void DoStart(); @@ -41,6 +42,7 @@ virtual std::string getDescription() const { return getClassDescription(); } protected: + //! indicates one of the available data sinks: combinations of client/server and TCP/UDP enum routeIndex_t { STCP=0, //!< server TCP SUDP, //!< server UDP diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/HeadPointControllerBehavior.cc ./Behaviors/Mon/HeadPointControllerBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Mon/HeadPointControllerBehavior.cc 2005-06-10 15:18:59.000000000 -0400 +++ ./Behaviors/Mon/HeadPointControllerBehavior.cc 2006-09-10 13:00:43.000000000 -0400 @@ -6,18 +6,22 @@ HeadPointControllerBehavior* HeadPointControllerBehavior::theOne = NULL; void HeadPointControllerBehavior::runCommand(unsigned char *command) { - // First, turn off the stop-if-no-heartbeat timer - erouter->removeTimer(this); - // Extract the command parameter float param; unsigned char *paramp = (unsigned char *) ¶m; +#if defined(BYTE_ORDER) && BYTE_ORDER==BIG_ENDIAN + paramp[0] = command[4]; + paramp[1] = command[3]; + paramp[2] = command[2]; + paramp[3] = command[1]; +#else paramp[0] = command[1]; paramp[1] = command[2]; paramp[2] = command[3]; paramp[3] = command[4]; - +#endif + // Find out what type of command this is switch(command[0]) { case CMD_tilt: diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/HeadPointControllerBehavior.h ./Behaviors/Mon/HeadPointControllerBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Mon/HeadPointControllerBehavior.h 2005-06-10 15:18:59.000000000 -0400 +++ ./Behaviors/Mon/HeadPointControllerBehavior.h 2005-09-13 18:10:36.000000000 -0400 @@ -11,6 +11,17 @@ #include "Shared/Config.h" //! Listens to control commands coming in from the command port for remotely controlling the head +/*! The communication protocol is a very simple binary format, shared + * with WalkControllerBehavior. Each command is sent as a 5-byte + * group. The first byte is a command selector, and the following 4 + * bytes are a floating point argument: + * + * - <@c char: command indicator> + * - <@c float: value> + * + * The valid values for command indicator are given by #CMD_tilt, + * #CMD_pan, or #CMD_roll ('t', 'p', or 'r' respectively). + */ class HeadPointControllerBehavior : public BehaviorBase { public: diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/MicrophoneServer.h ./Behaviors/Mon/MicrophoneServer.h --- ../Tekkotsu_2.4.1/Behaviors/Mon/MicrophoneServer.h 2005-08-07 00:11:03.000000000 -0400 +++ ./Behaviors/Mon/MicrophoneServer.h 2006-09-16 13:32:38.000000000 -0400 @@ -16,23 +16,28 @@ virtual void DoStop(); virtual void processEvent(const EventBase& event); + //! makes Aperios-specific call to set microphone mode static bool SetMicrophoneUnidirectional(bool unidirectional); + //! makes Aperios-specific call to set microphone mode static bool SetMicrophoneAlcEnabled(bool enabled); private: + //! max transmission buffer size for #socket static const unsigned int SEND_BUFFER_SIZE = 2048 + 16; MicrophoneServer(); //!< constructor MicrophoneServer(const MicrophoneServer& rhs); //!< don't call MicrophoneServer& operator=(const MicrophoneServer& rhs); //!< don't call - static MicrophoneServer* instance; + static MicrophoneServer* instance; //!< global instance of the server + //! returns size of a "frame" at the given sampling rate and resolution unsigned int GetResampledFrameSize( unsigned int samplesSize, unsigned int newSampleRate, unsigned int newSampleBits, bool newStereo); + //! performs sampling to a specified rate and resolution, stores into @a newSamples (which you must allocate) unsigned int ResampleFrame( const char* samples, unsigned int samplesSize, @@ -42,8 +47,10 @@ void* newSamples, unsigned int newSamplesSize); + //! aperios specific identifier for microphone access static const char* const MIC_LOCATOR; + //! socket for communication class Socket *socket; //! writes @a value to @a dst and advances @a dst diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/RawCamBehavior.cc ./Behaviors/Mon/RawCamBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Mon/RawCamBehavior.cc 2005-07-29 14:35:05.000000000 -0400 +++ ./Behaviors/Mon/RawCamBehavior.cc 2006-09-14 14:57:03.000000000 -0400 @@ -7,35 +7,19 @@ #include "Behaviors/Controller.h" #include "Shared/ProjectInterface.h" +RawCamBehavior* RawCamBehavior::theOne=NULL; + RawCamBehavior::RawCamBehavior() - : BehaviorBase("RawCamBehavior"), visRaw(NULL), packet(NULL), cur(NULL), avail(0), max_buf(0), lastProcessedTime(0) -{} + : CameraStreamBehavior("RawCamBehavior",visRaw), visRaw(NULL), packet(NULL), cur(NULL), avail(0), max_buf(0), lastProcessedTime(0) +{ + ASSERT(theOne==NULL,"there was already a RawCamBehavior running!"); + theOne=this; +} void RawCamBehavior::DoStart() { BehaviorBase::DoStart(); - std::vector args; - args.push_back("raw"); - char port[50]; - snprintf(port,50,"%d",config->vision.rawcam_port); - args.push_back(port); - if(config->vision.rawcam_transport==0) { - max_buf=UDP_WIRELESS_BUFFER_SIZE; - visRaw=wireless->socket(SocketNS::SOCK_DGRAM, 1024, max_buf); - args.push_back("udp"); - } else if(config->vision.rawcam_transport==1) { - max_buf=TCP_WIRELESS_BUFFER_SIZE; - visRaw=wireless->socket(SocketNS::SOCK_STREAM, 1024, max_buf); - wireless->setDaemon(visRaw,true); - args.push_back("tcp"); - } else { - serr->printf("ERROR: Invalid Config::vision.rawcam_transport: %d\n",config->vision.rawcam_transport); - return; - } - wireless->listen(visRaw,config->vision.rawcam_port); - - Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RawVisionGUI",config->vision.rawcam_port,args); - + setupServer(); erouter->addListener(this,EventBase::visRawCameraEGID,ProjectInterface::visRawCameraSID,EventBase::deactivateETID); erouter->addListener(this,EventBase::visJPEGEGID,ProjectInterface::visColorJPEGSID,EventBase::deactivateETID); erouter->addListener(this,EventBase::visJPEGEGID,ProjectInterface::visGrayscaleJPEGSID,EventBase::deactivateETID); @@ -44,14 +28,7 @@ void RawCamBehavior::DoStop() { erouter->removeListener(this); - if(wireless->isConnected(visRaw->sock)) - sendCloseConnectionPacket(); - Controller::closeGUI("RawVisionGUI"); - - // this could be considered a bug in our wireless - if we don't setDaemon(...,false) - // it will try to listen again even though we explicitly closed the server socket... - wireless->setDaemon(visRaw,false); - wireless->close(visRaw->sock); + closeServer(); BehaviorBase::DoStop(); } @@ -59,24 +36,57 @@ RawCamBehavior::processEvent(const EventBase& e) { if(!wireless->isConnected(visRaw->sock)) return; - const FilterBankEvent* fbke=dynamic_cast(&e); - ASSERTRET(fbke!=NULL,"unexpected event"); - 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_transport==0 && visRaw->getTransport()==SocketNS::SOCK_STREAM + || config->vision.rawcam_transport==1 && visRaw->getTransport()==SocketNS::SOCK_DGRAM) { + //reset the socket + closeServer(); + setupServer(); return; - 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) { - bool succ=writeColor(*fbke); - ASSERTRET(succ,"serialization failed"); - } else if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_SINGLE_CHANNEL) { - bool succ=writeSingleChannel(*fbke); - ASSERTRET(succ,"serialization failed"); - } else { - serr->printf("%s: Bad rawcam_encoding setting\n",getName().c_str()); + } + try { + const FilterBankEvent* fbke=dynamic_cast(&e); + if(fbke==NULL) { + CameraStreamBehavior::processEvent(e); + return; + } + 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) + return; + 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(!writeColor(*fbke)) { + if(packet) { //packet was opened, need to close it + cur=packet; // don't send anything + closePacket(); + } + //error message should already be printed in writeColor + //ASSERTRET(false,"serialization failed"); + } + } else if(config->vision.rawcam_encoding==Config::vision_config::ENCODE_SINGLE_CHANNEL) { + if(!writeSingleChannel(*fbke)) { + if(packet) { //packet was opened, need to close it + cur=packet; // don't send anything + closePacket(); + } + //error message should already be printed in writeSingleChannel + //ASSERTRET(false,"serialization failed"); + } + } else { + serr->printf("%s: Bad rawcam_encoding setting\n",getName().c_str()); + } + } catch(...) { + if(packet) { //packet was opened, need to close it + cur=packet; // don't send anything + closePacket(); + } + // typically this is a per-frame recurring error, so let's just stop now + serr->printf("%s: exception generated during image serialization, stopping stream.\n",getName().c_str()); + DoStop(); + throw; } } @@ -115,6 +125,44 @@ return getSourceLayer(RawCameraGenerator::CHAN_V,numLayers); } +void +RawCamBehavior::closeServer() { + if(wireless->isConnected(visRaw->sock)) + sendCloseConnectionPacket(); + Controller::closeGUI("RawVisionGUI"); + + // this could be considered a bug in our wireless - if we don't setDaemon(...,false) + // it will try to listen again even though we explicitly closed the server socket... + wireless->setDaemon(visRaw,false); + wireless->close(visRaw->sock); +} + +void +RawCamBehavior::setupServer() { + std::vector args; + args.push_back("raw"); + char port[50]; + snprintf(port,50,"%d",config->vision.rawcam_port); + args.push_back(port); + if(config->vision.rawcam_transport==0) { + max_buf=UDP_WIRELESS_BUFFER_SIZE; + visRaw=wireless->socket(SocketNS::SOCK_DGRAM, 1024, max_buf); + args.push_back("udp"); + } else if(config->vision.rawcam_transport==1) { + max_buf=TCP_WIRELESS_BUFFER_SIZE; + visRaw=wireless->socket(SocketNS::SOCK_STREAM, 1024, max_buf); + args.push_back("tcp"); + } else { + 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); + + Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RawVisionGUI",config->vision.rawcam_port,args); +} + bool RawCamBehavior::openPacket(FilterBankGenerator& fbkgen, unsigned int time, unsigned int layer) { if(packet!=NULL) @@ -124,31 +172,16 @@ ASSERT(cur==NULL,"cur non-NULL"); cur=NULL; char * buf=packet=(char*)visRaw->getWriteBuffer(avail); - ASSERTRETVAL(packet!=NULL,"could not get buffer",false); + ASSERTRETVAL(packet!=NULL,"dropped frame, network bandwidth is saturated (reduce frame rate or size)",false); - unsigned int used; - used=LoadSave::encode("TekkotsuImage",buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(config->vision.rawcam_encoding,buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(config->vision.rawcam_compression,buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; + 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;; - used=LoadSave::encode(fbkgen.getWidth(layer),buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(fbkgen.getHeight(layer),buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(time,buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(fbkgen.getFrameNumber(),buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; + 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;; + if(!LoadSave::encodeInc(time,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; + if(!LoadSave::encodeInc(fbkgen.getFrameNumber(),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; cur=buf; return true; @@ -167,38 +200,22 @@ if(const JPEGGenerator* jgen=dynamic_cast(&fbkgen)) if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_COLOR) return true; - unsigned int used=0; openPacket(fbkgen,e.getTimeStamp(),uv_layer); - ASSERTRETVAL(cur!=NULL,"header failed",false); + if(cur==NULL) //error should have been displayed by openPacket + return false; - used=LoadSave::encode("FbkImage",cur,avail); - ASSERTRETVAL(used!=0,"save blank image failed",false); - avail-=used; cur+=used; - used=LoadSave::encode(0,cur,avail); - ASSERTRETVAL(used!=0,"save blank image failed",false); - avail-=used; cur+=used; - used=LoadSave::encode(0,cur,avail); - ASSERTRETVAL(used!=0,"save blank image failed",false); - avail-=used; cur+=used; - used=LoadSave::encode(-1,cur,avail); - ASSERTRETVAL(used!=0,"save blank image failed",false); - avail-=used; cur+=used; - used=LoadSave::encode(RawCameraGenerator::CHAN_Y,cur,avail); - ASSERTRETVAL(used!=0,"save blank image failed",false); - avail-=used; cur+=used; - used=LoadSave::encode("blank",cur,avail); - ASSERTRETVAL(used!=0,"save blank image failed",false); - avail-=used; cur+=used; + if(!LoadSave::encodeInc("FbkImage",cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; + if(!LoadSave::encodeInc(0,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; + if(!LoadSave::encodeInc(0,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; + if(!LoadSave::encodeInc(-1,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; + if(!LoadSave::encodeInc(RawCameraGenerator::CHAN_Y,cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; + if(!LoadSave::encodeInc("blank",cur,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U); - used=fbkgen.SaveBuffer(cur,avail); - ASSERTRETVAL(used!=0,"save image failed",false); - avail-=used; cur+=used; + 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); - used=fbkgen.SaveBuffer(cur,avail); - ASSERTRETVAL(used!=0,"save image failed",false); - avail-=used; cur+=used; + 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(); } @@ -215,68 +232,51 @@ if(config->vision.rawcam_compression!=Config::vision_config::COMPRESS_JPEG) return true; if(jgen->getCurrentSourceFormat()==JPEGGenerator::SRC_COLOR && big_layer-small_layer<2) { - unsigned int used=0; openPacket(fbkgen,e.getTimeStamp(),big_layer); - ASSERTRETVAL(cur!=NULL,"header failed",false); + if(cur==NULL) //error should have been displayed by openPacket + return false; fbkgen.selectSaveImage(big_layer,0); - used=fbkgen.SaveBuffer(cur,avail); - ASSERTRETVAL(used!=0,"save image failed",false); - avail-=used; cur+=used; + 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) { - unsigned int used=0; bool opened=openPacket(fbkgen,e.getTimeStamp(),big_layer); - ASSERTRETVAL(cur!=NULL,"header failed",false); + if(cur==NULL) //error should have been displayed by openPacket + return false; if(big_layer==y_layer) { fbkgen.selectSaveImage(y_layer,RawCameraGenerator::CHAN_Y); - used=fbkgen.SaveBuffer(cur,avail); - ASSERTRETVAL(used!=0,"save image failed",false); - avail-=used; cur+=used; + 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); - used=fbkgen.SaveBuffer(cur,avail); - ASSERTRETVAL(used!=0,"save image failed",false); - avail-=used; cur+=used; - + 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); - used=fbkgen.SaveBuffer(cur,avail); - ASSERTRETVAL(used!=0,"save image failed",false); - avail-=used; cur+=used; + 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) closePacket(); } } else { - unsigned int used=0; bool opened=false; if(config->vision.rawcam_compression==Config::vision_config::COMPRESS_NONE || big_layer-small_layer>=2 && big_layer==uv_layer) { opened=openPacket(fbkgen,e.getTimeStamp(),big_layer); - ASSERTRETVAL(cur!=NULL,"header failed",false); - + if(cur==NULL) //error should have been displayed by openPacket + return false; fbkgen.selectSaveImage(y_layer,RawCameraGenerator::CHAN_Y); - used=fbkgen.SaveBuffer(cur,avail); - ASSERTRETVAL(used!=0,"save image failed",false); - avail-=used; cur+=used; + 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) { opened=openPacket(fbkgen,e.getTimeStamp(),big_layer); - ASSERTRETVAL(cur!=NULL,"header failed",false); - + if(cur==NULL) //error should have been displayed by openPacket + return false; fbkgen.selectSaveImage(uv_layer,RawCameraGenerator::CHAN_U); - used=fbkgen.SaveBuffer(cur,avail); - ASSERTRETVAL(used!=0,"save image failed",false); - avail-=used; cur+=used; - + 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); - used=fbkgen.SaveBuffer(cur,avail); - ASSERTRETVAL(used!=0,"save image failed",false); - avail-=used; cur+=used; + 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) @@ -297,14 +297,12 @@ return true; unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rawcam_y_skip; - unsigned int used=0; openPacket(fbkgen,e.getTimeStamp(),layer); - ASSERTRETVAL(cur!=NULL,"header failed",false); + if(cur==NULL) //error should have been displayed by openPacket + return false; fbkgen.selectSaveImage(layer,config->vision.rawcam_channel); - used=fbkgen.SaveBuffer(cur,avail); - ASSERTRETVAL(used!=0,"save image failed",false); - avail-=used; cur+=used; + 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(); } @@ -334,9 +332,13 @@ char msg[]="CloseConnection"; unsigned int len=strlen(msg)+LoadSave::stringpad; char * buf = (char*)visRaw->getWriteBuffer(len); - ASSERTRETVAL(buf!=NULL,"Could not get buffer for closing packet",false); + if(buf==NULL) { + std::cerr << "Could not get buffer for closing packet" << std::endl; + return false; + } unsigned int used=LoadSave::encode(msg,buf,len); - ASSERTRETVAL(used!=0,"Could not write close packet",false); + if(used==0) + std::cerr << "Could not write close packet" << std::endl; visRaw->write(used); return true; } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/RawCamBehavior.h ./Behaviors/Mon/RawCamBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Mon/RawCamBehavior.h 2005-08-07 00:11:03.000000000 -0400 +++ ./Behaviors/Mon/RawCamBehavior.h 2006-09-16 13:32:38.000000000 -0400 @@ -2,7 +2,7 @@ #ifndef INCLUDED_RawCamBehavior_h_ #define INCLUDED_RawCamBehavior_h_ -#include "Behaviors/BehaviorBase.h" +#include "CameraStreamBehavior.h" #include "Shared/Config.h" class Socket; @@ -38,10 +38,13 @@ * in the AIBO's byte order (little endian). Strings are encoded using * the LoadSave::encode(char*,unsigned int, unsigned int) method. */ -class RawCamBehavior : public BehaviorBase { +class RawCamBehavior : public CameraStreamBehavior { public: //! constructor RawCamBehavior(); + + //! destructor + ~RawCamBehavior() { theOne=NULL; } 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) static const unsigned int UDP_WIRELESS_BUFFER_SIZE=64*1024; //!< 64KB is the max udp packet size @@ -63,8 +66,15 @@ static unsigned int getSourceYLayer(unsigned int numLayers); //!< returns the layer which will be used out of the source, based on current ::config settings (i.e. compression, skip, etc) static unsigned int getSourceULayer(unsigned int numLayers); //!< returns the layer which will be used out of the source, based on current ::config settings (i.e. compression, skip, etc) static unsigned int getSourceVLayer(unsigned int numLayers); //!< returns the layer which will be used out of the source, based on current ::config settings (i.e. compression, skip, etc) - + protected: + static RawCamBehavior* theOne; //!< global instance of RawCamBehavior acting as server + //! function for network data to be sent to -- forwards to #theOne's receiveData() + static int networkCallback(char* buf, int bytes) { return theOne->receiveData(buf,bytes); } + + void closeServer(); //!addListener(this,EventBase::visRegionEGID,ProjectInterface::visRegionSID,EventBase::deactivateETID); +} + +void +RegionCamBehavior::DoStop() { + erouter->removeListener(this); + closeServer(); + BehaviorBase::DoStop(); +} + +void +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) { + closeServer(); + setupServer(); + return; + } + const FilterBankEvent* fbke=dynamic_cast(&e); + ASSERTRET(fbke!=NULL,"unexpected event"); +#if DEBUG + bool succ=writeRegions(*fbke); + ASSERTRET(succ,"serialization failed"); +#else + writeRegions(*fbke); +#endif +} + +void +RegionCamBehavior::closeServer() { + Controller::closeGUI("RegionVisionGUI"); + // this could be considered a bug in our wireless - if we don't setDaemon(...,false) + // it will try to listen again even though we explicitly closed the server socket... + wireless->setDaemon(visRegion,false); + wireless->close(visRegion->sock); +} + +void +RegionCamBehavior::setupServer() { std::vector args; args.push_back("reg"); char port[50]; @@ -36,30 +78,6 @@ } wireless->listen(visRegion,config->vision.region_port); Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RegionVisionGUI",config->vision.region_port,args); - - erouter->addListener(this,EventBase::visRegionEGID,ProjectInterface::visRegionSID); -} - -void -RegionCamBehavior::DoStop() { - erouter->removeListener(this); - Controller::closeGUI("RegionVisionGUI"); - - // this could be considered a bug in our wireless - if we don't setDaemon(...,false) - // it will try to listen again even though we explicitly closed the server socket... - wireless->setDaemon(visRegion,false); - wireless->close(visRegion->sock); - BehaviorBase::DoStop(); -} - -void -RegionCamBehavior::processEvent(const EventBase& e) { - if(!wireless->isConnected(visRegion->sock)) - return; - const FilterBankEvent* fbke=dynamic_cast(&e); - ASSERTRET(fbke!=NULL,"unexpected event"); - bool succ=writeRegions(*fbke); - ASSERTRET(succ,"serialization failed"); } bool @@ -73,29 +91,14 @@ char * buf=packet=(char*)visRegion->getWriteBuffer(avail); ASSERTRETVAL(packet!=NULL,"could not get buffer",false); - unsigned int used; - used=LoadSave::encode("TekkotsuImage",buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(Config::vision_config::ENCODE_SINGLE_CHANNEL,buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(Config::vision_config::COMPRESS_RLE,buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; + 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;; - used=LoadSave::encode(fbkgen.getWidth(layer),buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(fbkgen.getHeight(layer),buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(time,buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(fbkgen.getFrameNumber(),buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; + 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;; + if(!LoadSave::encodeInc(time,buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; + if(!LoadSave::encodeInc(fbkgen.getFrameNumber(),buf,avail,"ran out of space %s:%un",__FILE__,__LINE__)) return false;; cur=buf; return true; @@ -106,22 +109,18 @@ FilterBankGenerator& fbkgen=*e.getSource(); unsigned int layer=fbkgen.getNumLayers()-1-config->vision.regioncam_skip; - unsigned int used=0; 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.rlecam_channel); - used=regGen->SaveBuffer(cur,avail); - ASSERTRETVAL(used!=0,"save region image failed",false); - avail-=used; cur+=used; + 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) ASSERTRETVAL(seg!=NULL,"The source of RegionGenerator's source is not a SegmentedColorGenerator - how do i know what the colors are?",false); - if(0==(used=seg->encodeColors(cur,avail))) return false; - avail-=used; cur+=used; + if(!seg->encodeColorsInc(cur,avail)) return false; closePacket(); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/RegionCamBehavior.h ./Behaviors/Mon/RegionCamBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Mon/RegionCamBehavior.h 2005-08-05 15:44:22.000000000 -0400 +++ ./Behaviors/Mon/RegionCamBehavior.h 2006-09-16 13:32:38.000000000 -0400 @@ -27,12 +27,15 @@ static std::string getClassDescription() { char tmp[20]; - sprintf(tmp,"%d",config->vision.rle_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(); } protected: + void closeServer(); //! args; - args.push_back("rle"); - char port[50]; - snprintf(port,50,"%d",config->vision.rle_port); - args.push_back(port); - if(config->vision.rle_transport==0) { - max_buf=UDP_WIRELESS_BUFFER_SIZE; - visRLE=wireless->socket(SocketNS::SOCK_DGRAM, 1024, max_buf); - args.push_back("udp"); - } else if(config->vision.rle_transport==1) { - max_buf=TCP_WIRELESS_BUFFER_SIZE; - visRLE=wireless->socket(SocketNS::SOCK_STREAM, 1024, max_buf); - wireless->setDaemon(visRLE,true); - args.push_back("tcp"); - } else { - serr->printf("ERROR: Invalid Config::vision.rle_transport: %d\n",config->vision.rle_transport); - return; - } - wireless->listen(visRLE,config->vision.rle_port); - - Controller::loadGUI("org.tekkotsu.mon.VisionGUI","SegVisionGUI",config->vision.rle_port,args); - + setupServer(); erouter->addListener(this,EventBase::visSegmentEGID,ProjectInterface::visSegmentSID,EventBase::deactivateETID); erouter->addListener(this,EventBase::visRLEEGID,ProjectInterface::visRLESID,EventBase::deactivateETID); } @@ -45,33 +27,96 @@ void SegCamBehavior::DoStop() { erouter->removeListener(this); + closeServer(); + BehaviorBase::DoStop(); +} + +void +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) { + closeServer(); + setupServer(); + return; + } + try { + const FilterBankEvent* fbke=dynamic_cast(&e); + if(fbke==NULL) { + CameraStreamBehavior::processEvent(e); + return; + } + 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(!writeSeg(*fbke)) { + if(packet!=NULL) { + cur=packet; + closePacket(); + } + //error message should already have been reported + //ASSERTRET(succ,"serialization failed"); + } + } + if(config->vision.segcam_compression==Config::vision_config::COMPRESS_RLE && e.getGeneratorID()==EventBase::visRLEEGID) { + if(!writeRLE(*fbke)) { + if(packet!=NULL) { + cur=packet; + closePacket(); + } + //error message should already have been reported + //ASSERTRET(succ,"serialization failed"); + } + } + } catch(...) { + if(packet!=NULL) { + cur=packet; + closePacket(); + } + // typically this is a per-frame recurring error, so let's just stop now + serr->printf("%s: exception generated during image serialization, stopping stream.\n",getName().c_str()); + DoStop(); + throw; + } +} + +void +SegCamBehavior::closeServer() { if(wireless->isConnected(visRLE->sock)) sendCloseConnectionPacket(); Controller::closeGUI("SegVisionGUI"); - + // this could be considered a bug in our wireless - if we don't setDaemon(...,false) // it will try to listen again even though we explicitly closed the server socket... wireless->setDaemon(visRLE,false); wireless->close(visRLE->sock); - BehaviorBase::DoStop(); } void -SegCamBehavior::processEvent(const EventBase& e) { - if(!wireless->isConnected(visRLE->sock)) +SegCamBehavior::setupServer() { + std::vector args; + args.push_back("rle"); + char port[50]; + snprintf(port,50,"%d",config->vision.segcam_port); + args.push_back(port); + if(config->vision.segcam_transport==0) { + max_buf=UDP_WIRELESS_BUFFER_SIZE; + visRLE=wireless->socket(SocketNS::SOCK_DGRAM, 1024, max_buf); + args.push_back("udp"); + } else if(config->vision.segcam_transport==1) { + max_buf=TCP_WIRELESS_BUFFER_SIZE; + visRLE=wireless->socket(SocketNS::SOCK_STREAM, 1024, max_buf); + args.push_back("tcp"); + } else { + serr->printf("ERROR: Invalid Config::vision.segcam_transport: %d\n",config->vision.segcam_transport); return; - const FilterBankEvent* fbke=dynamic_cast(&e); - ASSERTRET(fbke!=NULL,"unexpected event"); - if ((get_time() - lastProcessedTime) < config->vision.rle_interval) // not enough time has gone by - return; - if(config->vision.rlecam_compression==Config::vision_config::COMPRESS_NONE && e.getGeneratorID()==EventBase::visSegmentEGID) { - bool succ=writeSeg(*fbke); - ASSERTRET(succ,"serialization failed"); - } - if(config->vision.rlecam_compression==Config::vision_config::COMPRESS_RLE && e.getGeneratorID()==EventBase::visRLEEGID) { - bool succ=writeRLE(*fbke); - ASSERTRET(succ,"serialization failed"); } + wireless->setDaemon(visRLE,true); + wireless->setReceiver(visRLE,networkCallback); + wireless->listen(visRLE,config->vision.segcam_port); + + Controller::loadGUI("org.tekkotsu.mon.VisionGUI","SegVisionGUI",config->vision.segcam_port,args); } bool @@ -83,31 +128,16 @@ ASSERT(cur==NULL,"cur non-NULL"); cur=NULL; char * buf=packet=(char*)visRLE->getWriteBuffer(avail); - ASSERTRETVAL(packet!=NULL,"could not get buffer",false); + ASSERTRETVAL(packet!=NULL,"dropped frame, network bandwidth is saturated (reduce frame rate or size)",false); - unsigned int used; - used=LoadSave::encode("TekkotsuImage",buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(Config::vision_config::ENCODE_SINGLE_CHANNEL,buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(Config::vision_config::COMPRESS_RLE,buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; + 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;; - used=LoadSave::encode(fbkgen.getWidth(layer),buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(fbkgen.getHeight(layer),buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(time,buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; - used=LoadSave::encode(fbkgen.getFrameNumber(),buf,avail); - ASSERTRETVAL(used!=0,"ran out of space",false); - avail-=used; buf+=used; + 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;; + if(!LoadSave::encodeInc(time,buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;; + if(!LoadSave::encodeInc(fbkgen.getFrameNumber(),buf,avail,"ran out of space %s:%u\n",__FILE__,__LINE__)) return false;; cur=buf; return true; @@ -117,24 +147,21 @@ SegCamBehavior::writeRLE(const FilterBankEvent& e) { FilterBankGenerator& fbkgen=*e.getSource(); - unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rlecam_skip; - unsigned int used=0; + unsigned int layer=fbkgen.getNumLayers()-1-config->vision.segcam_skip; openPacket(fbkgen,e.getTimeStamp(),layer); - ASSERTRETVAL(cur!=NULL,"header failed",false); + if(cur==NULL) //error should have been displayed by openPacket + return false; RLEGenerator * rle = dynamic_cast(&fbkgen); ASSERTRETVAL(rle!=NULL,"fbkgen isn't an RLEGenerator",false); - rle->selectSaveImage(layer,config->vision.rlecam_channel); - used=rle->SaveBuffer(cur,avail); - ASSERTRETVAL(used!=0,"save rle image failed",false); - avail-=used; cur+=used; + 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()); ASSERTRETVAL(seg!=NULL,"RLE's source is not a SegmentedColorGenerator - how do i know what the colors are?",false); - if(0==(used=seg->encodeColors(cur,avail))) return false; - avail-=used; cur+=used; + if(!seg->encodeColorsInc(cur,avail)) return false; closePacket(); @@ -145,15 +172,13 @@ SegCamBehavior::writeSeg(const FilterBankEvent& e) { FilterBankGenerator& fbkgen=*e.getSource(); - unsigned int layer=fbkgen.getNumLayers()-1-config->vision.rlecam_skip; - unsigned int used=0; + unsigned int layer=fbkgen.getNumLayers()-1-config->vision.segcam_skip; openPacket(fbkgen,e.getTimeStamp(),layer); - ASSERTRETVAL(cur!=NULL,"header failed",false); + if(cur==NULL) //error should have been displayed by openPacket + return false; - fbkgen.selectSaveImage(layer,config->vision.rlecam_channel); - used=fbkgen.SaveBuffer(cur,avail); - ASSERTRETVAL(used!=0,"save seg image failed",false); - avail-=used; cur+=used; + 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(); @@ -175,9 +200,13 @@ char msg[]="CloseConnection"; unsigned int len=strlen(msg)+LoadSave::stringpad; char * buf = (char*)visRLE->getWriteBuffer(len); - ASSERTRETVAL(buf!=NULL,"Could not get buffer for closing packet",false); + if(buf==NULL) { + std::cerr << "Could not get buffer for closing packet" << std::endl; + return false; + } unsigned int used=LoadSave::encode(msg,buf,len); - ASSERTRETVAL(used!=0,"Could not write close packet",false); + if(used==0) + std::cerr << "Could not write close packet" << std::endl; visRLE->write(used); return true; } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/SegCamBehavior.h ./Behaviors/Mon/SegCamBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Mon/SegCamBehavior.h 2005-07-29 14:35:05.000000000 -0400 +++ ./Behaviors/Mon/SegCamBehavior.h 2006-09-16 16:11:50.000000000 -0400 @@ -2,7 +2,7 @@ #ifndef INCLUDED_SegCamBehavior_h_ #define INCLUDED_SegCamBehavior_h_ -#include "Behaviors/BehaviorBase.h" +#include "CameraStreamBehavior.h" #include "Shared/Config.h" class Socket; @@ -55,11 +55,14 @@ * in the AIBO's byte order (little endian). Strings are encoded using * the LoadSave::encode(char*,unsigned int, unsigned int) method. */ -class SegCamBehavior : public BehaviorBase { +class SegCamBehavior : public CameraStreamBehavior { public: //! constructor SegCamBehavior(); + //! destructor + ~SegCamBehavior() { theOne=NULL; } + 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 static const unsigned int UDP_WIRELESS_BUFFER_SIZE=64*1024; //!< 64KB is the max udp packet size @@ -71,12 +74,19 @@ static std::string getClassDescription() { char tmp[20]; - snprintf(tmp,20,"%d",config->vision.rle_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(); } protected: + static SegCamBehavior* theOne; //!< global instance of SegCamBehavior acting as server + //! function for network data to be sent to -- forwards to #theOne's receiveData() + static int networkCallback(char* buf, int bytes) { return theOne->receiveData(buf,bytes); } + + void closeServer(); //!StopPlay(channel); + sndman->stopPlay(channel); channel = SoundManager::invalid_Play_ID; packet.samples->SetCapacity(0); @@ -216,17 +216,17 @@ void SpeakerServer::QueueFrame(const char* samples, int samplesSize) { if (channel != SoundManager::invalid_Play_ID) { - const int remainingTime = sndman->GetRemainTime(channel) - RobotInfo::SoundBufferTime; + const int remainingTime = sndman->getRemainTime(channel) - RobotInfo::SoundBufferTime; if (remainingTime > (int) config->sound.streaming.speaker_max_delay) { // Queue too long - sndman->StopPlay(channel); + sndman->stopPlay(channel); channel = SoundManager::invalid_Play_ID; } else if (remainingTime < 0) { // Queue underrun } else { // Try queueing SoundManager::Play_ID newChannel = - sndman->ChainBuffer(channel, samples, (int) samplesSize); + sndman->chainBuffer(channel, samples, (int) samplesSize); if (newChannel != SoundManager::invalid_Play_ID) { channel = newChannel; return; @@ -235,7 +235,7 @@ } // Start a new channel - channel = sndman->PlayBuffer(samples, samplesSize); + channel = sndman->playBuffer(samples, samplesSize); } const void* SpeakerServer::ResampleForSpeaker( diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/SpeakerServer.h ./Behaviors/Mon/SpeakerServer.h --- ../Tekkotsu_2.4.1/Behaviors/Mon/SpeakerServer.h 2005-08-07 00:11:03.000000000 -0400 +++ ./Behaviors/Mon/SpeakerServer.h 2006-09-16 16:11:50.000000000 -0400 @@ -8,33 +8,36 @@ //! Plays streamed audio via the speaker class SpeakerServer : public BehaviorBase { public: - static SpeakerServer* GetInstance(); - virtual ~SpeakerServer(); + static SpeakerServer* GetInstance(); //!< returns global #instance + virtual ~SpeakerServer(); //!< destructor virtual void DoStart(); virtual void DoStop(); + //! registered by DoStart() to be called by networking module with incoming data static int socket_callback(char *buf, int size); private: - SpeakerServer(); + SpeakerServer(); //!< constructor SpeakerServer(const SpeakerServer& rhs); //!< don't call SpeakerServer& operator=(const SpeakerServer& rhs); //!< don't call - static SpeakerServer* instance; + static SpeakerServer* instance; //!< global instance of the server (only ever want to have one of these) - int GotSocketData(char* data, int dataSize); - class Socket *socket; + int GotSocketData(char* data, int dataSize); //!< should be called with new sound data from the network + class Socket *socket; //!< network communications socket for receiving sound data + //! returns the next sizeof(short) bytes from @a buf as a short static short GetShort(const void* buf) { short result; memcpy(&result, buf, sizeof(short)); return result; } - static const int MAX_PACKET_SIZE = 1024 * 1024; - static const int RECEIVE_BUFFER_SIZE = 2048; + static const int MAX_PACKET_SIZE = 1024 * 1024; //!< maximum size of sound buffer to send to system + static const int RECEIVE_BUFFER_SIZE = 2048; //!< maximum network packet size to accept + //! stores information about current sound buffer class Packet { public: - Packet(); - virtual ~Packet(); + Packet(); //!< constructor + virtual ~Packet(); //!< destructor - class Buffer* header; + class Buffer* header; int size; int type; bool skipped; @@ -55,7 +58,11 @@ class Buffer* resampled; void AddPacket( - const void* samples, int samplesSize, int sampleRate, byte sampleBits); + const void* samples, + int samplesSize, + int sampleRate, + byte sampleBits); + const void* ResampleForSpeaker( const void* samples, int samplesSize, diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/SpiderMachineBehavior.cc ./Behaviors/Mon/SpiderMachineBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Mon/SpiderMachineBehavior.cc 2005-06-01 01:47:45.000000000 -0400 +++ ./Behaviors/Mon/SpiderMachineBehavior.cc 2006-02-21 00:27:13.000000000 -0500 @@ -163,7 +163,7 @@ } const StateNode * SpiderMachineBehavior::find(const std::string& name) { - for(registry_t::const_iterator it=registry.begin(); it!=registry.end(); it++) { + 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; @@ -175,13 +175,13 @@ void SpiderMachineBehavior::runCommand(const std::string& s) { if(s==std::string("list")) { unsigned int numstate=0; - for(registry_t::const_iterator it=registry.begin(); it!=registry.end(); it++) { + 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=registry.begin(); it!=registry.end(); it++) { + 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()); diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/UPennWalkControllerBehavior.cc ./Behaviors/Mon/UPennWalkControllerBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Mon/UPennWalkControllerBehavior.cc 2005-06-01 01:47:46.000000000 -0400 +++ ./Behaviors/Mon/UPennWalkControllerBehavior.cc 2006-09-18 14:08:07.000000000 -0400 @@ -45,19 +45,19 @@ cout << "MECHA: hey, reprogram this button!" << endl; break; case CMD_opt5: - sndman->PlayFile("howl.wav"); + sndman->playFile("howl.wav"); break; case CMD_opt6: - sndman->PlayFile("yap.wav"); + sndman->playFile("yap.wav"); break; case CMD_opt7: - sndman->PlayFile("whimper.wav"); + sndman->playFile("whimper.wav"); break; case CMD_opt8: - sndman->PlayFile("growl.wav"); + sndman->playFile("growl.wav"); break; case CMD_opt9: - sndman->PlayFile("barkmed.wav"); + sndman->playFile("barkmed.wav"); break; // The options button commands. default: diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/WMMonitorBehavior.cc ./Behaviors/Mon/WMMonitorBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Mon/WMMonitorBehavior.cc 2004-04-16 16:17:22.000000000 -0400 +++ ./Behaviors/Mon/WMMonitorBehavior.cc 2005-11-08 16:36:08.000000000 -0500 @@ -60,7 +60,7 @@ WMitem_base* WMMonitorBehavior::find (std::string& s) { WMregistry* wmreg=&GlobalWM; - unsigned int pos=s.find('.'); + std::string::size_type pos=s.find('.'); while (pos!=std::string::npos) { bool changed=false; std::string subreg=s.substr(0, pos); @@ -126,7 +126,7 @@ //pass a line at a time to the controller while(s.size()>0) { - unsigned int endline=s.find('\n'); + std::string::size_type endline=s.find('\n'); if(endline==std::string::npos) { incomplete+=s; return 0; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/WalkControllerBehavior.cc ./Behaviors/Mon/WalkControllerBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Mon/WalkControllerBehavior.cc 2005-06-01 01:47:46.000000000 -0400 +++ ./Behaviors/Mon/WalkControllerBehavior.cc 2006-09-18 14:08:07.000000000 -0400 @@ -14,10 +14,17 @@ float param; unsigned char *paramp = (unsigned char *) ¶m; +#if defined(BYTE_ORDER) && BYTE_ORDER==BIG_ENDIAN + paramp[0] = command[4]; + paramp[1] = command[3]; + paramp[2] = command[2]; + paramp[3] = command[1]; +#else paramp[0] = command[1]; paramp[1] = command[2]; paramp[2] = command[3]; paramp[3] = command[4]; +#endif // Find out what type of command this is switch(command[0]) { @@ -45,19 +52,19 @@ cout << "MECHA: hey, reprogram this button!" << endl; break; case CMD_opt5: - sndman->PlayFile("howl.wav"); + sndman->playFile("howl.wav"); break; case CMD_opt6: - sndman->PlayFile("yap.wav"); + sndman->playFile("yap.wav"); break; case CMD_opt7: - sndman->PlayFile("whimper.wav"); + sndman->playFile("whimper.wav"); break; case CMD_opt8: - sndman->PlayFile("growl.wav"); + sndman->playFile("growl.wav"); break; case CMD_opt9: - sndman->PlayFile("barkmed.wav"); + sndman->playFile("barkmed.wav"); break; // The options button commands. default: diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/WalkControllerBehavior.h ./Behaviors/Mon/WalkControllerBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Mon/WalkControllerBehavior.h 2004-11-10 20:45:36.000000000 -0500 +++ ./Behaviors/Mon/WalkControllerBehavior.h 2005-09-13 18:10:36.000000000 -0400 @@ -13,6 +13,18 @@ #include "Shared/Config.h" //! 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 + * with HeadPointControllerBehavior. Each command is sent as a + * 5-byte group. The first byte is a command selector, and the + * following 4 bytes are a floating point argument: + * + * - <@c char: command indicator> + * - <@c float: value> + * + * The valid values for command indicator are given by #CMD_fwd, + * #CMD_roto, or #CMD_side ('f', 'r', or 's' respectively). Others + * are listed below, but are not currently used. + */ class WalkControllerBehavior : public BehaviorBase { public: diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/WorldStateSerializerBehavior.cc ./Behaviors/Mon/WorldStateSerializerBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Mon/WorldStateSerializerBehavior.cc 2005-02-21 03:05:57.000000000 -0500 +++ ./Behaviors/Mon/WorldStateSerializerBehavior.cc 2006-09-19 18:51:38.000000000 -0400 @@ -2,63 +2,131 @@ #include "Shared/WorldState.h" #include "Wireless/Wireless.h" #include "Shared/Config.h" +#include "Behaviors/Controller.h" #include "Events/EventRouter.h" +#include "Shared/LoadSave.h" WorldStateSerializerBehavior::WorldStateSerializerBehavior() - : BehaviorBase("WorldStateSerializerBehavior"), wsJoints(NULL), wsPIDs(NULL), lastProcessedTime(0) +: BehaviorBase("WorldStateSerializerBehavior"), wsJoints(NULL), wsPIDs(NULL), lastProcessedTime(0) { - wsJoints=wireless->socket(SocketNS::SOCK_STREAM, 1024, 2048); - wireless->setDaemon(wsJoints); - wireless->listen(wsJoints, config->main.wsjoints_port); - wsPIDs=wireless->socket(SocketNS::SOCK_STREAM, 1024, 2048); - wireless->setDaemon(wsPIDs); - wireless->listen(wsPIDs, config->main.wspids_port); } void WorldStateSerializerBehavior::DoStart() { 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 + 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 + wireless->setDaemon(wsJoints); + wireless->listen(wsJoints, config->main.wsjoints_port); + + Controller::loadGUI(getGUIType(),getGUIType(),getPort()); + erouter->addListener(this,EventBase::sensorEGID); } void WorldStateSerializerBehavior::DoStop() { erouter->removeListener(this); + + Controller::closeGUI(getGUIType()); + + wireless->setDaemon(wsJoints,false); + wireless->close(wsJoints); + wireless->setDaemon(wsPIDs,false); + wireless->close(wsPIDs); + BehaviorBase::DoStop(); // do this last } void WorldStateSerializerBehavior::processEvent(const EventBase& e) { - if ((e.getTimeStamp() - lastProcessedTime) < config->main.worldState_interval) // not enough time has gone by - return; - lastProcessedTime = e.getTimeStamp(); - char *buf=(char*)wsPIDs->getWriteBuffer((NumPIDJoints*3)*sizeof(float)+2*sizeof(unsigned int)); - if (buf) { - encode(&buf,state->lastSensorUpdateTime); - encode(&buf,NumPIDJoints); - encode(&buf,state->pids,NumPIDJoints*3); - wsPIDs->write((NumPIDJoints*3)*sizeof(float)+2*sizeof(unsigned int)); - } - - buf=(char*)wsJoints->getWriteBuffer((NumPIDJoints*2+NumSensors+NumButtons)*sizeof(float)+4*sizeof(unsigned int)); - if (buf) { - encode(&buf,state->lastSensorUpdateTime); - encode(&buf,NumPIDJoints); - encode(&buf,&state->outputs[PIDJointOffset], NumPIDJoints); - encode(&buf,NumSensors); - encode(&buf,state->sensors,NumSensors); - encode(&buf,NumButtons); - encode(&buf,state->buttons,NumButtons); - encode(&buf,state->pidduties,NumPIDJoints); - wsJoints->write((NumPIDJoints*2+NumSensors+NumButtons)*sizeof(float)+4*sizeof(unsigned int)); - } + const size_t transmitPIDSize=(NumPIDJoints*3)*LoadSave::getSerializedSize()+2*LoadSave::getSerializedSize(); + if ((e.getTimeStamp() - lastProcessedTime) < config->main.worldState_interval) // not enough time has gone by + return; + lastProcessedTime = e.getTimeStamp(); + char *buf=(char*)wsPIDs->getWriteBuffer(transmitPIDSize); + if(buf==NULL) { + if(wireless->isConnected(wsPIDs->sock)) + std::cout << "WorldStateSerializer dropped pid at " << e.getTimeStamp() << std::endl; + } else { +#if LOADSAVE_SWAPBYTES + // if we need to swap bytes, need to use this slightly slower method + unsigned int remain=transmitPIDSize; + LoadSave::encodeInc(state->lastSensorUpdateTime,buf,remain); + LoadSave::encodeInc(NumPIDJoints,buf,remain); + for(unsigned int i=0; ipids[i][j],buf,remain); + ASSERT(remain==0,"buffer remains"); + wsPIDs->write(transmitPIDSize-remain); +#else + // this is the original version, doesn't handle byte swapping, but faster + copy(&buf,state->lastSensorUpdateTime); + copy(&buf,NumPIDJoints); + copy(&buf,state->pids,NumPIDJoints*3); + wsPIDs->write(transmitPIDSize); +#endif + } + + 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; + //std::cout << "transmitting at " << e.getTimeStamp() << " with " << (wsJoints->getAvailableSendSize() / float(transmitJointsSize)) << std::endl; + buf=(char*)wsJoints->getWriteBuffer(transmitJointsSize); + if(buf==NULL) { + if(wireless->isConnected(wsJoints->sock)) + std::cout << "WorldStateSerializer dropped state at " << e.getTimeStamp() << std::endl; + } else { +#if LOADSAVE_SWAPBYTES + // if we need to swap bytes, need to use this slightly slower method + unsigned int remain=transmitJointsSize; + memcpy(buf,RobotName,robotNameLen); + buf+=robotNameLen; remain-=robotNameLen; + LoadSave::encodeInc(state->lastSensorUpdateTime,buf,remain); + LoadSave::encodeInc(state->frameNumber,buf,remain); + LoadSave::encodeInc(NumOutputs,buf,remain); + for(unsigned int i=0; ioutputs[i],buf,remain); + LoadSave::encodeInc(NumSensors,buf,remain); + for(unsigned int i=0; isensors[i],buf,remain); + LoadSave::encodeInc(NumButtons,buf,remain); + for(unsigned int i=0; ibuttons[i],buf,remain); + LoadSave::encodeInc(NumPIDJoints,buf,remain); + for(unsigned int i=0; ipidduties[i],buf,remain); + ASSERT(remain==0,"buffer remains"); + wsJoints->write(transmitJointsSize-remain); +#else + // this is the original version, doesn't handle byte swapping, but faster + copy(&buf,RobotName,robotNameLen); + copy(&buf,state->lastSensorUpdateTime); + copy(&buf,state->frameNumber); + copy(&buf,NumOutputs); + copy(&buf,state->outputs,NumOutputs); + copy(&buf,NumSensors); + copy(&buf,state->sensors,NumSensors); + copy(&buf,NumButtons); + copy(&buf,state->buttons,NumButtons); + copy(&buf,NumPIDJoints); + copy(&buf,state->pidduties,NumPIDJoints); + wsJoints->write(transmitJointsSize); +#endif + } } /*! @file - * @brief Implements WorldStateSerializer, which copies WorldState into a buffer for transmission over the network - * @author alokl (Creator) - * - * $Author: ejt $ - * $Name: HEAD $ - * $Revision: 1.1 $ - * $State: Exp $ - * $Date: 2006/10/04 04:21:12 $ - */ +* @brief Implements WorldStateSerializer, which copies WorldState into a buffer for transmission over the network +* @author alokl (Creator) +* +* $Author: ejt $ +* $Name: HEAD $ +* $Revision: 1.1 $ +* $State: Exp $ +* $Date: 2006/10/04 04:21:12 $ +*/ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Mon/WorldStateSerializerBehavior.h ./Behaviors/Mon/WorldStateSerializerBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Mon/WorldStateSerializerBehavior.h 2005-02-21 03:05:57.000000000 -0500 +++ ./Behaviors/Mon/WorldStateSerializerBehavior.h 2006-09-09 19:28:59.000000000 -0400 @@ -13,23 +13,26 @@ * of values. * * Protocol: + * - <@c char[]: modelName> (null terminated character array) * - <@c unsigned @c int: timestamp> - * - <@c unsigned @c int: ::NumPIDJoints> - * - for each i of ::NumPIDJoints: - * - <@c float: position of joint i> + * - <@c unsigned @c int: framenumber> + * - <@c unsigned @c int: ::NumOutputs> + * - for each i of ::NumOutputs: + * - <@c float: position of output i> * - <@c unsigned @c int: ::NumSensors> * - for each i of ::NumSensors: * - <@c float: value of sensor i> * - <@c unsigned @c int: ::NumButtons> * - for each i of ::NumButtons: * - <@c float: value of button i> + * - <@c unsigned @c int: ::NumPIDJoints> * - for each i of ::NumPIDJoints: * - <@c float: duty cycle of joint i> * */ class WorldStateSerializerBehavior : public BehaviorBase { public: - WorldStateSerializerBehavior(); //!< constructor - + WorldStateSerializerBehavior(); //!< constructor + virtual void DoStart(); //!< starts listening for sensor update events virtual void DoStop(); //!< stops listening for events virtual void processEvent(const EventBase& e); //!< core functionality - performs serialization, sends to sockets @@ -39,31 +42,36 @@ return tmp; } virtual std::string getDescription() const { return getClassDescription(); } - + + //! returns string corresponding to the Java GUI which should be launched + virtual std::string getGUIType() const { return "org.tekkotsu.mon.WorldStateRecordGUI"; } + //! returns port number the Java GUI should connect to + virtual unsigned int getPort() const { return config->main.wsjoints_port; } + protected: - //! writes @a value to @a dst and advances @a dst - template - inline static void encode(char **dst, const T& value) { - memcpy(*dst, &value, sizeof(T)); - // it'd be nice to use network byte order, but we'll save the aibo extra work - //hostToNetwork(*dst, (char *)&value, sizeof(T)); - (*dst) += sizeof(T); - } - - //! writes @a length bytes from @a src to @a dst - template - inline static void encode(char **dst, const T * src, int num) { - memcpy(*dst, src, num*sizeof(T)); - (*dst) += num*sizeof(T); - } + //! writes @a value to @a dst and advances @a dst by 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& value) { + memcpy(*dst, &value, sizeof(T)); + (*dst) += sizeof(T); + } - Socket *wsJoints; //!< socket for sending current joint data - Socket *wsPIDs; //!< socket for sending current PID info - unsigned int lastProcessedTime; //!< the time that the last event was processed + //! 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) { + memcpy(*dst, src, num*sizeof(T)); + (*dst) += num*sizeof(T); + } + Socket *wsJoints; //!< socket for sending current joint data + Socket *wsPIDs; //!< socket for sending current PID info + unsigned int lastProcessedTime; //!< the time that the last event was processed + private: - WorldStateSerializerBehavior(const WorldStateSerializerBehavior&); //!< don't call - WorldStateSerializerBehavior& operator= (const WorldStateSerializerBehavior&); //!< don't call + WorldStateSerializerBehavior(const WorldStateSerializerBehavior&); //!< don't call + WorldStateSerializerBehavior& operator= (const WorldStateSerializerBehavior&); //!< don't call }; /*! @file diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Nodes/HeadPointerNode.h ./Behaviors/Nodes/HeadPointerNode.h --- ../Tekkotsu_2.4.1/Behaviors/Nodes/HeadPointerNode.h 2005-01-24 17:23:50.000000000 -0500 +++ ./Behaviors/Nodes/HeadPointerNode.h 2006-09-27 16:10:27.000000000 -0400 @@ -2,64 +2,42 @@ #ifndef INCLUDED_HeadPointerNode_h_ #define INCLUDED_HeadPointerNode_h_ -#include "Behaviors/StateNode.h" -#include "Events/EventRouter.h" +#include "MCNode.h" #include "Motion/HeadPointerMC.h" -//! A simple StateNode that executes a LedMC motion command and throws a status event upon completion -class HeadPointerNode : public StateNode { -protected: - SharedObject head_mc; //!< MotionCommand used by this node - MotionManager::MC_ID head_id; //!< id number for the MotionCommand - -public: - //! constructor - HeadPointerNode(const std::string & nodename="HeadPointerNode") : - StateNode("HeadPointerNode",nodename), head_mc(), head_id(MotionManager::invalid_MC_ID) {} - - //! activate the node - virtual void DoStart() { - head_id = motman->addPersistentMotion(head_mc); - erouter->addListener(this,EventBase::motmanEGID,head_id,EventBase::statusETID); - StateNode::DoStart(); // don't activate transitions until our listener has been added - } - - //! deactivate the node - virtual void DoStop() { - motman->removeMotion(head_id); - head_id = MotionManager::invalid_MC_ID; - erouter->removeListener(this); - StateNode::DoStop(); - } - - //! receive motmanEGID status event and throw stateMachineEGID status event - virtual void processEvent(const EventBase&) { - postCompletionEvent(); - } - - //! reveal the MotionCommand - SharedObject& getMC() { return head_mc; } - - //! reveal the MC_ID - MotionManager::MC_ID& getMC_ID() { return head_id; } - -protected: - //! constructor - HeadPointerNode(const std::string &classname, const std::string &nodename) : - StateNode(classname,nodename), head_mc(), head_id(MotionManager::invalid_MC_ID) {} +// You don't actually need to declare extern strings in order to use +// MCNode, but it's nice... If you left the name and description +// off, it would substitute MCNode's default values, but that would +// yield rather ambiguous debugging output +//!default name for HeadPointerNode's (have to instantiate a variable in order to use as a template argument) +/*! instantiation will be placed in MCNode.cc (instead of HeadPointerNode.cc) to avoid file bloat */ +extern const char defHeadPointerNodeName[]; +//!default description for HeadPointerNode's (have to instantiate a variable in order to use as a template argument) +/*! instantiation will be placed in MCNode.cc (instead of HeadPointerNode.cc) to avoid file bloat */ +extern const char defHeadPointerNodeDesc[]; +//! A simple StateNode that executes a HeadPointerMC motion command +class HeadPointerNode : public MCNode { +public: + //! default constructor, use type name as instance name + HeadPointerNode() : MCNode() {} + + //! constructor, take an instance name + HeadPointerNode(const std::string& nm) : MCNode(nm) {} }; + /*! @file * @brief Defines HeadPointerNode, a simple StateNode that runs a HeadPointerMC motion command and throws a status event upon completion * @author dst (Creator) + * @author ejt (Rewrote using MCNode) * * $Author: ejt $ - * $Name: HEAD $ - * $Revision: 1.1 $ + * $Name: HEAD $ + * $Revision: 1.1 $ * $State: Exp $ - * $Date: 2006/10/04 04:21:12 $ + * $Date: 2006/10/04 04:21:12 $ */ #endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Nodes/LedNode.h ./Behaviors/Nodes/LedNode.h --- ../Tekkotsu_2.4.1/Behaviors/Nodes/LedNode.h 2005-06-01 01:47:46.000000000 -0400 +++ ./Behaviors/Nodes/LedNode.h 2006-09-16 13:32:38.000000000 -0400 @@ -2,53 +2,39 @@ #ifndef INCLUDED_LedNode_h_ #define INCLUDED_LedNode_h_ -#include "Behaviors/StateNode.h" -#include "Events/EventRouter.h" +#include "MCNode.h" #include "Motion/LedMC.h" //! A simple StateNode that executes a LedMC motion command and throws a status event upon completion -class LedNode : public StateNode { -protected: - SharedObject leds_mc; //!< MotionCommand used by this node - MotionManager::MC_ID leds_id; //!< id number for the MotionCommand - +/*! Extends MCNode slightly so that each time the LedMC is accessed, any flash commands are reset. + * This allows a flash to be triggered each time the node starts */ +class LedNode : public MCNode { public: - //! constructor - LedNode(std::string nodename="LedNode") : - StateNode("LedNode",nodename), leds_mc(), leds_id(MotionManager::invalid_MC_ID) {} - - //! activate the node - virtual void DoStart() { - leds_id = motman->addPersistentMotion(leds_mc); - erouter->addListener(this,EventBase::motmanEGID,leds_id,EventBase::statusETID); - StateNode::DoStart(); // don't activate transitions until our listener has been added - } - - //! deactivate the node - virtual void DoStop() { - motman->removeMotion(leds_id); - leds_id = MotionManager::invalid_MC_ID; - erouter->removeListener(this); - StateNode::DoStop(); - } - - //! receive motmanEGID status event and throw stateMachineEGID status event - virtual void processEvent(const EventBase&) { - postCompletionEvent(); - } - - //! reveal the MotionCommand - SharedObject& getMC() { return leds_mc; } - - //! reveal the MC_ID - MotionManager::MC_ID& getMC_ID() { return leds_id; } - + //! default constructor, use type name as instance name + LedNode() : MCNode("LedNode","LedNode"), lastAccess(0) {} + + //! constructor, take an instance name + LedNode(const std::string& nm) : MCNode("LedNode",nm), lastAccess(0) {} + + static std::string getClassDescription() { return "Displays a pattern on the LEDs for as long as the state is active"; } + virtual std::string getDescription() const { return getClassDescription(); } + protected: - //! constructor - LedNode(const std::string &classname, const std::string &nodename) : - StateNode(classname,nodename), leds_mc(), leds_id(MotionManager::invalid_MC_ID) {} - - + //! constructor for subclasses (which would need to provide a different class name) + LedNode(const std::string &class_name, const std::string &node_name) : MCNode(class_name,node_name), lastAccess(0) {} + + //! extends MCNode implementation so that each time the LedMC is accessed, any flash commands are reset. + virtual SharedObject& getPrivateMC() { + unsigned int curtime=get_time(); + bool isFirstCreation=(mc==NULL); + SharedObject& l=MCNode::getPrivateMC(); + if(!isFirstCreation) + l->extendFlash(curtime-lastAccess); + lastAccess=curtime; + return l; + } + + unsigned int lastAccess; //!< stores time of last call to getPrivateMC() }; /*! @file diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Nodes/MCNode.cc ./Behaviors/Nodes/MCNode.cc --- ../Tekkotsu_2.4.1/Behaviors/Nodes/MCNode.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Nodes/MCNode.cc 2006-09-27 16:10:27.000000000 -0400 @@ -0,0 +1,70 @@ +#include "MCNode.h" +#include "Events/EventRouter.h" + +const char MCNodeBase::defName[]="MCNode"; +const char MCNodeBase::defDesc[]="A generic wrapper for any MotionCommand"; + +// These externs are declared in their respective header files, but +// 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 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."; + + +void MCNodeBase::DoStart() { + StateNode::DoStart(); // do this first (required) + if(mc_id==MotionManager::invalid_MC_ID) + mc_id = motman->addPersistentMotion(getPrivateMC()); + else + motman->setPriority(mc_id,MotionManager::kStdPriority); + erouter->addListener(this,EventBase::motmanEGID,mc_id,EventBase::statusETID); +} + +void MCNodeBase::processEvent(const EventBase& /*e*/) { + if(mcCompletes) + postCompletionEvent(); +} + +void MCNodeBase::DoStop() { + erouter->removeListener(this); //generally a good idea, unsubscribe all + if(hasPrivateMC()) { + motman->removeMotion(mc_id); + mc_id=MotionManager::invalid_MC_ID; + } else if(mc_id!=MotionManager::invalid_MC_ID) { + motman->setPriority(mc_id,MotionManager::kIgnoredPriority); + } + StateNode::DoStop(); // do this last (required) +} + +void MCNodeBase::setMC(MotionManager::MC_ID mcid) { + if(hasPrivateMC()) { + if(mc_id!=MotionManager::invalid_MC_ID) + motman->removeMotion(mc_id); + delete mc; + mc=NULL; + } + mc_id=mcid; +} + + +/*! @file + * @brief Implement's MCNode's default name and description strings (the class is templated, so everything else has to go in the header) + * @author Ethan Tira-Thompson (ejt) (Creator) + * + * $Author: ejt $ + * $Name: HEAD $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2006/10/04 04:21:12 $ + */ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Nodes/MCNode.h ./Behaviors/Nodes/MCNode.h --- ../Tekkotsu_2.4.1/Behaviors/Nodes/MCNode.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Nodes/MCNode.h 2006-09-27 16:12:37.000000000 -0400 @@ -0,0 +1,128 @@ +//-*-c++-*- +#ifndef INCLUDED_MCNode_h_ +#define INCLUDED_MCNode_h_ + +#include "Behaviors/StateNode.h" +#include "Motion/MotionManager.h" +#include "Motion/MMAccessor.h" + +//! Common parent class for all the templated MCNode, which is what you want to instantiate. +class MCNodeBase : public StateNode { +public: + static const char defName[]; //!< the default name for MCNodes -- can be overridden via MCNode's template arguments + static const char defDesc[]; //!< the default description for MCNodes -- can be overridden via MCNode's template arguments + + //! destructor, free #mc + virtual ~MCNodeBase() { delete mc; } + + //! Adds the motion command to the motion manager, add a listener for the motion's completion event + virtual void DoStart(); + + //! Assumes the event is a completion event from the motion, throws a corresponding state node completion event + virtual void processEvent(const EventBase& /*e*/); + + //! Removes the motion command from the motion manager if it was our own creation + virtual void DoStop(); + + //! Allows you to assign a previously created motion, which might be shared among several MCNodes + /*! If this node already has an #mc, then it will be freed, removing from MotionManager if necessary */ + virtual void setMC(MotionManager::MC_ID mcid); + + //! reveal the MC_ID; if the motion isn't currently active, returns MotionManager::invalid_MC_ID + virtual MotionManager::MC_ID getMC_ID() { return mc_id; } + + static std::string getClassDescription() { return defName; } + virtual std::string getDescription() const { return getClassDescription(); } + +protected: + //! constructor for subclasses (which would need to provide a different class name) + MCNodeBase(const std::string &class_name, const std::string &node_name, bool expectCompletion=true) + : StateNode(class_name,node_name), mc(NULL), mc_id(MotionManager::invalid_MC_ID), mcCompletes(expectCompletion) + {} + + //! returns reference to #mc or a new SharedObject if #mc is currently NULL (so it will always return a valid value) + /*! if a particular motion command needs some initial setup besides the default constructor, + * overriding this function is a good opportunity to do so */ + virtual SharedObjectBase& getPrivateMC()=0; + + //! returns true if the motion command being used was created internally via getPrivateMC() + virtual bool hasPrivateMC() { return mc!=NULL; } + + SharedObjectBase* mc; //!< MotionCommand used by this node (may be NULL if sharing the MC with other nodes) + MotionManager::MC_ID mc_id; //!< id number for the MotionCommand + bool mcCompletes; //!< if true, will post a completion when the underlying MotionCommand posts a status + +private: + MCNodeBase(const MCNodeBase&); //!< don't call (copy constructor) + MCNodeBase& operator=(const MCNodeBase&); //!< don't call (assignment operator) +}; + +//! A generic wrapper for any MotionCommand, note that some functions are provided by the MCNodeBase common base class, namely MCNodeBase::setMC() and MCNodeBase::getMC_ID() +template +class MCNode : public MCNodeBase { +public: + //! default constructor, use type name as instance name + MCNode() + : MCNodeBase(mcName,mcName,completes) + {} + + //! constructor, take an instance name + MCNode(const std::string& nm) + : MCNodeBase(mcName,nm,completes) + {} + + //! destructor + virtual ~MCNode() {} + + //! reveal the MotionCommand through an MMAccessor + /*! This is a no-op if the motion command hasn't been added to motion manager yet, and enforces mutual exclusion if it has */ + virtual MMAccessor getMC(); + + static std::string getClassDescription() { return mcDesc; } + virtual std::string getDescription() const { return getClassDescription(); } + +protected: + //! constructor for subclasses (which would need to provide a different class name) + MCNode(const std::string &class_name, const std::string &node_name) + : MCNodeBase(class_name,node_name) + {} + + virtual SharedObject& getPrivateMC(); +}; + + +// **************************** +// ******* IMPLEMENTATION ******* +// **************************** + +template +MMAccessor MCNode::getMC() { + if(mc_id==MotionManager::invalid_MC_ID) { + // motion hasn't been added to motion manager yet + return MMAccessor(*getPrivateMC()); + } else { + // motion has been added to motion manager, check it out + return MMAccessor(mc_id); + } +} + +template +SharedObject& MCNode::getPrivateMC() { + if(mc==NULL) + mc=new SharedObject; + return dynamic_cast&>(*mc); +} + + +/*! @file + * @brief Defines MCNode, which provides generic wrapper for any MotionCommand + * @author Ethan Tira-Thompson (ejt) (Creator) + * + * $Author: ejt $ + * $Name: HEAD $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2006/10/04 04:21:12 $ + */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Nodes/MotionSequenceNode.h ./Behaviors/Nodes/MotionSequenceNode.h --- ../Tekkotsu_2.4.1/Behaviors/Nodes/MotionSequenceNode.h 2005-01-27 15:08:15.000000000 -0500 +++ ./Behaviors/Nodes/MotionSequenceNode.h 2006-09-09 00:32:33.000000000 -0400 @@ -109,7 +109,7 @@ } else { MMAccessor > ms(msid); ms->clear(); - ms->LoadFile(file.c_str()); + ms->loadFile(file.c_str()); ms->setTime(1); } filename=file; diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Nodes/PlayMotionSequenceNode.h ./Behaviors/Nodes/PlayMotionSequenceNode.h --- ../Tekkotsu_2.4.1/Behaviors/Nodes/PlayMotionSequenceNode.h 2005-01-21 17:50:37.000000000 -0500 +++ ./Behaviors/Nodes/PlayMotionSequenceNode.h 1969-12-31 19:00:00.000000000 -0500 @@ -1,27 +0,0 @@ -//-*-c++-*- -#ifndef INCLUDED_PlayMotionSequenceNode_h_ -#define INCLUDED_PlayMotionSequenceNode_h_ - -#warning PlayMotionSequenceNode has been deprecated, please use MotionSequenceNode instead. - -#include "MotionSequenceNode.h" -typedef TinyMotionSequenceNode TinyPlayMotionSequenceNode; //!< @deprecated, use MotionSequenceNode directly -typedef SmallMotionSequenceNode SmallPlayMotionSequenceNode; //!< @deprecated, use MotionSequenceNode directly -typedef MediumMotionSequenceNode MediumPlayMotionSequenceNode; //!< @deprecated, use MotionSequenceNode directly -typedef LargeMotionSequenceNode LargePlayMotionSequenceNode; //!< @deprecated, use MotionSequenceNode directly -typedef XLargeMotionSequenceNode XLargePlayMotionSequenceNode; //!< @deprecated, use MotionSequenceNode directly - - -/*! @file - * @brief Deprecated version of MotionSequenceNode - * @deprecated use MotionSequenceNode - * @author ejt (Creator) - * - * $Author: ejt $ - * $Name: HEAD $ - * $Revision: 1.1 $ - * $State: Exp $ - * $Date: 2006/10/04 04:21:12 $ - */ - -#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Nodes/SoundNode.h ./Behaviors/Nodes/SoundNode.h --- ../Tekkotsu_2.4.1/Behaviors/Nodes/SoundNode.h 2005-06-01 01:47:46.000000000 -0400 +++ ./Behaviors/Nodes/SoundNode.h 2006-09-18 14:07:59.000000000 -0400 @@ -9,7 +9,7 @@ //! A simple StateNode that plays a sound upon startup and throws a status event on completion /*! Doesn't automatically preload the sound buffer - if you want the sound file * to be preloaded, you'll have to make the - * SoundManager::LoadFile() / SoundManager::ReleaseFile() calls yourself. + * SoundManager::loadFile() / SoundManager::releaseFile() calls yourself. * * By default, sound playback will continue even after this node has been deactivated. * If this is not the behavior you desire, set the #autostop flag (through setAutoStop()) @@ -33,7 +33,7 @@ virtual void DoStart() { StateNode::DoStart(); // don't activate transitions until our listener has been added if(filename.size()>0) { - curplay_id = sndman->PlayFile(filename); + curplay_id = sndman->playFile(filename); erouter->addListener(this,EventBase::audioEGID,curplay_id,EventBase::deactivateETID); } } @@ -41,7 +41,7 @@ //! deactivate the node, doesn't stop the sound playback unless the #autostop flag has been set virtual void DoStop() { if(autostop) - StopPlay(); + stopPlay(); erouter->removeListener(this); StateNode::DoStop(); } @@ -53,8 +53,8 @@ } //! interrupts playing of the current sound - void StopPlay() { - sndman->StopPlay(curplay_id); + void stopPlay() { + sndman->stopPlay(curplay_id); curplay_id = SoundManager::invalid_Play_ID; } diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Nodes/TailWagNode.h ./Behaviors/Nodes/TailWagNode.h --- ../Tekkotsu_2.4.1/Behaviors/Nodes/TailWagNode.h 2005-01-24 17:23:50.000000000 -0500 +++ ./Behaviors/Nodes/TailWagNode.h 2006-09-27 16:10:27.000000000 -0400 @@ -2,48 +2,29 @@ #ifndef INCLUDED_TailWagNode_h_ #define INCLUDED_TailWagNode_h_ -#include "Behaviors/StateNode.h" -#include "Events/EventRouter.h" +#include "MCNode.h" #include "Motion/TailWagMC.h" -//! A simple StateNode that executes a TailWagMC motion command -class TailWagNode : public StateNode { -protected: - SharedObject tail_mc; //!< MotionCommand used by this node - MotionManager::MC_ID tail_id; //!< id number for the MotionCommand - -public: - //! constructor - TailWagNode(std::string nodename="TailWagNode") : - StateNode("TailWagNode",nodename), tail_mc(), tail_id(MotionManager::invalid_MC_ID) {} - - //! activate the node - virtual void DoStart() { - StateNode::DoStart(); - tail_id = motman->addPersistentMotion(tail_mc); - erouter->addListener(this,EventBase::motmanEGID,tail_id,EventBase::statusETID); - } - - //! deactivate the node - virtual void DoStop() { - motman->removeMotion(tail_id); - tail_id = MotionManager::invalid_MC_ID; - erouter->removeListener(this); - StateNode::DoStop(); - } - - //! receive motmanEGID status event and throw stateMachineEGID status event - this doesn't ever actually happen for a TailWagMC, but just for completeness... - virtual void processEvent(const EventBase&) { - postCompletionEvent(); - } - - //! reveal the MotionCommand itself, use getMC_ID() instead if this node isActive(). - SharedObject& getMC() { return tail_mc; } - - //! reveal the MC_ID - MotionManager::MC_ID& getMC_ID() { return tail_id; } +// You don't actually need to declare extern strings in order to use +// MCNode, but it's nice... If you left the name and description +// off, it would substitute MCNode's default values, but that would +// yield rather ambiguous debugging output +//!default name for TailWagNode'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 defTailWagNodeName[]; +//!default description for TailWagNode'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 defTailWagNodeDesc[]; +//! A simple StateNode that executes a TailWagMC motion command +class TailWagNode : public MCNode { +public: + //! default constructor, use type name as instance name + TailWagNode() : MCNode() {} + + //! constructor, take an instance name + TailWagNode(const std::string& nm) : MCNode(nm) {} }; /*! @file diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Nodes/WalkNode.h ./Behaviors/Nodes/WalkNode.h --- ../Tekkotsu_2.4.1/Behaviors/Nodes/WalkNode.h 2005-08-07 00:11:03.000000000 -0400 +++ ./Behaviors/Nodes/WalkNode.h 2006-09-27 16:10:27.000000000 -0400 @@ -2,15 +2,23 @@ #ifndef INCLUDED_WalkNode_h_ #define INCLUDED_WalkNode_h_ -#include "Behaviors/StateNode.h" +#include "MCNode.h" #include "Motion/MotionManager.h" #include "Motion/WalkMC.h" #include "Motion/MMAccessor.h" #include "Events/LocomotionEvent.h" #include "Events/EventRouter.h" -//! A StateNode for walking in a direction -class WalkNode : public StateNode { +//!default name for WalkEngineNode'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 defWalkNodeName[]; +//!default description for WalkEngineNode'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 defWalkNodeDesc[]; + +//! A StateNode for walking in a direction, use the template parameter to specify a custom walk MC, or use the ::WalkNode typedef to accept the "default" walk +template +class WalkEngineNode : public MCNode { public: //! lets us interpret values as either distances or velocities enum WalkMode_t { @@ -20,247 +28,128 @@ public: //!constructor - WalkNode() - : StateNode("WalkNode"), walkid(MotionManager::invalid_MC_ID), walkidIsMine(true), x(0), y(0), a(0), n(-1), walkMode() + WalkEngineNode() + : MCNode(), x(0), y(0), a(0), n(-1), walkMode() {} //!constructor, positive @a yvel is counter-clockwise from above (to match coordinate system), assumes velocity - WalkNode(float xvel, float yvel, float avel) - : StateNode("WalkNode"), walkid(MotionManager::invalid_MC_ID), walkidIsMine(true), x(xvel), y(yvel), a(avel), n(-1), walkMode(VelocityWalkMode) + WalkEngineNode(float xvel, float yvel, float avel) + : MCNode(), x(xvel), y(yvel), a(avel), n(-1), walkMode(VelocityWalkMode) {} //!constructor, positive @a yvel is counter-clockwise from above (to match coordinate system), assumes distance - WalkNode(float xvel, float yvel, float avel, int steps) - : StateNode("WalkNode"), walkid(MotionManager::invalid_MC_ID), walkidIsMine(true), x(xvel), y(yvel), a(avel), n(steps), walkMode(DistanceWalkMode) + WalkEngineNode(float xdist, float ydist, float adist, int steps) + : MCNode(), x(xdist), y(ydist), a(adist), n(steps), walkMode(DistanceWalkMode) {} //!constructor, positive @a yvel is counter-clockwise from above (to match coordinate system), assumes velocity - WalkNode(const std::string& name, float xvel, float yvel, float avel) - : StateNode("WalkNode",name), walkid(MotionManager::invalid_MC_ID), walkidIsMine(true), x(xvel), y(yvel), a(avel), n(-1), walkMode(VelocityWalkMode) + WalkEngineNode(const std::string& name, float xvel, float yvel, float avel) + : MCNode(name), x(xvel), y(yvel), a(avel), n(-1), walkMode(VelocityWalkMode) {} //!constructor, positive @a yvel is counter-clockwise from above (to match coordinate system), assumes distance - WalkNode(const std::string& name, float xvel, float yvel, float avel, int steps) - : StateNode("WalkNode",name), walkid(MotionManager::invalid_MC_ID), walkidIsMine(true), x(xvel), y(yvel), a(avel), n(steps), walkMode(DistanceWalkMode) + WalkEngineNode(const std::string& name, float xdist, float ydist, float adist, int steps) + : MCNode(name), x(xdist), y(ydist), a(adist), n(steps), walkMode(DistanceWalkMode) {} - //!destructor, check if we need to call our teardown - ~WalkNode() { - if(issetup) - teardown(); - } + //!destructor + ~WalkEngineNode() {} //! sets the velocity of the walk - void setDisplacement(float xd, float yd, float ad, int np = -1) { - //cout << "SET_DISPLACEMENT" << endl; - //updateWalk(xd,yd,ad,np,false); - storeValues(xd, yd, ad, np, DistanceWalkMode); + /*! @param xdist x displacement (mm, positive is forward) + * @param ydist y displacement (mm, positive is left) + * @param adist angular displacement (rad, positive is counter-clockwise) + * @param steps how many steps to take to achieve displacement (velocity depends on walk's cycle time parameter) */ + void setDisplacement(float xdist, float ydist, float adist, int steps = -1) { + storeValues(xdist, ydist, adist, steps, DistanceWalkMode); } //! sets the velocity of the walk + /*! @param xvel x velocity (mm/s, positive is forward) + * @param yvel y velocity (mm/s, positive is left) + * @param avel angular velocity (rad/s, positive is counter-clockwise) + * @param np how many steps to take at specified velocity (resulting distance depends on walk's cycle time parameter) */ void setVelocity(float xvel, float yvel, float avel, int np = -1) { - //updateWalk(xvel,yvel,avel,np); - storeValues(xvel, yvel, avel, np, VelocityWalkMode); + storeValues(xvel, yvel, avel, np, VelocityWalkMode); } //! sets the velocity in x direction (positive is forward) void setXVelocity(float xvel) { x=xvel; storeValues(xvel, y, a, n, VelocityWalkMode); } //! returns the velocity in x direction (positive is forward) - float getXVelocity() { return x; } - - //! sets the velocity in y direction (positive is forward) + float getX() { return x; } + + //! sets the velocity in y direction (positive is left) void setYVelocity(float yvel) { y=yvel; storeValues(x, yvel, a, n, VelocityWalkMode); } - - //! returns the velocity in y direction (positive is forward) - float getYVelocity() { return y; } - + + //! returns the velocity in y direction (positive is left) + float getY() { return y; } + //! sets the velocity of the turn, positive is counter-clockwise from above (to match coordinate system) void setAVelocity(float avel) { a=avel; storeValues(x, y, avel, n, VelocityWalkMode); } - - //! returns the velocity of the turn, positive is counter-clockwise from above (to match coordinate system) - float getAVelocity() { return a; } + + //! returns the velocity of the turn (positive is counter-clockwise from above (to match coordinate system)) + float getA() { return a; } + + //! returns the number of steps being taken (not necessarily the number *remaining*) + int getSteps() { return n; } virtual void DoStart() { - StateNode::DoStart(); - if (walkid != MotionManager::invalid_MC_ID) { - erouter->addListener(this, EventBase::locomotionEGID, walkid, EventBase::statusETID); - } - - updateWMC(); - } - - virtual void DoStop() { - //added the 0 - //cout << "-----------WALK NODE: DoStop()" << endl; - erouter->removeListener(this); - if(walkid!=MotionManager::invalid_MC_ID) { - MMAccessor walk(walkid); - walk->setTargetVelocity(0,0,0,0); - } - //updateWalk(0,0,0,0); - StateNode::DoStop(); - } - - //! receive locomotionEGID status event and throw stateMachineEGID status event, ie a completion event - virtual void processEvent(const EventBase& e) { - //cout << "PROCESSING WALK EVENTS" << endl; - if (e.getGeneratorID() == EventBase::locomotionEGID) { - const LocomotionEvent le = *reinterpret_cast(&e); - //cout << "LE description: " << le.getDescription() << endl; - //cout << "LE X: " << le.x << " = 0? " << (le.x == 0) << endl; - //cout << "LE Y: " << le.y << " = 0? " << (le.y == 0) << endl; - //cout << "LE A: " << le.a << " = 0? " << (le.a == 0) << endl; - if (le.x == 0 && le.y == 0 && le.a == 0) { - //cout << "Posting Completion Event for Walk." << endl; - postCompletionEvent(); - } - } - } - - - //! removes #walkid if #walkidIsMine - virtual void teardown() { - if(walkidIsMine) { - motman->removeMotion(walkid); - walkid=MotionManager::invalid_MC_ID; - } - StateNode::teardown(); - } - - //! use this to force the WalkNode to use a shared WalkMC - set to MotionManager::invalid_MC_ID to reset to internally generated walk - virtual void setWalkID(MotionManager::MC_ID id) { - if(walkidIsMine) { - motman->removeMotion(walkid); - walkid=MotionManager::invalid_MC_ID; - } - erouter->removeListener(this, EventBase::locomotionEGID); - walkid=id; - walkidIsMine=(id==MotionManager::invalid_MC_ID); - erouter->addListener(this, EventBase::locomotionEGID, walkid, EventBase::statusETID); + MCNode::DoStart(); + updateWMC(); } - //! use this to access the WalkMC that the WalkNode is using - virtual MotionManager::MC_ID getWalkID() { return walkid; } - - //! returns true if #walkid was created (and will be destroyed) by this WalkNode - false if assigned by setWalkID() - virtual bool ownsWalkID() { return walkidIsMine; } - protected: + //!constructor, positive @a yvel is counter-clockwise from above (to match coordinate system), assumes velocity + WalkEngineNode(const std::string& className, const std::string& instanceName, float xvel, float yvel, float avel) + : MCNode(className,instanceName), x(xvel), y(yvel), a(avel), n(-1), walkMode(VelocityWalkMode) + {} + + //!constructor, positive @a yvel is counter-clockwise from above (to match coordinate system), assumes distance + WalkEngineNode(const std::string& className, const std::string& instanceName, float xdist, float ydist, float adist, int steps) + : MCNode(className,instanceName), x(xdist), y(ydist), a(adist), n(steps), walkMode(DistanceWalkMode) + {} + //! stores the values and if active, calls updateWMC() - void storeValues(float xp, float yp, float ap, int np, WalkMode_t wmode) { - x = xp; - y = yp; - a = ap; - n = np; - walkMode = wmode; + void storeValues(float xp, float yp, float ap, int np, WalkMode_t wmode) { + x = xp; + y = yp; + a = ap; + n = np; + walkMode = wmode; - if (isActive()) { - updateWMC(); - } - } + if(MCNode::isActive()) + updateWMC(); + } //! makes the appropriate calls on the WalkMC - void updateWMC() { - if(walkid==MotionManager::invalid_MC_ID) { - SharedObject walk; - MotionManager::MC_ID id = motman->addPersistentMotion(walk); - setWalkID(id); - walkidIsMine=true; - } - MMAccessor walk(walkid); - switch(walkMode) { - case VelocityWalkMode: - walk->setTargetVelocity(x,y,a,n); - break; - case DistanceWalkMode: - walk->setTargetDisplacement(x,y,a,n); // WalkMC will calculate velocities. - break; - default: - std::cout << "Unknown Walk Mode" << std::endl; - break; - } - } - - /* - void updateWMC() { - if(walkid==MotionManager::invalid_MC_ID) { - SharedObject walk; - switch(walkMode) { - case VelocityWalkMode: - walk->setTargetVelocity(x,y,a,n); - break; - case DistanceWalkMode: - walk->setTargetDisplacement(x,y,a,n); // WalkMC will calculate velocities. - break; - default: - cout << "Unknown Walk Mode" << endl; - break; - } - MotionManager::MC_ID id = motman->addPersistentMotion(walk); - setWalkID(id); - walkidIsMine=true; - } else { - MMAccessor walk(walkid); - switch(walkMode) { - case VelocityWalkMode: - walk->setTargetVelocity(x,y,a,n); - break; - case DistanceWalkMode: - walk->setTargetDisplacement(x,y,a,n); // WalkMC will calculate velocities. - break; - default: - cout << "Unknown Walk Mode" << endl; - break; - } - } - } - */ - - /* - //!if the walk is invalid, create; then set xya - void updateWalk(float xp, float yp, float ap, int np=-1, bool isVelocity=true) { - cout << "NNNNNNNNNPPPPPPPPP: " << np << endl; - vector3d velocities; - if(walkid==MotionManager::invalid_MC_ID) { - SharedObject walk; - if (isVelocity) - walk->setTargetVelocity(xp,yp,ap,np); - else - walk->setTargetDisplacement(xp,yp,ap,np); // WalkMC will calculate velocities. - velocities = walk->getCurVelocity(); - MotionManager::MC_ID id = motman->addPersistentMotion(walk); - setWalkID(id); - walkidIsMine=true; - } else { - MMAccessor walk(walkid); - if (isVelocity) - walk->setTargetVelocity(xp,yp,ap,np); - else - walk->setTargetDisplacement(xp,yp,ap,np); // WalkMC will calculate velocities. - - velocities = walk->getCurVelocity(); // extract the velocities WalkMC calculated - } - - x = velocities.x; - y = velocities.y; - a = velocities.z; - n = np; + void updateWMC() { + MMAccessor walk = MCNode::getMC(); + switch(walkMode) { + case VelocityWalkMode: + walk->setTargetVelocity(x,y,a,n); + break; + case DistanceWalkMode: + walk->setTargetDisplacement(x,y,a,n); // WalkMC will calculate velocities. + break; + default: + std::cout << "Unknown Walk Mode" << std::endl; + break; + } } - */ - MotionManager::MC_ID walkid; //!< the current WalkMC - bool walkidIsMine; //!< true if the walk was created in updateWalk (instead of assigned externally) float x; //!< velocity in x direction (positive is forward), or distance if #walkMode is DistanceWalkMode float y; //!< velocity in y direction (positive is dog's left), or distance if #walkMode is DistanceWalkMode float a; //!< velocity of the turn, positive is counter-clockwise from above (to match coordinate system), or distance if #walkMode is DistanceWalkMode int n; //!< number of steps (-1 means walk forever) - - WalkMode_t walkMode; //!< the current interpretation of #x, #y, and #a + WalkMode_t walkMode; //!< the current interpretation of #x, #y, and #a }; +//! the prototypical WalkNode, using a WalkMC +typedef WalkEngineNode WalkNode; + /*! @file - * @brief Describes WalkNode, a StateNode for walking in a direction + * @brief Describes WalkEngineNode, a StateNode for walking in a direction; use the template parameter to specify a custom walk MC, or use the WalkNode typedef to accept the "default" walk * @author ejt (Creator) * * $Author: ejt $ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Nodes/WalkToTargetNode.h ./Behaviors/Nodes/WalkToTargetNode.h --- ../Tekkotsu_2.4.1/Behaviors/Nodes/WalkToTargetNode.h 2004-12-08 14:05:50.000000000 -0500 +++ ./Behaviors/Nodes/WalkToTargetNode.h 2005-12-15 13:51:27.000000000 -0500 @@ -26,7 +26,7 @@ static std::string getClassDescription() { return "walks towards a visual target, using some basic logic for moving the head to track it"; } virtual std::string getDescription() const { return getClassDescription(); } - //uses head to watch ball, walks towards it + //! uses head to watch ball, walks towards it virtual void processEvent(const EventBase& event); virtual Transition* newDefaultLostTrans(StateNode* dest); //!< returns a suggested transition for detecting "lost" condition, but you don't have to use it diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/README ./Behaviors/README --- ../Tekkotsu_2.4.1/Behaviors/README 2003-03-01 15:53:25.000000000 -0500 +++ ./Behaviors/README 2006-09-14 15:22:11.000000000 -0400 @@ -8,4 +8,7 @@ Nodes - For finite state machines, we'll put general purpose nodes here. + Services - These are behaviors which provide a useful service during execution, not just + sample code. For instance, automatic get up, battery monitoring, etc. + Transitions - For finite state machines, we'll put general purpose transitions here. diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Services/AutoGetupBehavior.h ./Behaviors/Services/AutoGetupBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Services/AutoGetupBehavior.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Services/AutoGetupBehavior.h 2006-09-18 14:07:56.000000000 -0400 @@ -0,0 +1,83 @@ +//-*-c++-*- +#ifndef INCLUDED_AutoGetupBehavior_h_ +#define INCLUDED_AutoGetupBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Shared/WorldState.h" +#include "Events/EventRouter.h" +#include "IPC/SharedObject.h" +#include "Motion/MotionManager.h" +#include "Motion/MotionSequenceMC.h" +#include "Shared/Config.h" +#include "Sound/SoundManager.h" + +//! a little background behavior to keep the robot on its feet +class AutoGetupBehavior : public BehaviorBase { +public: + //! constructor + AutoGetupBehavior() : BehaviorBase("AutoGetupBehavior"), back(0), side(0), gamma(.9), sensitivity(.85*.85), waiting(false) {} + //! destructor + virtual ~AutoGetupBehavior() {} + + //! Listens for the SensorSourceID::UpdatedSID + virtual void DoStart() { + BehaviorBase::DoStart(); + erouter->addListener(this,EventBase::sensorEGID,SensorSourceID::UpdatedSID); + } + //! Stops listening for events + virtual void DoStop() { + erouter->removeListener(this); + BehaviorBase::DoStop(); + } + //! Run appropriate motion script if the robot falls over + virtual void processEvent(const EventBase &event) { + if(event.getGeneratorID()==EventBase::motmanEGID) { + //previous attempt at getting up has completed + cout << "Getup complete" << endl; + erouter->removeListener(this,EventBase::motmanEGID); + waiting=false; + return; + } + back=back*gamma+(1-gamma)*state->sensors[BAccelOffset]; + 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; + 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) + if(fabs(back)motion.makePath("gu_side.mot"); + else if(back<0) + gu=config->motion.makePath("gu_back.mot"); + else + gu=config->motion.makePath("gu_front.mot"); + SharedObject getup(gu.c_str()); + MotionManager::MC_ID id=motman->addPrunableMotion(getup,MotionManager::kHighPriority); + erouter->addListener(this,EventBase::motmanEGID,id,EventBase::deactivateETID); + waiting=true; + } + } + static std::string getClassDescription() { return "Monitors gravity's influence on the accelerometers - if it seems the robot has fallen over, it runs appropriate getup script"; } + virtual std::string getDescription() const { return getClassDescription(); } + +protected: + float back; //!< exponential average of backwards accel + float side; //!< exponential average of sideways accel + float gamma; //!< default 0.9, gamma parameter for exponential average of above + float sensitivity; //!< default 0.85*0.85, squared threshold to consider having fallen over, use values 0-1 + bool waiting; //!< true while we're waiting to hear from completion of MotionSequence, won't try again until this is cleared +}; + +/*! @file + * @brief Defines AutoGetupBehavior, a little background behavior to keep the robot on its feet + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: HEAD $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2006/10/04 04:21:12 $ + */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Services/BatteryMonitorBehavior.h ./Behaviors/Services/BatteryMonitorBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Services/BatteryMonitorBehavior.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Services/BatteryMonitorBehavior.h 2006-09-14 15:14:03.000000000 -0400 @@ -0,0 +1,165 @@ +//-*-c++-*- +#ifndef INCLUDED_BatteryMonitorBehavior_h_ +#define INCLUDED_BatteryMonitorBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Shared/debuget.h" +#include "Shared/WorldState.h" +#include "Events/EventRouter.h" +#include "IPC/SharedObject.h" +#include "Motion/MotionManager.h" +#include "Motion/PostureMC.h" +#include "Motion/LedMC.h" +#include "Shared/ERS210Info.h" +#include "Shared/ERS220Info.h" +#include "Shared/ERS7Info.h" +#include "Motion/MMAccessor.h" + +//! A background behavior which will monitor the power level and flip the ears when appropriate on a 210, or blink the headlight if a 220 +/*! Think of this as a simple example class. For exercise, try using a MotionSequenceMC instead + * of switching the ears back manually using a PostureMC */ +class BatteryMonitorBehavior : public BehaviorBase { +public: + static const unsigned int max_t=10000; //!< max time between ear flips when at "high power" mark + static const unsigned int high_power_p=20; //!< percent of 100 which is point at which to begin warning + static const unsigned int no_power_p=14; //!< percent of 100 at which power will fail (approximate!) + + //! constructor + BatteryMonitorBehavior() : BehaviorBase("BatteryMonitorBehavior"), pose(NULL), pose_id(MotionManager::invalid_MC_ID), led_id(MotionManager::invalid_MC_ID) {} + //! destructor + virtual ~BatteryMonitorBehavior() {} + + //! Listens for the PowerSourceID::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); + //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)); + } + //! Stops listening for events + virtual void DoStop() { + if(pose!=NULL) + stopWarning(); + erouter->removeListener(this); + BehaviorBase::DoStop(); + } + //! Adds a BatteryMonitorMC to motman if power goes low + virtual void processEvent(const EventBase &event) { + if(event.getGeneratorID()==EventBase::powerEGID) { + //just check for low power status + bool shouldwarn=shouldWarn(); + if(pose!=NULL && !shouldwarn) + stopWarning(); + else if(pose==NULL && shouldwarn) + startWarning(); + } else { + ASSERTRET(event.getGeneratorID()==EventBase::timerEGID,"Unrequested event "<setPriority(led_id,MotionManager::kEmergencyPriority+1); + MMAccessor led(led_id); + led->displayPercent(state->sensors[PowerRemainOffset],LedEngine::major,LedEngine::major); + } else + motman->setPriority(led_id,MotionManager::kIgnoredPriority); + erouter->addTimer(this,1,128+flipdelay,false); + } else { + motman->setPriority(led_id,MotionManager::kEmergencyPriority+1); + MMAccessor led(led_id); + led->displayPercent(state->sensors[PowerRemainOffset],LedEngine::major,LedEngine::major); + erouter->addTimer(this,2,128,false); + } + } break; + case 2: { // release ear until next flap, hide LEDs display + ASSERTRET(pose!=NULL,"Extra timer 1"); + setFlipper(false); + motman->setPriority(led_id,MotionManager::kIgnoredPriority); + erouter->addTimer(this,1,calcFlipDelay(),false); + } break; + default: + ASSERTRET(false,"Unrequested timer " << event.getName()); + break; + } + } + } + static std::string getClassDescription() { return "Reports the current battery status, and starts flicks the ears to warn when it gets too low"; } + 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]; } + +protected: + //! adds a pose and a timer to get the ears flipping + void startWarning() { + serr->printf("LOW BATTERY\n"); + pose_id=motman->addPersistentMotion(SharedObject(),MotionManager::kEmergencyPriority+1); + pose=(PostureMC*)motman->peekMotion(pose_id); + SharedObject led; + led->displayPercent(state->sensors[PowerRemainOffset],LedEngine::major,LedEngine::major); + led_id=motman->addPersistentMotion(led,MotionManager::kEmergencyPriority+1); + setFlipper(true); + erouter->addTimer(this,2,128,false); + } + //! removes pose, in case battery magically charges + void stopWarning() { + serr->printf("BATTERY GOOD\n"); + motman->removeMotion(pose_id); + motman->removeMotion(led_id); + led_id=pose_id=MotionManager::invalid_MC_ID; + pose=NULL; + erouter->removeTimer(this,1); + erouter->removeTimer(this,2); + } + //! makes the ears flip more rapidly as power declines. Flips back and forth once every 15 seconds at 15%, down to flipping constantly at 5%. + unsigned int calcFlipDelay() { + const float high_power=high_power_p/100.0; + const float no_power=no_power_p/100.0; + float cur_power=state->sensors[PowerRemainOffset]; + if(cur_powerrobotDesign & 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(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 + MotionManager::MC_ID led_id; //!< id of LedMC if we are currently warning, MotionManager::invalid_MC_ID otherwise + +private: + BatteryMonitorBehavior(const BatteryMonitorBehavior&); //!< don't copy behaviors + BatteryMonitorBehavior operator=(const BatteryMonitorBehavior&); //!< don't assign behaviors +}; + +/*! @file + * @brief Defines BatteryMonitorBehavior, a background behavior to trigger BatteryMonitorMC to warn when the power is low + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: HEAD $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2006/10/04 04:21:12 $ + */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Services/FlashIPAddrBehavior.cc ./Behaviors/Services/FlashIPAddrBehavior.cc --- ../Tekkotsu_2.4.1/Behaviors/Services/FlashIPAddrBehavior.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Services/FlashIPAddrBehavior.cc 2006-09-18 14:07:56.000000000 -0400 @@ -0,0 +1,187 @@ +#include "FlashIPAddrBehavior.h" + +#include "Events/EventRouter.h" +#include "Motion/MMAccessor.h" +#include "Motion/LedEngine.h" +#include "Shared/WorldState.h" +#include "Shared/Config.h" +#include "Sound/SoundManager.h" +#include "Wireless/Wireless.h" + +void FlashIPAddrBehavior::DoStart() { + BehaviorBase::DoStart(); // do this first + if(config->behaviors.flash_on_start) { + setupSequence(); + loadSounds(); + ms_id = motman->addPrunableMotion(ms,MotionManager::kEmergencyPriority+1); + erouter->addListener(this,EventBase::motmanEGID,ms_id,EventBase::deactivateETID); + } + erouter->addListener(this,EventBase::buttonEGID,button1); + erouter->addListener(this,EventBase::buttonEGID,button2); +} + +void FlashIPAddrBehavior::DoStop() { + erouter->removeListener(this); + motman->removeMotion(ms_id); + ms_id=MotionManager::invalid_MC_ID; + releaseSounds(); + BehaviorBase::DoStop(); // do this last +} + +void FlashIPAddrBehavior::processEvent(const EventBase& e) { + if(e.getGeneratorID()==EventBase::timerEGID) { + + if(e.getSourceID()==ACTIVATE_TIMER) { + //buttons have been held down long enough, time to run display + if(ms_id!=MotionManager::invalid_MC_ID) { + //there's already one running, have to check it out to clear it + MMAccessor ms_acc(ms_id); + setupSequence(); + } else + setupSequence(); + loadSounds(); + ms_id = motman->addPrunableMotion(ms); + erouter->addListener(this,EventBase::motmanEGID,ms_id,EventBase::deactivateETID); + + } else { //its time to play a digit sound file + //the source id was set to correspond to an element of the sounds vector + if(e.getSourceID()>=sounds.size()) + serr->printf("ERROR: %s received invalid timer event %s\n",getName().c_str(),e.getName().c_str()); + else { + sndman->playFile(sounds[e.getSourceID()]); + if(e.getSourceID()==sounds.size()-1) + releaseSounds(); + } + + } + + } 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 + if(e.getTypeID()==EventBase::activateETID) { + if(state->buttons[button1] && state->buttons[button2]) + erouter->addTimer(this,ACTIVATE_TIMER,2000,false); + } else if(e.getTypeID()==EventBase::deactivateETID) + erouter->removeTimer(this,ACTIVATE_TIMER); + + } else if(e.getGeneratorID()==EventBase::motmanEGID) { + // display has completed, mark it as such + if(e.getSourceID()!=ms_id) + serr->printf("WARNING: %s received event %s, doesn't match ms_id (%d)\n",getName().c_str(),e.getName().c_str(),ms_id); + ms_id=MotionManager::invalid_MC_ID; + erouter->removeListener(this,EventBase::motmanEGID); + + } +} + +void FlashIPAddrBehavior::loadSounds() { + for(unsigned int i=0; iloadFile(sounds[i]); +} + +void FlashIPAddrBehavior::releaseSounds() { + for(unsigned int i=0; ireleaseFile(sounds[i]); + sounds.clear(); +} + +void FlashIPAddrBehavior::setupSequence() { + const unsigned int DISP_TIME=600; + const unsigned int GROUP_TIME=500; + const unsigned int DOT_TIME=400; + const unsigned int FADE_TIME=1; + const unsigned int BLANK_TIME=100-FADE_TIME*2; + erouter->removeTimer(this); + ms->clear(); + releaseSounds(); + unsigned int a=wireless->getIPAddress(); + unsigned int n=config->behaviors.flash_bytes; + if(n>4) + n=4; + LedEngine disp; + for(unsigned int i=n-1; i!=-1U; i--) { + unsigned int byte=(a>>(i*8))&0xFF; + unsigned int digits=1; + if(byte>=10) + digits++; + if(byte>=100) + digits++; + //cout << "byte " << i << " is " << byte << " -- " << digits << " digits" << endl; + //cout << "Setting LEDs: "; + for(unsigned int d=0; daddTimer(this,sounds.size(),ms->getTime()+delay,false); + sounds.push_back(soundfile); + for(unsigned int j=0; j(LEDOffset+j))) + //cout << j << ' '; + ms->setOutputCmd(LEDOffset+j,disp.getValue(static_cast(LEDOffset+j))); + } + ms->advanceTime(DISP_TIME); + for(unsigned int j=0; jsetOutputCmd(LEDOffset+j,disp.getValue(static_cast(LEDOffset+j))); + ms->advanceTime(FADE_TIME); + for(unsigned int j=0; jsetOutputCmd(LEDOffset+j,0); + ms->advanceTime(BLANK_TIME); + if(d==digits-1) + ms->advanceTime(GROUP_TIME); + for(unsigned int j=0; jsetOutputCmd(LEDOffset+j,0); + ms->advanceTime(FADE_TIME); + } + //cout << endl; + if(i!=0) { + LEDBitMask_t dot=1<robotDesign&WorldState::ERS210Mask) { + dot=LedEngine::ERS210numMasks[10]; + } else if(state->robotDesign&WorldState::ERS220Mask) { + dot=LedEngine::ERS220numMasks[10]; + } else if(state->robotDesign&WorldState::ERS7Mask) { + dot=LedEngine::ERS7numMasks[10]; + } + erouter->addTimer(this,sounds.size(),ms->getTime()+delay,false); + sounds.push_back("numbers/dot.wav"); + for(unsigned int j=0; jsetOutputCmd(LEDOffset+j,(dot>>j)&1); + ms->advanceTime(DOT_TIME); + for(unsigned int j=0; jsetOutputCmd(LEDOffset+j,(dot>>j)&1); + ms->advanceTime(FADE_TIME); + for(unsigned int j=0; jsetOutputCmd(LEDOffset+j,0); + ms->advanceTime(BLANK_TIME); + for(unsigned int j=0; jsetOutputCmd(LEDOffset+j,0); + ms->advanceTime(FADE_TIME); + } + } + ms->play(); +} + + +/*! @file + * @brief Implements FlashIPAddrBehavior, which displays IP address by flashing a series of numbers on the LED face panel + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: HEAD $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2006/10/04 04:21:12 $ + */ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Services/FlashIPAddrBehavior.h ./Behaviors/Services/FlashIPAddrBehavior.h --- ../Tekkotsu_2.4.1/Behaviors/Services/FlashIPAddrBehavior.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Services/FlashIPAddrBehavior.h 2006-09-14 15:14:04.000000000 -0400 @@ -0,0 +1,67 @@ +//-*-c++-*- +#ifndef INCLUDED_FlashIPAddrBehavior_h_ +#define INCLUDED_FlashIPAddrBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Motion/MotionManager.h" +#include "Motion/MotionSequenceMC.h" + +//! Displays IP address by speaking the digits and flashing a series of numbers on the LED face panel +/*! Will only run the display on DoStart() if the flash_on_start + * config variable is set. Otherwise you will need to hold down the + * buttons specified by #button1 and #button2 to trigger the display. + * Note that if the e-stop is active it will intercept the button + * events, so turn off e-stop first. */ +class FlashIPAddrBehavior : public BehaviorBase { +public: + //! constructor + FlashIPAddrBehavior() + : BehaviorBase("FlashIPAddrBehavior"), sounds(), ms(), ms_id(MotionManager::invalid_MC_ID) + {} + + virtual void DoStart(); //!< if the Config::behavior_config::flash_on_start flag is set, will setup and run + virtual void DoStop(); //!< halts any display which may be in progress + + //! Receives button events, timers, and motman manager pruning notifications + 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 "; + pre+=buttonNames[button1]; + pre+=" and "; + pre+=buttonNames[button2]; + pre+=" to trigger any time while running"; + return pre; + } + virtual std::string getDescription() const { return getClassDescription(); } + +protected: + typedef XLargeMotionSequenceMC MSMC_t; //!< used to flash the LEDs to report the IP address + + void loadSounds(); //!< loads the numeric sounds into memory + void releaseSounds(); //!< releases the numeric sounds + void setupSequence(); //!< construct the motion sequence for flashing leds, request timers to play corresponding sound file + + 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 + + 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) + static const unsigned int delay=64; //!< time (in milliseconds) to expect #ms to be delayed before it actually starts + + SharedObject ms; //!< motion sequence used to control the LEDs + MotionManager::MC_ID ms_id; //!< id number of #ms +}; + +/*! @file + * @brief Describes FlashIPAddrBehavior, which displays IP address by flashing a series of numbers on the LED face panel + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: HEAD $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2006/10/04 04:21:12 $ + */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Services/WorldStateVelDaemon.h ./Behaviors/Services/WorldStateVelDaemon.h --- ../Tekkotsu_2.4.1/Behaviors/Services/WorldStateVelDaemon.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Services/WorldStateVelDaemon.h 2006-09-14 15:14:04.000000000 -0400 @@ -0,0 +1,95 @@ +//-*-c++-*- +#ifndef INCLUDED_WorldStateVelDaemon_h_ +#define INCLUDED_WorldStateVelDaemon_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Events/EventRouter.h" +#include "Events/LocomotionEvent.h" +#include "Shared/WorldState.h" +#include "Events/EventTrapper.h" + +//! Listens for LocomotionEvents and updates the velocity fields of WorldState +/*! If we get multiple ways of locomoting, this would be a good place + * to manage them to determine the actual final velocity. + * + * Right now it'll correctly handle one (or more i suppose) e-stops + * with a single other locomotor. But if there's two active + * locomotors, I dunno how to handle that. + */ +class WorldStateVelDaemon : public BehaviorBase, public EventTrapper { +public: + //! constructor + WorldStateVelDaemon() : BehaviorBase("WorldStateVelDaemon"), estopTime(1), old_x(0), old_y(0), old_a(0) {} + + virtual void DoStart() { + BehaviorBase::DoStart(); // do this first + erouter->addTrapper(this,EventBase::locomotionEGID); + erouter->addListener(this,EventBase::estopEGID); + } + + virtual void DoStop() { + erouter->removeListener(this); + erouter->removeTrapper(this); + BehaviorBase::DoStop(); // do this last + } + + //! traps locomotion events - will filter them out if currently in EStop + virtual bool trapEvent(const EventBase& e) { + const LocomotionEvent& le=dynamic_cast(e); + old_x=le.x; + old_y=le.y; + old_a=le.a; + if(!estopTime) { + state->vel_x=le.x; + state->vel_y=le.y; + state->vel_a=le.a; + state->vel_time=le.getTimeStamp(); + return false; + } + return true; + } + + virtual void processEvent(const EventBase& e) { + if(e.getTypeID()==EventBase::deactivateETID) { + if(estopTime) { + estopTime=0; + LocomotionEvent le(EventBase::locomotionEGID,e.getSourceID(),EventBase::statusETID,e.getTimeStamp()-state->vel_time); + le.setXYA(old_x,old_y,old_a); + erouter->postEvent(le); + } + } else { + if(!estopTime) { + float older_x=old_x; + float older_y=old_y; + float older_a=old_a; + erouter->postEvent(LocomotionEvent(EventBase::locomotionEGID,e.getSourceID(),EventBase::statusETID,e.getTimeStamp()-state->vel_time)); + estopTime=e.getTimeStamp(); + old_x=older_x; + old_y=older_y; + old_a=older_a; + } + } + } + + static std::string getClassDescription() { return "Keeps the WorldState's velocity fields up to date"; } + virtual std::string getDescription() const { return getClassDescription(); } + +protected: + unsigned int estopTime; //!< time estop activation was received + float old_x; //!< current velocity of underlying locomotor + float old_y; //!< current velocity of underlying locomotor + float old_a; //!< current velocity of underlying locomotor +}; + +/*! @file + * @brief Defines WorldStateVelDaemon, which listens for LocomotionEvents and updates the velocity fields of WorldState + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: HEAD $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2006/10/04 04:21:12 $ + */ + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/StateNode.cc ./Behaviors/StateNode.cc --- ../Tekkotsu_2.4.1/Behaviors/StateNode.cc 2005-04-15 17:30:55.000000000 -0400 +++ ./Behaviors/StateNode.cc 2005-11-08 16:36:06.000000000 -0500 @@ -79,15 +79,15 @@ } void StateNode::postStartEvent() { - erouter->postEvent(EventBase::stateMachineEGID,reinterpret_cast(this),EventBase::activateETID,0,getName(),1); + erouter->postEvent(EventBase::stateMachineEGID,reinterpret_cast(this),EventBase::activateETID,0,getName(),1); } void StateNode::postCompletionEvent(float magnitude/*=0*/) { - erouter->postEvent(EventBase::stateMachineEGID,reinterpret_cast(this),EventBase::statusETID,get_time()-startedTime,getName(),magnitude); + erouter->postEvent(EventBase::stateMachineEGID,reinterpret_cast(this),EventBase::statusETID,get_time()-startedTime,getName(),magnitude); } void StateNode::postStopEvent() { - erouter->postEvent(EventBase::stateMachineEGID,reinterpret_cast(this),EventBase::deactivateETID,get_time()-startedTime,getName(),0); + erouter->postEvent(EventBase::stateMachineEGID,reinterpret_cast(this),EventBase::deactivateETID,get_time()-startedTime,getName(),0); } /*! @file diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/StateNode.h ./Behaviors/StateNode.h --- ../Tekkotsu_2.4.1/Behaviors/StateNode.h 2005-08-07 00:11:02.000000000 -0400 +++ ./Behaviors/StateNode.h 2006-10-03 18:11:44.000000000 -0400 @@ -8,11 +8,21 @@ #include //! Recursive data structure - both a state machine controller as well as a node within a state machine itself -/*! Override setup() to setup your own Transition and StateNode network. - * There are two StateNode templates in project/templates/: statenode.h and statemachine.h. - * statenode.h is meant for leaf nodes, which directly implement the execution of a task. - * statemachine.h is meant for nodes which contain a network of transitions and subnodes, which together solve the task. -*/ +/*! + * 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. + * + * You can override setup to create a sub-network, as well as overriding DoStart and DoStop, in the same class. + * + * There are two StateNode templates in project/templates/: + * - statenode.h + * is intended for leaf nodes, which directly implement the execution of a task. + * - statemachine.h + * is intended for nodes which contain a network of transitions and subnodes, which together solve the task. + */ class StateNode : public BehaviorBase { public: //!constructor, pass a name to use diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Transition.cc ./Behaviors/Transition.cc --- ../Tekkotsu_2.4.1/Behaviors/Transition.cc 2005-08-03 14:47:58.000000000 -0400 +++ ./Behaviors/Transition.cc 2006-09-18 14:08:05.000000000 -0400 @@ -10,9 +10,9 @@ AddReference(); //just in case a side effect of this transition is to dereference the transition, we don't want to be deleted while still transitioning if(sound.size()!=0) - sndman->PlayFile(sound); + sndman->playFile(sound); - erouter->postEvent(EventBase::stateTransitionEGID,reinterpret_cast(this),EventBase::statusETID,0,getName(),1); + erouter->postEvent(EventBase::stateTransitionEGID,reinterpret_cast(this),EventBase::statusETID,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... diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Transition.h ./Behaviors/Transition.h --- ../Tekkotsu_2.4.1/Behaviors/Transition.h 2005-08-07 00:11:02.000000000 -0400 +++ ./Behaviors/Transition.h 2006-10-03 17:08:20.000000000 -0400 @@ -32,7 +32,7 @@ * depending whether the source was added to the transition or the * transition was added to the source. Confusing? Exactly. * - * A template file is available at project/templates/transition.h, + * A template file is available at project/templates/transition.h, * which will help you get moving faster. */ class Transition : public BehaviorBase { diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Transitions/RandomTrans.cc ./Behaviors/Transitions/RandomTrans.cc --- ../Tekkotsu_2.4.1/Behaviors/Transitions/RandomTrans.cc 2005-06-01 01:47:46.000000000 -0400 +++ ./Behaviors/Transitions/RandomTrans.cc 2006-09-18 14:08:04.000000000 -0400 @@ -34,7 +34,7 @@ void RandomTrans::fire() { AddReference(); // for safety if ( sound.size()!=0 ) - sndman->PlayFile(sound); + sndman->playFile(sound); for(size_t i=0; iisActive()) // don't deactivate a non-active node diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/Behaviors/Transitions/SignalTrans.h ./Behaviors/Transitions/SignalTrans.h --- ../Tekkotsu_2.4.1/Behaviors/Transitions/SignalTrans.h 1969-12-31 19:00:00.000000000 -0500 +++ ./Behaviors/Transitions/SignalTrans.h 2006-09-16 16:11:51.000000000 -0400 @@ -0,0 +1,51 @@ +//-*-c++-*- +#ifndef INCLUDED_SignalTrans_h_ +#define INCLUDED_SignalTrans_h_ + +#include "Behaviors/Transition.h" +#include "Events/DataEvent.h" +#include "Events/EventRouter.h" + +//! causes a transition if a DataEvent from stateSignalEGID occurs with a specific value +/*! This allows a state node to signal a transition to another state + * in a clean symbolic way. Only the transition itself needs to + * know the address of the destination node. + */ + +template +class SignalTrans : public Transition { +public: + //! Constructor + SignalTrans(StateNode *destination, const T &value) : + Transition("SignalTrans",destination), val(value) + { } + + //! Constructor + SignalTrans(const std::string &name, StateNode *destination, const T &value) : + Transition("SignalTrans",name,destination), val(value) + { } + + virtual void DoStart() { + Transition::DoStart(); + for ( std::vector::const_iterator it = srcs.begin(); it != srcs.end(); it++ ) + erouter->addListener(this,EventBase::stateSignalEGID,(unsigned int)*it); + } + + virtual void processEvent(const EventBase &event) { + const DataEvent &d_event = dynamic_cast&>(event); + if ( d_event.getData() == val ) + fire(); + } + +protected: + //! Constructor + SignalTrans(const std::string &classname, const std::string &instancename, + StateNode *destination, const T &value) : + Transition(classname,instancename,destination,value) + { } + + T val; //!< value to compare against + +}; + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/AgentData.cc ./DualCoding/AgentData.cc --- ../Tekkotsu_2.4.1/DualCoding/AgentData.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/AgentData.cc 2006-05-19 13:14:59.000000000 -0400 @@ -0,0 +1,135 @@ +//-*-c++-*- + +#include +#include +#include + +#include "BaseData.h" // superclass +#include "Point.h" // Point data member +#include "Measures.h" // coordinate_t; AngPi data member +#include "ShapeTypes.h" // agentDataType + +#include "SketchSpace.h" +#include "Sketch.h" +#include "ShapeSpace.h" // required by DATASTUFF_CC +#include "ShapeRoot.h" // required by DATASTUFF_CC + +#include "AgentData.h" +#include "ShapeAgent.h" + +namespace DualCoding { + +DATASTUFF_CC(AgentData); + +AgentData::AgentData(ShapeSpace& _space, const Point &c) + : BaseData(_space,agentDataType), center_pt(c), orientation_pt(c), orientation(0) +{ mobile = true; } + +AgentData::AgentData(const AgentData& otherData) + : BaseData(otherData), + center_pt(otherData.center_pt), + orientation_pt(otherData.orientation_pt), + orientation(otherData.orientation) +{ mobile = true; } + +bool AgentData::isMatchFor(const ShapeRoot&) const { + return false; +} + +//! Print information about this shape. (Virtual in BaseData.) +void +AgentData::printParams() const { + cout << "Type = " << getTypeName(); + cout << "Shape ID = " << getId() << endl; + cout << "Parent ID = " << getParentId() << endl; + + // Print critical points. + cout << endl; + cout << "center{" << getCentroid().coords(1) << ", " << getCentroid().coords(2) << "}" << endl; + + cout << "orientation = " << orientation << endl; + //cout << "color = " << getColor() << endl; + printf("color = %d %d %d\n",getColor().red,getColor().green,getColor().blue); + + cout << "mobile = " << isMobile() << endl; + cout << "viewable = " << isViewable() << endl; +} + + +//! Transformations. (Virtual in BaseData.) +void AgentData::applyTransform(const NEWMAT::Matrix& Tmat) { + // cout << "AgentData::applyTransform: " << getId() << endl; + // cout << Tmat << endl; + center_pt.applyTransform(Tmat); + orientation_pt.applyTransform(Tmat); + orientation = orientation - (AngTwoPi)atan2(Tmat(1,2),Tmat(1,1)); + // updateOrientation(); +} + +//! Functions to set properties. +//{ +void +AgentData::setOrientation(AngTwoPi _orientation) { + orientation = _orientation; + orientation_pt.setCoords(getCentroid().coords(1) + 0.5*cos(orientation), + getCentroid().coords(2) + 0.5*sin(orientation)); + deleteRendering(); +} +//} + +void AgentData::updateOrientation() { + Point heading = orientation_pt-center_pt; + orientation = atan2(heading.coordY(),heading.coordX()); +} + +void AgentData::projectToGround(const NEWMAT::Matrix& camToBase, + const NEWMAT::ColumnVector& groundplane) { + center_pt.projectToGround(camToBase,groundplane); + orientation_pt.projectToGround(camToBase,groundplane); + updateOrientation(); +} + +bool AgentData::updateParams(const ShapeRoot& other, bool force) { + if (isMatchFor(other) || force) { + const AgentData& other_agent = ShapeRootTypeConst(other,AgentData).getData(); + int other_conf = other_agent.getConfidence(); + if (other_conf <= 0) + return true; + center_pt = (center_pt*confidence + other_agent.getCentroid()*other_conf) / (confidence+other_conf); + orientation = orientation*((orientation_t)confidence/(confidence+other_conf)) + + other_agent.getOrientation()*((orientation_t)confidence/(confidence+other_conf)); + return true; + } + return false; +} + +//! Render into a sketch space and return reference. (Private.) +Sketch* AgentData::render() const { + Sketch* draw_result = + new Sketch(space->getDualSpace(), "render("+getName()+")"); + draw_result = 0; + + // JJW This does not take orientation into account. + // should probably take principal axis into account later on + int cx = int(getCentroid().coords(1)); + int cy = int(getCentroid().coords(2)); + + // Sure the agent rendering is terribly inefficient, but it works + const float a = 2.0; + const float b = 2.0; + const float x_skip = atan(1/(0.5*a)); // minimum x-diff w/o gaps + for(float x = (cx-a); x<(cx+a); x+=x_skip) { + float y_y0_sq = (b*b) * (1 - (x-cx)*(x-cx)/(a*a)); + if(y_y0_sq > 0) { + int y_bot = cy + (int)(sqrt(y_y0_sq)); + int y_top = cy - (int)(sqrt(y_y0_sq)); + (*draw_result)((int)x,y_bot) = true; + (*draw_result)((int)x,y_top) = true; + } + } + (*draw_result)(cx-(int)a, cy) = true; // fill in "holes" at ends + (*draw_result)(cx+(int)a, cy) = true; + return draw_result; +} + +} // namespace diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/AgentData.h ./DualCoding/AgentData.h --- ../Tekkotsu_2.4.1/DualCoding/AgentData.h 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/AgentData.h 2006-10-03 19:23:00.000000000 -0400 @@ -0,0 +1,80 @@ +//-*-c++-*- +#ifndef _AGENTDATA_H_ +#define _AGENTDATA_H_ + +#include "BaseData.h" // superclass +#include "Point.h" // Point data member +#include "Measures.h" // coordinate_t; AngPi data member +#include "ShapeTypes.h" // agentDataType + +namespace DualCoding { + +class ShapeRoot; +class SketchSpace; +template class Sketch; + +class AgentData : public BaseData { +private: + Point center_pt; + Point orientation_pt; + AngTwoPi orientation; + +public: + //! Constructor + AgentData(ShapeSpace& _space, const Point &c); + + //! Copy constructor + AgentData(const AgentData& otherData); + + static ShapeType_t getStaticType() { return agentDataType; } + + // ========================================= + // BEGIN FUNCTIONS + // ========================================= + + DATASTUFF_H(AgentData); + + //! Centroid. (Virtual in BaseData.) + Point getCentroid() const { return center_pt; } + + //! Match agents based on their parameters. (Virtual in BaseData.) + virtual bool isMatchFor(const ShapeRoot& other) const; + + //! Print information about this shape. (Virtual in BaseData.) + virtual void printParams() const; + + virtual bool updateParams(const ShapeRoot& other, bool force=false); + + virtual void projectToGround(const NEWMAT::Matrix& camToBase, + const NEWMAT::ColumnVector& groundplane); + + //! Transformations. (Virtual in BaseData.) + virtual void applyTransform(const NEWMAT::Matrix& Tmat); + + + virtual unsigned short getDimension() const { return 3; } + + AngTwoPi getOrientation() const { return orientation; } + +protected: + //! Updates orientation according to @param orientation_pt + void updateOrientation(); + +private: + friend class MapBuilder; + //! Functions to set property values. + //@{ + void setOrientation(AngTwoPi _orientation); //!< Don't call this; use MapBuilder::setAgent() + void setCentroidPt(const Point &otherPt) { center_pt.setCoords(otherPt); } //!< Don't call this; use MapBuilder::setAgent() + //@} + + //! Render into a sketch space and return reference. + virtual Sketch* render() const; + + AgentData& operator=(const AgentData&); //!< don't call + +}; + +} // namespace + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/BaseData.cc ./DualCoding/BaseData.cc --- ../Tekkotsu_2.4.1/DualCoding/BaseData.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/BaseData.cc 2006-08-10 23:20:22.000000000 -0400 @@ -0,0 +1,190 @@ +#include "Macrodefs.h" +#include "Measures.h" + +#include "Sketch.h" // this must precede references to Sketch +#include "BaseData.h" +#include "ShapeRoot.h" +#include "ShapePoint.h" +#include "SketchDataRoot.h" +#include "ShapeSpace.h" + +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)) {} + +BoundingBox::BoundingBox(const std::vector &vec) : + xmin(0), ymin(0), xmax(0), ymax(0) { + if ( vec.size() > 0 ) { + *this = vec[0]->getBoundingBox(); + for ( size_t i = 1; igetBoundingBox()); + } +} + +std::ostream& operator<< (std::ostream& out, const BoundingBox &b) { + out << "BoundingBox(" << b.xmin << "," << b.ymin << "," + << b.xmax << "," << b.ymax << ")"; + return out; +} + + +BaseData::BaseData(ShapeSpace& _space, ShapeType_t _type, int _parentId) : + space(&_space), name(data_name(_type)), type(_type), + id(0), parentId(_parentId), lastMatchId(0), + refcount(0), viewable(true), + color_rgb((ProjectInterface::getNumColors() != -1U) ? ProjectInterface::getColorRGB(1) : rgb(0,0,255)), // color 0 is invalid, so use color 1 as default, or blue if colors aren't loaded yet + confidence(1), + mobile(false), + rendering_sketch(NULL) +{}; + + + /* +BaseData::BaseData(ShapeType_t _type, int _parentId) : + space(NULL), name(data_name(_type)), type(_type), + id(0), parentId(_parentId), lastMatchId(0), + refcount(0), viewable(true), + color_rgb(ProjectInterface::defSegmentedColorGenerator?ProjectInterface::getColorRGB(1):rgb(0,0,255)), // color 0 is invalid, so use color 1 as default, or blue if colors aren't loaded yet + confidence(1), + mobile(false), + rendering_sketch(NULL) +{}; + */ + +BaseData::BaseData(const BaseData& other) + : space(other.space), name(other.name), type(other.type), + id(0), parentId(other.parentId), lastMatchId(other.lastMatchId), + refcount(0), viewable(other.viewable), + color_rgb(other.color_rgb), + confidence(other.confidence), + mobile(other.mobile), + rendering_sketch(NULL) +{ + // cout << "copied BaseData: parentID " << parentId << " <-> " << other.parentId << endl; +}; + + +BaseData::~BaseData(void) { + if ( rendering_sketch != NULL ) + delete rendering_sketch; +} + +Shape BaseData::getCentroidPtShape() const { + PointData *pt = new PointData(*space,getCentroid()); + pt->inheritFrom(*this); + return Shape(pt); +} + +BaseData& BaseData::operator=(const BaseData& other) { + // assumes &other =? this check is done by the sub class using BaseData::operator= + // if (&other == this) + // return *this; + + space = other.space ? &(*other.space) : NULL; + name = other.name; + type = other.type; + id = other.id; + parentId = other.parentId; + lastMatchId = other.lastMatchId; + refcount = other.refcount; + viewable = other.viewable; + color_rgb = other.color_rgb; + confidence = other.confidence; + mobile = other.mobile; + rendering_sketch = other.rendering_sketch ? &(*rendering_sketch) : NULL; + return *this; +} + +void BaseData::inheritFrom(const BaseData &parent) { // used by leftPtShape, etc. + setParentId(parent.getViewableId()); + setColor(parent.getColor()); +} + +void BaseData::inheritFrom(const ShapeRoot &parent) { + setParentId(parent->getViewableId()); + setColor(parent->getColor()); +} + +void BaseData::inheritFrom(const SketchDataRoot &parent) { + setParentId(parent.getViewableId()); + setColor(parent.getColor()); +} + +void BaseData::V(std::string const &_name) { + setViewable(true); + if ( !_name.empty() ) setName(_name); +} + +void BaseData::N(std::string const &_name) { + setViewable(false); + if ( !_name.empty() ) setName(_name); +} + +ReferenceFrameType_t BaseData::getRefFrameType() const { + return space->getRefFrameType(); } + + +//!Type. +//{ +//! Get shape type name. +const char* BaseData::getTypeName() const { return data_name(type); } + +//! Test the shape type. +bool BaseData::isType(ShapeType_t this_type) const { return this_type == type; } + +//! Test that two shapes are of same type. +bool BaseData::isSameTypeAs(const ShapeRoot& other) const { + return((bool)(isType(other->type))); } + + +bool BaseData::isSameColorAs(const ShapeRoot& other) const { + return getColor() == other->getColor(); } + +void BaseData::setColor(string const &color_name) { + setColor(ProjectInterface::getColorRGB(color_name)); +} + +void BaseData::setColor(rgb new_color) { + color_rgb = new_color; + if ( rendering_sketch != NULL ) + (*rendering_sketch)->setColor(new_color); +} + + +bool BaseData::isMobile() const { return mobile; } + +void BaseData::setMobile(bool _mobile) { mobile = _mobile; } + +void BaseData::deleteRendering() { + delete rendering_sketch; + rendering_sketch = NULL; +} + +Sketch& BaseData::getRendering() { + if ( rendering_sketch != NULL ) + return *rendering_sketch; + rendering_sketch = render(); + (*rendering_sketch)->setColor(getColor()); + (*rendering_sketch)->setParentId(id); + (*rendering_sketch)->setName("render("+getName()+")"); + return *rendering_sketch; +} + + +void BaseData::increaseConfidence(int n, int maxConfidence) { + confidence += n; + if ( maxConfidence > 0 ) + confidence = min(confidence, maxConfidence); +} + +void BaseData::increaseConfidence(const BaseData& other, int maxConfidence) { + increaseConfidence(other.getConfidence() > 0 ? other.getConfidence()+1 : 2, maxConfidence); +} + +void BaseData::increaseConfidence(const ShapeRoot& other, int maxConfidence) { + increaseConfidence(other.getData(), maxConfidence); +} + +} // namespace diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/BaseData.h ./DualCoding/BaseData.h --- ../Tekkotsu_2.4.1/DualCoding/BaseData.h 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/BaseData.h 2006-08-10 23:20:22.000000000 -0400 @@ -0,0 +1,225 @@ +//-*-c++-*- +#ifndef _BASEDATA_H_ +#define _BASEDATA_H_ + +#include +#include +#include +#include "Wireless/Socket.h" // needed for sout + +#include "Macrodefs.h" +#include "Measures.h" +#include "Point.h" +#include "Shared/newmat/newmat.h" +#include "Motion/Kinematics.h" +#include "ShapeTypes.h" +#include "Vision/colors.h" + +namespace DualCoding { + +class Point; +class PointData; +class SketchSpace; +template class Sketch; +class ShapeRoot; +class SketchDataRoot; +template class Shape; + +class BoundingBox { +public: + coordinate_t xmin, ymin, xmax, ymax; + + BoundingBox(coordinate_t _xmin, coordinate_t _ymin, coordinate_t _xmax, coordinate_t _ymax) : + xmin(_xmin), ymin(_ymin), xmax(_xmax), ymax(_ymax) {} + + BoundingBox() : xmin(0), ymin(0), xmax(0), ymax(0) {} + + BoundingBox(const Point &p) : + xmin(p.coordX()), ymin(p.coordY()), xmax(p.coordX()), ymax(p.coordY()) {} + + BoundingBox(const BoundingBox &b1, const BoundingBox &b2); + + BoundingBox(const std::vector &vec); + +}; + +std::ostream& operator<< (std::ostream& out, const BoundingBox &b); + +class BaseData { +public: + friend class ShapeRoot; + friend class ShapeSpace; + +protected: + ShapeSpace *space; + std::string name; + ShapeType_t type; + int id; + int parentId; + int lastMatchId; //!< Id of the shape in the preceding space that gave rise to or was matched to this one. + int refcount; + bool viewable; + rgb color_rgb; + + int confidence; //!< Confidence that this shape exists and isn't noise. + bool mobile; //!< True if this shape can move in the world + + Sketch* rendering_sketch; + +public: + //! Constructor + BaseData(ShapeSpace& _space, ShapeType_t typeval, int _parentId=0); + + //! Copy constructor + BaseData(const BaseData& otherData); + + // BaseData(ShapeType_t typeval, int _parentID=0); + + //! Destructor. + virtual ~BaseData(void); + + virtual BaseData* clone(void) const = 0; + + ShapeSpace& getSpace() const { return *space; } + ReferenceFrameType_t getRefFrameType() const; + + int getId() const { return id; } + int getParentId() const { return parentId; } + void setParentId(int _pid) { parentId = _pid; } + + bool isViewable() const { return viewable; } + void setViewable(bool _viewable) { viewable = _viewable; } + + int getViewableId() const { + if ( viewable ) + return id; + else + return parentId; + } + + void inheritFrom(const BaseData &parent); + void inheritFrom(const ShapeRoot &parent); + void inheritFrom(const SketchDataRoot &parent); + + int getLastMatchId() const { return lastMatchId; } + void setLastMatchId(int _lmid) { lastMatchId = _lmid; } + + const std::string& getName() const { return name; } + void setName(const std::string& _name) { name = _name; } + + void V(std::string const &_name=""); + void N(std::string const &_name=""); + + virtual BoundingBox getBoundingBox() const { return BoundingBox(); } + + //! Confidence. + //@{ + virtual int getConfidence() const { return confidence; } //!< returns confidence of Data. Reimpletemnted in PolygonData + + void increaseConfidence(int n=1, int maxConfidence=-1); + void increaseConfidence(const BaseData& other, int maxConfidence=-1); + void increaseConfidence(const ShapeRoot& other, int maxConfidence=-1); + + void decreaseConfidence() { confidence--; } + void setConfidence(const BaseData& other) { confidence = other.getConfidence(); } + //@} + + //! Type. + //@{ + //! Get the shape type. + virtual ShapeType_t getType() const=0; + const char* getTypeName() const; + + //! Test the shape type. + bool isType(ShapeType_t this_type) const; + + //! Test if shape types are the same. + bool isSameTypeAs(const ShapeRoot& other) const; + //@} + + + //! Color. + //@{ + //! Get the color. + rgb getColor() const { return color_rgb; } + + //! Set shape and rendering sketch color. + //@{ + void setColor(std::string const &color_name); + void setColor(rgb new_color); + //@} + + //! Test if shape colors are the same. + bool isSameColorAs(const ShapeRoot& other) const; + + //@} + + //! Shapes match if their coordinates agree. DOES NOT Assume type and color already checked. + virtual bool isMatchFor(const ShapeRoot& other) const = 0; + + //! Combine two shapes by taking weighted average depending on confidence level. + // virtual void mergeWith(const ShapeRoot& other) = 0; + + //! Shapes are admissible to the local map if they're large enough not to be noise. + virtual bool isAdmissible() const { return true; } + + //! Update shape parameters after matching to another shape. + virtual bool updateParams(const ShapeRoot& other, bool forceUpdate=false) = 0; + + //! returns if a point is inside the shape or not. Reimplemented by EllipseData, SphereData, PolygonData + virtual bool isInside(const Point&) const { return false; } + + virtual unsigned short getDimension() const = 0; + + //! Mobility + //@{ + bool isMobile() const; + void setMobile(bool mobility); + //@} + + void deleteRendering(); + + //! return the centroid of the shape in point format + virtual Point getCentroid() const=0; + + virtual Shape getCentroidPtShape() const; + + //! Prints information about this shape. + virtual void printParams() const=0; + + //! Apply a transformation matrix to the shape. + virtual void applyTransform(const NEWMAT::Matrix& Tmat)=0; + + //! Project to ground plane using given matrix + virtual void projectToGround(const NEWMAT::Matrix& camToBase, + const NEWMAT::ColumnVector& groundplane)=0; + + //! Update properties of the shape derived from endpoints or other basic parameters. + // virtual void update_derived_properties() {} + + //! Rendering. + //@{ + //! Returns a pointer to the rendering associated with the ShapeRoot object. + //! If no such rendering exists, it is created. + Sketch& getRendering(); + + //! Render into a sketch space. + virtual Sketch* render() const=0; + //@} + + //! Copy operator. Assumes "&other =? this" check is done by the sub class calling this operator + BaseData& operator=(const BaseData& other); +}; + +#define DATASTUFF_H(T) \ + virtual ShapeType_t getType() const { return getStaticType(); } \ + virtual BaseData* clone() const; \ + Shape copy() const; + +#define DATASTUFF_CC(T) \ + BaseData* T::clone() const { return new T(*this); } \ + Shape T::copy() const { return Shape((T*)clone()); } + +} // namespace + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/BlobData.cc ./DualCoding/BlobData.cc --- ../Tekkotsu_2.4.1/DualCoding/BlobData.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/BlobData.cc 2006-08-02 17:27:14.000000000 -0400 @@ -0,0 +1,912 @@ +//-*-c++-*- +#include +#include + +#include "Vision/cmvision.h" + +#include "SketchSpace.h" +#include "Sketch.h" +#include "ShapeSpace.h" +#include "ShapeRoot.h" + +#include "BlobData.h" +#include "LineData.h" // for drawline2d +#include "ShapeBlob.h" +#include "visops.h" +#include "Region.h" +#include "ShapeLine.h" +#include "ShapePoint.h" + +#include "BrickOps.h" + +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) : + BaseData(_space, getStaticType()), + topLeft(_topLeft), topRight(_topRight), + bottomLeft(_bottomLeft), bottomRight(_bottomRight), + area(_area), runvec(_runvec), orientation(_orientation) +{ + setColor(_rgbvalue); +} + +DATASTUFF_CC(BlobData); + +//! return the centroid of the shape in point format +Point BlobData::getCentroid() const { + return Point((topLeft.coords + topRight.coords + bottomLeft.coords + bottomRight.coords) / 4, + getRefFrameType()); +} + +void BlobData::printParams() const { + cout << "Blob" + << " tl=" << topLeft.coords + << " tr=" << topRight.coords + << " bl=" << bottomLeft.coords + << " br=" << bottomRight.coords + << " area=" << area + << endl; +} + +Sketch* BlobData::render() const { + SketchSpace &SkS = space->getDualSpace(); + Sketch* result = new Sketch(SkS, "render("+getName()+")"); + *result = false; + switch ( orientation ) { + case groundplane: + if ( space->getRefFrameType() == camcentric ) { + for ( std::vector::const_iterator it = runvec.begin(); + it != runvec.end(); it++ ) { + const BlobData::run &r = *it; + int const xstop = r.x + r.width; + for ( int xi=r.x; xi& other_blob = ShapeRootTypeConst(other,BlobData); + float dist = getCentroid().distanceFrom(other_blob->getCentroid()); + return dist < 2*sqrt(area); + } +} + +bool BlobData::updateParams(const ShapeRoot& other, bool forceUpdate) { + const Shape& other_blob = ShapeRootTypeConst(other,BlobData); + int other_conf = other_blob->confidence; + if (other_conf <= 0) { + if (forceUpdate) + other_conf = 1; + else + return false; + } + confidence += other_conf; + const int sumconf = confidence + other_conf; + topLeft = (topLeft*confidence + other_blob->topLeft*other_conf) / sumconf; + 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; + update_derived_properties(); + return true; +} + +std::vector > +BlobData::extractBlobs(const Sketch &sketch, + const set& colors, int minarea, + BlobData::BlobOrientation_t orient, + 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()); + for ( std::vector >::iterator it = result.begin(); + it != result.end(); it++ ) + (*it)->setColor(rgbvalue); + 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); +} + + +std::vector > +BlobData::extractBlobs(const Sketch &sketch, + const set& colors, int minarea, + BlobData::BlobOrientation_t orient, int maxblobs) { + int parent = sketch->getId(); + uchar *pixels = &((*sketch.pixels)[0]); + + // convert pixel array to RLE + int const maxRuns = (sketch.width * sketch.height) / 8; + CMVision::run *rle_buffer = new CMVision::run[maxRuns]; + unsigned int const numRuns = CMVision::EncodeRuns(rle_buffer, pixels, sketch.width, sketch.height, maxRuns); + + // convert RLE to region list + CMVision::ConnectComponents(rle_buffer,numRuns); + int const maxRegions = (sketch.width * sketch.height) / 16; // formula from RegionGenerator.h + CMVision::region *regions = new CMVision::region[maxRegions]; + unsigned int numRegions = CMVision::ExtractRegions(regions, maxRegions, rle_buffer, numRuns); + unsigned int const numColors = ProjectInterface::getNumColors(); + CMVision::color_class_state *ccs = new CMVision::color_class_state[numColors]; + unsigned int const maxArea = CMVision::SeparateRegions(ccs, numColors, regions, numRegions); + CMVision::SortRegions(ccs, numColors, maxArea); + CMVision::MergeRegions(ccs, int(numColors), rle_buffer); + + // extract blobs from region list + std::vector > result(20); + result.clear(); + ShapeSpace &ShS = sketch->getSpace().getDualSpace(); + // for ( size_t color=1; color < numColors; color++ ) { + 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; + if ( list_head != NULL ) { + // const rgb rgbvalue = ProjectInterface::getColorRGB(color); + const rgb rgbvalue = ProjectInterface::getColorRGB(*it); + 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); + blobdat->setParentId(parent); + result.push_back(Shape(blobdat)); + } + } + } + delete[] ccs; + delete[] regions; + delete[] rle_buffer; + 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 rgb rgbvalue) { + int const x1 = reg.x1; + int const y1 = reg.y1; + int const x2 = reg.x2; + int const y2 = reg.y2; + // Count the number of runs so we can allocate a vector of the right size. + // The first run might be numbered 0, so we must use -1 as end of list indicator. + int numruns = 0; + for (int runidx = reg.run_start; runidx != -1; + runidx = rle_buff[runidx].next ? rle_buff[runidx].next : -1) + ++numruns; + std::vector runvec(numruns); + runvec.clear(); + // now fill in the run vector + for (int runidx = reg.run_start; runidx != -1; + runidx = rle_buff[runidx].next ? rle_buff[runidx].next : -1) { + const CMVision::run &this_run = rle_buff[runidx]; + runvec.push_back(BlobData::run(this_run.x, this_run.y, this_run.width)); + } + if ( space.getRefFrameType() == camcentric ) + return new BlobData(space, + Point(x1,y1),Point(x2,y1), + Point(x1,y2),Point(x2,y2), + reg.area, runvec, orient, rgbvalue); + else + return new BlobData(space, + Point(x2,y2),Point(x1,y2), + Point(x2,y1),Point(x1,y1), + reg.area, runvec, orient, rgbvalue); +} + + + + + + + // 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 + for (unsigned int i=0; isetParentId(getViewableId()); + } + + // Handling off-screen bricks: + // If our fit of corners is close to the edge of the image, + // re-try fitting 3 or 5 corners to the brick + bool onEdge = false; + int width = space->getDualSpace().getWidth(); + int height = space->getDualSpace().getHeight(); + for (unsigned int i=0; i width - 5 || + fitCorners[i].coordY() < 5 || fitCorners[i].coordY() > height - 5) { + onEdge = true; + break; + } + } + if (onEdge && nExpected == 4) { + std::vector outsideCandidates; + for (unsigned int i=0; i width - 5 || + candidates[i].coordY() < 5 || candidates[i].coordY() > height - 5) { + outsideCandidates.push_back(i); + } + } + + + std::vector candidates3(candidates), candidates5; + + if (outsideCandidates.size() == 0) { + std::cout<<"Err? final points are near the edge, but the candidates aren't?"< width - 5) { + p1.setCoords(width,0); + p2.setCoords(width,height); + } + else if (candidates[outC].coordY() < 5) { + p1.setCoords(0,0); + p2.setCoords(width,0); + } + else { + p1.setCoords(0,height); + p2.setCoords(width,height); + } + LineData edgeLine(*space, p1,p2); + LineData l1(*space, candidates[(outC+3)%4], candidates[outC]); + LineData l2(*space, candidates[(outC+1)%4], candidates[outC]); + candidates5.push_back(candidates[(outC+3)%4]); + candidates5.push_back(l1.intersectionWithLine(edgeLine)); + candidates5.push_back(l2.intersectionWithLine(edgeLine)); + candidates5.push_back(candidates[(outC+1)%4]); + candidates5.push_back(candidates[(outC+2)%4]); + } + + if (outsideCandidates.size() == 2) { + Point betweenOutside = (candidates[outsideCandidates[0]] + candidates[outsideCandidates[1]])/2; + candidates3[outsideCandidates[0]].setCoords(betweenOutside); + candidates3.erase(candidates3.begin() + outsideCandidates[1]); + + int dC = outsideCandidates[1] - outsideCandidates[0]; + int c1 = outsideCandidates[1]; + candidates5.push_back(candidates[outsideCandidates[0]]); + candidates5.push_back(betweenOutside); + candidates5.push_back(candidates[outsideCandidates[1]]); + candidates5.push_back(candidates[(c1+dC)%4]); + candidates5.push_back(candidates[(c1+2*dC)%4]); + } + + if (outsideCandidates.size() > 2) { + // Not gonna get very good points out of this, probably + } + + float value3, value5; + for (unsigned int i=0; isetParentId(getViewableId()); + } + for (unsigned int i=0; isetParentId(getViewableId()); + } + std::vector fitcorners3 = findCornersShapeFit(3, candidates3, value3); + std::vector fitcorners5 = findCornersShapeFit(5, candidates5, value5); + for (unsigned int i=0; isetParentId(getViewableId()); + } + for (unsigned int i=0; isetParentId(getViewableId()); + } + } + + // right now just take the corners from quadrilateral fitting + 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 + /* + const float MIN_BOUNDING_SCORE = .75; + + float derivScore = -1, diagScore = -1; + + std::vector derivCorners = findCornersDerivative(); + + if (derivCorners.size() == nExpected) { + + derivScore = getBoundingQuadrilateralInteriorPointRatio(derivCorners); + std::cout<<"Derivative score for ("< MIN_BOUNDING_SCORE) { + return derivCorners; + } + } + + std::vector diagCorners = findCornersDiagonal(); + + if (diagCorners.size() == nExpected) { + diagScore = getBoundingQuadrilateralInteriorPointRatio(diagCorners); + std::cout<<"Diagonal score for ("< derivScore) { + return diagCorners; + } + else if (derivScore > .5) { + return derivCorners; + } + } + + // can we integrate sets of incomplete / overcomplete points? + + std::vector result; + + return result; + */ + } + + + /* + * Derivative approach to finding the corners of the blobs. + * Computes the distance from the center to the edge in a circle around the blob + * Takes a couple derivatives, and looks for peaks. + * + * Works well on big shapes, poorly on small ones + * Doesn't make any guarantees as to how many points are returned. + */ + std::vector BlobData::findCornersDerivative() + { + std::vector corners; + + float radius = sqrt((topRight.coordX()-topLeft.coordX())*(topRight.coordX()-topLeft.coordX()) + + (bottomLeft.coordY()-topLeft.coordY())*(bottomLeft.coordY()-topLeft.coordY()))/2 + 1; + int len = (int)(2*M_PI*radius + 1); + float distances[len]; + Point points[len]; + Point centroid = getCentroid(); + NEW_SKETCH(rendering, bool, getRendering()); + + int i=0, maxi = 0; + + maxi = findRadialDistancesFromPoint(centroid, radius, rendering, distances, points); + float gdist[len], ddist[len], d2dist[len], d3dist[len]; + applyGaussian(distances, gdist, len); + takeDerivative(gdist, ddist, len); + takeDerivative(ddist, d2dist, len); + takeDerivative(d2dist, d3dist, len); + + drawHist(gdist, len, rendering); + drawHist(ddist, len, rendering); + drawHist(d2dist, len, rendering); + + + // Corners are negative peaks in the second derivative of the distance from the center + // Zero-crossings in the first derivative should work, but we want to capture contour changes that + // don't necessarily cross zero (steep increase -> shallow increase could be a corner) + + const float MIN_D2 = 5.0; + + float curmin = -MIN_D2; + int curi = -1; + int curonpeak = 0; + for (i=0; i -MIN_D2 || (d3dist[i-1] > 0 && d3dist[i] <= 0)) { + curonpeak = 0; + curmin = -MIN_D2; + corners.push_back(points[curi]); + NEW_SHAPE(cornerpoint, PointData, Shape(*space, points[curi])); + cornerpoint->setParentId(rendering->getViewableId()); + } + } + } + + + // Normalize returned corners to counter-clock-wise order; + vector reversedCorners; + for (i=corners.size()-1; i>=0; i--){ + reversedCorners.push_back(corners[i]); + } + + return reversedCorners; + } + + + /* + * Diagonal approach to finding corners + * ad-hoc heuristic works well for normal parallelograms + * + * fails on trapezoids and triangles, and where nCorners != 4. + * + * + * Computes radial distance from the center like the derivative method + * Finds the peak in distance (furthest point is once corner) + * Finds the peak in the opposite region (another corner) + * + * At this point it can draw the diagonal. The breakdown occurs when + * it thinks it has a diagonal at this point but it isn't an actual diagonal + * + * Then split the quadrilateral into two triangles, and the furthest points from the + * diagonal are the two remaining corners. + */ + std::vector BlobData::findCornersDiagonal() + { + std::vector corners; + + float radius = sqrt((topRight.coordX()-topLeft.coordX())*(topRight.coordX()-topLeft.coordX()) + + (bottomLeft.coordY()-topLeft.coordY())*(bottomLeft.coordY()-topLeft.coordY()))/2 + 1; + int len = (int)(2*M_PI*radius + 1); + float distances[len]; + Point points[len]; + Point centroid = getCentroid(); + NEW_SKETCH(rendering, bool, getRendering()); + + int i=0; + int maxi = 0, origmaxi = 0; + bool stillmax = false; + + maxi = findRadialDistancesFromPoint(centroid, radius, rendering, distances, points); + + // Find second max + int maxi2 = 0; + float max2 = 0; + stillmax = false; + origmaxi = -1; + for (i=0; i= max2 && + abs(i-maxi) > len*3/8 && + abs(i-maxi) < len*5/8) { + if (distances[i] > max2) { + maxi2 = i; + max2 = distances[i]; + origmaxi = maxi2; + stillmax = true; + } + else if (stillmax){ + maxi2 = (origmaxi+i)/2; + } + } + else { + stillmax = false; + } + } + + corners.push_back(points[maxi]); + corners.push_back(points[maxi2]); + std::cout<<"Corners: ("<(*space, corners[0], corners[1])); + diag->firstPt().setActive(false); + diag->secondPt().setActive(false); + diag->setParentId(rendering->getViewableId()); + + NEW_SKETCH_N(filled, bool, visops::topHalfPlane(diag)); + NEW_SKETCH(side1, bool, filled & rendering); + NEW_SKETCH(side2, bool, !filled & rendering); + + const float MIN_PT_DIST = 3.0; + + Point pt3 = (Region::extractRegion(side1)).mostDistantPtFrom(diag.getData()); + Point pt4 = (Region::extractRegion(side2)).mostDistantPtFrom(diag.getData()); + if (diag->perpendicularDistanceFrom(pt3) > MIN_PT_DIST) + corners.push_back(pt3); + if (diag->perpendicularDistanceFrom(pt4) > MIN_PT_DIST) + corners.push_back(pt4); + + + // Sort the corners into order going around the brick + std::vector resultCorners; + std::vector angles; + + float ta; + Point tp; + for (i=0; i<(int)corners.size(); i++) { + Point di = corners[i] - centroid; + angles.push_back(atan2(di.coordY(), di.coordX())); + resultCorners.push_back(corners[i]); + for (int j=i-1; j>=0; j--) { + if (angles[j+1] > angles[j]) { + ta = angles[j]; + angles[j] = angles[j+1]; + angles[j+1] = ta; + tp = resultCorners[j]; + resultCorners[j] = resultCorners[j+1]; + resultCorners[j+1] = tp; + } + else{ + break; + } + } + } + + NEW_SHAPE(cornerline1, LineData, Shape(*space, resultCorners[0], resultCorners[1])); + cornerline1->setParentId(rendering->getViewableId()); + if (resultCorners.size() > 3) { + NEW_SHAPE(cornerline2, LineData, Shape(*space, resultCorners[2], resultCorners[3])); + cornerline2->setParentId(rendering->getViewableId()); + } + + return resultCorners; + } + + + + + + // find corners by fitting a quadrilateral to the blob + // + // Its expecting a set of candidate points that are roughly aligned with one set of edges and are + // significantly wider than the blob itself. + // It contracts the candidate points to form a rough bounding box, + // then does simulated annealing on random perturbations of the end points to get a good fit + // + // Potential quadrilateral fits are scored by maximizing the number of edge points of the blob that + // lie under one of the lines, and minimizing the total area. + // A fixed minimum edge length constraint is also enforced. + std::vector BlobData::findCornersShapeFit(unsigned int ncorners, std::vector& candidates, + float &bestValue) + { + NEW_SKETCH(rendering, bool, getRendering()); + + std::vector bestPoints(ncorners), curTest(ncorners); + float bestScore; + int bestEdgeCount; + std::vector > testPoints; + std::vector testScores; + std::vector testEdgeCounts; + + if (candidates.size() == ncorners) { + for (unsigned int i=0; i 0 & nsum < 8 & getRendering()); + int edgeTotal = 0; + for (unsigned int x=0; xgetWidth(); x++) { + borderPixels(x,0) = 0; + borderPixels(x,borderPixels->getHeight() - 1) = 0; + } + for (unsigned int y=0; ygetHeight(); y++) { + borderPixels(0,y) = 0; + borderPixels(borderPixels->getWidth() - 1, y) = 0; + } + for (unsigned int i=0; igetNumPixels(); i++) { + if (borderPixels->at(i)) + edgeTotal++; + } + + bestScore = getBoundingQuadrilateralScore(*this, bestPoints, borderPixels, bestEdgeCount, *space); + + int testCount = 0, testEdgeCount; + Point dp; + float dpDist, testRatio; + bool hasMoved; + + float annealingScalar = 1.0; + bool doingRandomMovement = false; + const float ANNEALING_CAP = 25.0; + const float WEIGHT_SCALAR = .2; + + const float MIN_DISTANCE = 10.0; + const float MIN_BOUNDING_RATIO = 0.8; + + int iterationCount = 0, annealingStart = 0; + // Right now it just keeps going until the annealing weight gets to a set threshold + // Should probably be improved by checking the quality of the fit at intervals + // especially if the points have stopped moving + while (annealingScalar < ANNEALING_CAP) { + + hasMoved = false; + + // Test each corner in succession + for (unsigned int i=0; i MIN_DISTANCE) { + + dp/=dpDist; + curTest[i]+=dp; + testRatio = getBoundingQuadrilateralInteriorPointRatio(*this, curTest, *space); + if (testRatio > MIN_BOUNDING_RATIO) { + testScores.push_back(getBoundingQuadrilateralScore(*this, curTest, borderPixels, + testEdgeCount, *space)); + testPoints.push_back(curTest); + testEdgeCounts.push_back(testEdgeCount); + testCount++; + } + + // Look outward too if we're in the annealing stage + if (doingRandomMovement) { + curTest[i].setCoords(bestPoints[i]); + curTest[i]-=dp; + testRatio = getBoundingQuadrilateralInteriorPointRatio(*this, curTest, *space); + if (testRatio > MIN_BOUNDING_RATIO) { + testScores.push_back(getBoundingQuadrilateralScore(*this, curTest, borderPixels, + testEdgeCount, *space)); + testPoints.push_back(curTest); + testEdgeCounts.push_back(testEdgeCount); + testCount++; + } + + } + + } + + curTest[i].setCoords(bestPoints[i]); + dp.setCoords(curTest[(i+ncorners-1)%ncorners] - curTest[i]); + dpDist = curTest[i].distanceFrom(curTest[(i+ncorners-1)%ncorners]); + // Don't allow corners to get too close to each other + if (dpDist > MIN_DISTANCE) { + + dp/=dpDist; + curTest[i]+=dp; + testRatio = getBoundingQuadrilateralInteriorPointRatio(*this, curTest, *space); + if (testRatio > MIN_BOUNDING_RATIO) { + testScores.push_back(getBoundingQuadrilateralScore(*this, curTest, borderPixels, + testEdgeCount, *space)); + testPoints.push_back(curTest); + testEdgeCounts.push_back(testEdgeCount); + testCount++; + } + + // Look outward too if we're in the annealing stage + if (doingRandomMovement) { + curTest[i].setCoords(bestPoints[i]); + curTest[i]-=dp; + testRatio = getBoundingQuadrilateralInteriorPointRatio(*this, curTest, *space); + if (testRatio > MIN_BOUNDING_RATIO) { + testScores.push_back(getBoundingQuadrilateralScore(*this, curTest, borderPixels, + testEdgeCount, *space)); + testPoints.push_back(curTest); + testEdgeCounts.push_back(testEdgeCount); + testCount++; + } + } + + } + + + testScores.push_back(bestScore); + testPoints.push_back(bestPoints); + testEdgeCounts.push_back(bestEdgeCount); + testCount++; + + int move = -1; + if (doingRandomMovement) { + move = pickMove(testScores, annealingScalar); + } + else { + move = 0; + for (int j=0; j= testCount) + std::cout<<"Hmm, picked a bad move somewhere ("<setParentId(borderPixels->getViewableId()); + } + } + + iterationCount++; + if (iterationCount > 500) { + std::cout<<"Warning, annealing stopped by max iteration count\n"< &b1, const Shape &b2) const { + return b1->getArea() < b2->getArea(); +} + +} // namespace diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/BlobData.h ./DualCoding/BlobData.h --- ../Tekkotsu_2.4.1/DualCoding/BlobData.h 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/BlobData.h 2006-07-21 13:57:44.000000000 -0400 @@ -0,0 +1,149 @@ +//-*-c++-*- +#ifndef _BLOBDATA_H_ +#define _BLOBDATA_H_ + +#include +#include +#include +#include + +#include "Shared/newmat/newmat.h" +#include "Vision/cmv_types.h" + +#include "BaseData.h" // superclass +#include "Point.h" // Point data member +#include "ShapeTypes.h" // blobDataType +#include "ShapeFuns.h" + +namespace DualCoding { + +class ShapeRoot; +template class Sketch; + +//! Blob shapes, described by bounding boxes and an optional list of runs. + +class BlobData : public BaseData { + public: + + // Bounding quadrilateral; may not be square when projected to world space. + Point topLeft, topRight, bottomLeft, bottomRight; + + float area; + + struct run { + public: + unsigned short int x, y, width; + run() : x(0), y(0), width(0) {} + run(unsigned short int _x, unsigned short int _y, unsigned short int _width) : + 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; + + 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); + + static ShapeType_t getStaticType() { return blobDataType; } + + DATASTUFF_H(BlobData); + + friend class Shape; + + //! return the centroid of the shape in point format + virtual Point getCentroid() const; + + //! Area of the blob + float getArea() { return area; } + + //! Print information about this shape. + virtual void printParams() const; + + //! Transformations. (Virtual in BaseData.) + virtual void applyTransform(const NEWMAT::Matrix& Tmat); + + //! Project to ground + // virtual void projectToGround(int xres, int yres, const NEWMAT::ColumnVector& groundplane); + virtual void projectToGround(const NEWMAT::Matrix& camToBase, + const NEWMAT::ColumnVector& groundplane); + + //! Update derived properties + virtual void update_derived_properties(); + + //! Match blobs based on their parameters. (Virtual in BaseData.) + virtual bool isMatchFor(const ShapeRoot& other) const; + + virtual bool updateParams(const ShapeRoot& other, bool forceUpdate=false); + + virtual unsigned short getDimension() const { return (orientation==groundplane) ? 2 : 3; } + + + //! 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, + int maxblobs=50); + + //! 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, + int maxblobs=50); + + static BlobData* + new_blob(ShapeSpace& space, + const CMVision::region ®, + const CMVision::run *rle_buff, + const BlobData::BlobOrientation_t orient, + const rgb rgbvalue); + + 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 { + public: + bool operator() (const Shape &b1, const Shape &b2) const; + }; + +private: + //! Render into a sketch space and return reference. (Private.) + virtual Sketch* render() const; + + BlobData& operator=(const BlobData&); //!< don't call + +}; + +} // namespace + +#endif // BLOBDATA_H_ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/BrickData.cc ./DualCoding/BrickData.cc --- ../Tekkotsu_2.4.1/DualCoding/BrickData.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/BrickData.cc 2006-09-23 23:45:20.000000000 -0400 @@ -0,0 +1,1167 @@ +//-*-c++-*- + +#include +#include + +#include "BaseData.h" // superclass +#include "Point.h" // Point data member +#include "ShapeTypes.h" // brickDataType + +#include "SketchSpace.h" +#include "Sketch.h" +#include "visops.h" + +#include "ShapeSpace.h" // required by DATASTUFF_CC +#include "ShapeRoot.h" // required by DATASTUFF_CC + +#include "BrickData.h" +#include "ShapeBrick.h" +#include "ShapePoint.h" +#include "Region.h" + + +namespace DualCoding { + +BrickData::BrickData(ShapeSpace& _space, + const EndPoint &_GFL, const EndPoint &_GFR, + const EndPoint &_GBL, const EndPoint &_GBR, + const EndPoint &_TFL, const EndPoint &_TFR, + const EndPoint &_TBL, const EndPoint &_TBR) + : BaseData(_space,brickDataType), + GFL(_GFL), GFR(_GFR), GBL(_GBL), GBR(_GBR), + TFL(_TFL), TFR(_TFR), TBL(_TBL), TBR(_TBR), + centroid((GFL + GFR + GBL + GBR + TFL + TFR + TBL + TBR) / 8) +{ +} + + +DATASTUFF_CC(BrickData); + +bool BrickData::isMatchFor(const ShapeRoot& other) const { + if (!(isSameTypeAs(other) && isSameColorAs(other))) + return false; + // const Shape& other_point = ShapeRootTypeConst(other,BrickData); + // float dist = the_point.distanceFrom(other_point->getCentroid()); + float dist = 0; + return dist < 20; // *** DST hack +} + +void BrickData::mergeWith(const ShapeRoot& other) { + const Shape& other_brick = ShapeRootTypeConst(other,BrickData); + if (other_brick->confidence <= 0) + return; + /* + const int other_conf = other_point->confidence; + confidence += other_conf; + the_point = (the_point*confidence + other_point->getCentroid()*other_conf) / (confidence+other_conf);*/ +} + +bool BrickData::updateParams(const ShapeRoot& other, bool) { + const Shape& other_brick = *static_cast*>(&other); + // ++confidence; + GFL = (GFL*(confidence-1) + other_brick->getGFL())/confidence; + GFR = (GFR*(confidence-1) + other_brick->getGFR())/confidence; + GBL = (GBL*(confidence-1) + other_brick->getGBL())/confidence; + GBR = (GBR*(confidence-1) + other_brick->getGBR())/confidence; + TFL = (TFL*(confidence-1) + other_brick->getTFL())/confidence; + TFR = (TFR*(confidence-1) + other_brick->getTFR())/confidence; + TBL = (TBL*(confidence-1) + other_brick->getTBL())/confidence; + TBR = (TBR*(confidence-1) + other_brick->getTBR())/confidence; + deleteRendering(); + return true; +} + +//! Print information about this shape. (Virtual in BaseData.) +void +BrickData::printParams() const { + cout << "Type = " << getTypeName(); + cout << "Shape ID = " << getId() << endl; + cout << "Parent ID = " << getParentId() << endl; + + // Print critical points. + cout << endl; + //cout << "center{" << getCentroid().coordX() << ", " << getCentroid().coordY() << "}" << endl; + /* Didn't work initially... not sure why, didn't try too hard. + cout<<"GFL: "; GFL.printData(); cout<* BrickData::render() const { + SketchSpace &renderspace = space->getDualSpace(); + //int const width = renderspace.getWidth(); + //int const height = renderspace.getHeight(); + //float x1,y1,x2,y2; + Sketch* draw_result = + new Sketch(renderspace, "render("+getName()+")"); + (*draw_result)->setParentId(getViewableId()); + (*draw_result)->setColor(getColor()); + *draw_result = 0; + LineData GF(*space, GFL, GFR); + *draw_result = *draw_result & GF.getRendering(); + LineData GL(*space, GFL, GBL); + *draw_result = *draw_result & GL.getRendering(); + LineData GB(*space, GBL, GBR); + *draw_result = *draw_result & GB.getRendering(); + LineData GR(*space, GBR, GFR); + *draw_result = *draw_result & GR.getRendering(); + + return draw_result; +} + + + + + // Old version of brick extraction + // Final version resides in extractBrick +std::vector > BrickData::findBricks(ShapeSpace& ShS, std::vector > lines) +{ + const float lengthConst = .3; + std::vector > resultLines; + std::vector > resultBricks; + float longLength, shortLength; + + if (lines.size() < 3) + { + return resultBricks; + } + + lines = stable_sort(lines, not2(LineData::LengthLessThan())); + + DO_SHAPEVEC(lines, LineData, l1, { + DO_SHAPENEXT(lines, LineData, l1, l2, { + if (l1->getLength() > l2->getLength()){ + longLength = l1->getLength(); + shortLength = l2->getLength(); + } + else { + longLength = l2->getLength(); + shortLength = l1->getLength(); + } + if (LineData::ParallelTest()(l1,l2) && !LineData::ColinearTest()(l1,l2) && + (shortLength / longLength > lengthConst)) { + DO_SHAPENEXT(lines, LineData, l2, l3, { + if (LineData::ParallelTest()(l1,l3) && + !LineData::ColinearTest()(l1,l3) && + LineData::ParallelTest()(l2,l3) && + !LineData::ColinearTest()(l2,l3) && + (l3->getLength() / longLength > lengthConst) && + (shortLength / l3->getLength() > lengthConst)) { + NEW_SHAPE_N(l1_2, LineData, new LineData(ShS, l1->leftPt(), l1->rightPt())); + NEW_SHAPE_N(l2_2, LineData, new LineData(ShS, l2->leftPt(), l2->rightPt())); + NEW_SHAPE_N(l3_2, LineData, new LineData(ShS, l3->leftPt(), l3->rightPt())); + l1_2->setParentId(l1->getViewableId()); + l2_2->setParentId(l1->getViewableId()); + l3_2->setParentId(l1->getViewableId()); + resultLines.push_back(l1_2); + resultLines.push_back(l2_2); + resultLines.push_back(l3_2); + } + }); + } + }); + }); + + if (resultLines.size() < 3) { + return resultBricks; + } + + for (unsigned int i=0; i& final1 = ShapeRootTypeConst(resultLines[i+0],LineData); + const Shape& final2 = ShapeRootTypeConst(resultLines[i+1],LineData); + const Shape& final3 = ShapeRootTypeConst(resultLines[i+2],LineData); + /*cout<<"3 lines:"<printEnds(); + final2->printEnds(); + final3->printEnds();*/ + + std::vector > threeLines; + + if (final1->bottomPt().isBelow(final2->bottomPt(),camcentric)) + { + if (final1->bottomPt().isBelow(final3->bottomPt(),camcentric)) + { + threeLines.push_back(final1); + if (final2->bottomPt().isBelow(final3->bottomPt(),camcentric)) + { + threeLines.push_back(final2); + threeLines.push_back(final3); + } + else + { + threeLines.push_back(final3); + threeLines.push_back(final3); + } + } + else + { + threeLines.push_back(final3); + threeLines.push_back(final1); + threeLines.push_back(final2); + } + } + else + { + if (final2->bottomPt().isBelow(final3->bottomPt(),camcentric)) + { + threeLines.push_back(final2); + if (final1->bottomPt().isBelow(final3->bottomPt(),camcentric)) + { + threeLines.push_back(final1); + threeLines.push_back(final3); + } + else + { + threeLines.push_back(final3); + threeLines.push_back(final1); + } + } + else + { + threeLines.push_back(final3); + threeLines.push_back(final2); + threeLines.push_back(final1); + } + } + const Shape &bottom = ShapeRootTypeConst(threeLines[0], LineData); + const Shape &mid = ShapeRootTypeConst(threeLines[1], LineData); + const Shape &top = ShapeRootTypeConst(threeLines[2], LineData); + + /*cout<<"Sorted Lines: "<printEnds(); + mid->printEnds(); + top->printEnds();*/ + + Point gbl(top->leftPt()+(bottom->leftPt() - mid->leftPt())); + Point gbr(top->rightPt()+(bottom->rightPt() - mid->rightPt())); + Shape newBrick (ShS, (Point&)(bottom->leftPt()), (Point&)bottom->rightPt(), + gbl, gbr, + (Point&)mid->leftPt(), (Point&)mid->rightPt(), + (Point&)top->leftPt(), (Point&)top->rightPt()); + + newBrick->setParentId(final1->getViewableId()); + + NEW_SHAPE(brickl1, LineData, Shape(bottom.getData())); + NEW_SHAPE(brickl2, LineData, Shape(mid.getData())); + NEW_SHAPE(brickl3, LineData, Shape(top.getData())); + + brickl1->setParentId(newBrick->getViewableId()); + brickl2->setParentId(newBrick->getViewableId()); + brickl3->setParentId(newBrick->getViewableId()); + + resultBricks.push_back(newBrick); + } + + return resultBricks; +} + + +// Find bricks from 3 vectors of candidate blobs +// Each vector is blobs of a different face color +// +// Also an old version. Final version is in extractBrick +std::vector > BrickData::findBricksFromBlobs(ShapeSpace& ShS, + std::vector > blobs1, + std::vector > blobs2, + std::vector > blobs3) +{ + + const float distanceThresh = 10; + + unsigned int i, i1, i2; + Shape blob1, blob2, blob3; + + std::vector > resultBricks; + + Point GFL, GFR, GBL, GBR, TFL, TFR, TBL, TBR; + Point c12,c13,c23; + + std::vector > corners1; + std::vector > corners2; + std::vector > corners3; + std::vector used1; + std::vector used2; + std::vector used3; + for (i=0; ifindCornersDiagonal()); + } + for (i=0; ifindCornersDiagonal()); + } + for (i=0; ifindCornersDiagonal()); + } + + // Look for bricks with a good common edge between each pair of viable brick face colors + int used; + for (i1=0; i1=0) { + used1[i1] = true; + used2[i2] = true; + used3[used] = true; + resultBricks[resultBricks.size()-1]->setParentId(blobs1[i1]->getViewableId()); + } + } + } + } + + for (i1=0; i1=0) { + used1[i1] = true; + used3[i2] = true; + used2[used] = true; + resultBricks[resultBricks.size()-1]->setParentId(blobs1[i1]->getViewableId()); + } + } + } + } + + for (i1=0; i1=0) { + used3[i1] = true; + used2[i2] = true; + used1[used] = true; + resultBricks[resultBricks.size()-1]->setParentId(blobs3[i1]->getViewableId()); + } + } + } + } + + + return resultBricks; +} + + +// Subroutine for blob brick detection +// If the two sides match, finds the third side that matches +// Extrapolates all the points and adds the brick to the vector +// returns the index of the third face, or -1 if no match is found +// +// subroutine for an old version +int BrickData::addBrickWithTwoSides(ShapeSpace &ShS, + std::vector& corners1, + std::vector& corners2, + std::vector >& blobs3, + std::vector >& result, + float distanceThresh) +{ + unsigned int blobi; + int i1=-1,j1=-1, i2=-1, j2=-1; + for (int i=0; i<4 && i2<0; i++) { + for (int j=0; j<4 && j2<0; j++) { + if (corners1[i].distanceFrom(corners2[j]) < distanceThresh) { + if (corners1[(i+1)%4].distanceFrom(corners2[(j+3)%4]) < distanceThresh) { + i1 = i; + i2 = (i+1)%4; + j1 = (j+3)%4; + j2 = j; + } + else if (corners1[(i+3)%4].distanceFrom(corners2[(j+1)%4]) < distanceThresh) { + i1 = (i+3)%4; + i2 = i; + j1 = j; + j2 = (j+1)%4; + } + if (i2>=0) { + // Two matching corners have been found: (i1,j2), (i2, j1) + // Look for the third side + bool found = false; + Point center, mid1, mid2, mid3, c1, c2, c3; + for (blobi=0; blobi 1) { + // (i1,j2, k) is the center + found = true; + mid1 = (corners1[i2]+corners2[j1])/2; + if (blobs3[blobi][k].distanceFrom(corners1[i1]) < distanceThresh) + center = (corners1[i1]+corners2[j2]+blobs3[blobi][k])/3; + else + center = (corners1[i1]+corners2[j2])/2; + if (blobs3[blobi][(k+1)%4].distanceFrom(corners1[(i1+3)%4]) < distanceThresh) + mid2 = (blobs3[blobi][(k+1)%4] + corners1[(i1+3)%4])/2; + else + mid2 = corners1[(i1+3)%4]; + if (blobs3[blobi][(k+3)%4].distanceFrom(corners2[(j2+1)%4]) < distanceThresh) + mid3 = (blobs3[blobi][(k+3)%4] + corners2[(j2+1)%4])/2; + else + mid3 = corners2[(j2+1)%4]; + c1 = corners1[(i1+2)%4]; + c2 = blobs3[blobi][(k+2)%4]; + c3 = corners2[(j2+2)%4]; + } + else if (((blobs3[blobi][k].distanceFrom(corners1[i2]) < distanceThresh) + + (blobs3[blobi][(k+1)%4].distanceFrom(corners2[(j1+3)%4]) < distanceThresh) + + (blobs3[blobi][(k+3)%4].distanceFrom(corners1[(i2+1)%4]) < distanceThresh)) > 1) { + // (i2, j1, k) is the center + found = true; + mid1 = (corners1[i1]+corners2[j2])/2; + if (blobs3[blobi][k].distanceFrom(corners1[i2]) < distanceThresh) + center = (corners1[i2]+corners2[j1]+blobs3[blobi][k])/3; + else + center = (corners1[i2]+corners2[j1])/2; + if (blobs3[blobi][(k+1)%4].distanceFrom(corners2[(j1+3)%4]) < distanceThresh) + mid2 = (blobs3[blobi][(k+1)%4] + corners2[(j1+3)%4])/2; + else + mid2 = corners2[(j1+3)%4]; + if (blobs3[blobi][(k+3)%4].distanceFrom(corners1[(i2+1)%4]) < distanceThresh) + mid3 = (blobs3[blobi][(k+3)%4] + corners1[(i2+1)%4])/2; + else + mid3 = corners1[(i2+1)%4]; + c1 = corners2[(j1+2)%4]; + c2 = blobs3[blobi][(k+2)%4]; + c3 = corners1[(i2+2)%4]; + } + } + + } + + if (found) { + // Found a brick, figure out where the top / left / etc sides are + + // Debug shapes + NEW_SHAPE(centerp, PointData, new PointData(ShS, center)); + NEW_SHAPE(mid1p, PointData, new PointData(ShS, mid1)); + NEW_SHAPE(mid2p, PointData, new PointData(ShS, mid2)); + NEW_SHAPE(mid3p, PointData, new PointData(ShS, mid3)); + NEW_SHAPE(c1p, PointData, new PointData(ShS, c1)); + NEW_SHAPE(c2p, PointData, new PointData(ShS, c2)); + NEW_SHAPE(c3p, PointData, new PointData(ShS, c3)); + centerp->setColor(rgb(150,0,255)); + mid1p->setColor(rgb(150,0,255)); + mid2p->setColor(rgb(150,0,255)); + mid3p->setColor(rgb(150,0,255)); + c1p->setColor(rgb(150,0,255)); + c2p->setColor(rgb(150,0,255)); + c3p->setColor(rgb(150,0,255)); + + Point GFL, GFR, GBL, GBR, TFL, TFR, TBL, TBR; + + TFR = center; + if (mid1.isBelow(center, camcentric) && mid1.isBelow(mid2, camcentric) && mid1.isBelow(mid3, camcentric)) { + GFR = mid1; + GBR = c1; + if (mid2.isRightOf(mid3, camcentric)) { + TBR = mid2; + TBL = c2; + TFL = mid3; + GFL = c3; + } + else { + TBR = mid3; + TBL = c3; + TFL = mid2; + GFL = c2; + } + } + else if (mid2.isBelow(center, camcentric) && mid2.isBelow(mid1, camcentric) && mid2.isBelow(mid3, camcentric)) { + GFR = mid2; + GBR = c2; + if (mid1.isRightOf(mid3, camcentric)) { + TBR = mid1; + TBL = c1; + TFL = mid3; + GFL = c3; + } + else { + TBR = mid3; + TBL = c3; + TFL = mid1; + GFL = c1; + } + } + else { + GFR = mid3; + GBR = c3; + if (mid2.isRightOf(mid1, camcentric)) { + TBR = mid2; + TBL = c2; + TFL = mid1; + GFL = c1; + } + else { + TBR = mid1; + TBL = c1; + TFL = mid2; + GFL = c2; + } + } + + GBL = GFL + (TBL - TFL); + + Shape newBrick(ShS, GFL, GFR, GBL, GBR, TFL, TFR, TBL, TBR); + result.push_back(newBrick); + centerp->setParentId(newBrick->getViewableId()); + mid1p->setParentId(newBrick->getViewableId()); + mid2p->setParentId(newBrick->getViewableId()); + mid3p->setParentId(newBrick->getViewableId()); + c1p->setParentId(newBrick->getViewableId()); + c2p->setParentId(newBrick->getViewableId()); + c3p->setParentId(newBrick->getViewableId()); + return blobi; + } + else { + // What do we do if we get two good faces and no third? + } + + } + } + } + } + + return -1; + +} + + + + + +/* Unified brick extraction starting from 2 or 3 blobs, each of a different color + * + ************* Final Version *************** + * + * Checks the relative locations of the blobs (to ensure they are adjacent enough to + * be part of the same brick) + * + * Computes the interior edge lines from the regions close to two faces. + * + * Computes the corners of each face individually. + * This step is handled in blobData::findCorners + * + * Combine all the corner guesses together and extrapolate missing corners. + * + * + * Some helper functions can be found in BrickOps.h + */ +Shape BrickData::extractBrick(ShapeSpace& space, vector > &blobs) +{ + unsigned int nblobs = blobs.size(); + std::vector centroids; + + ShapeRoot invalid; + if (nblobs > 3 || nblobs < 2) { + return ShapeRootType(invalid,BrickData); + } + + for (unsigned int i=0; igetCentroid()); + } + + // Check inter-blob distance + // If any pair of blobs are too far apart, this set is invalid + for (unsigned int i=0; i= + blobs[i]->topLeft.distanceFrom(centroids[i]) + + blobs[j]->topLeft.distanceFrom(centroids[j])){ + return ShapeRootType(invalid,BrickData); + } + } + } + + + // arbitrary face assignment, aligned as the brick is in camera space + // this will probably break down if the dog tilts its head + // + // if we only have two faces, we're assuming only the top and "left" faces are visible + // always assume a top face is visible, becuase any shape without a top face is much more likely + // to be a pyramid than a tall brick + int top=-1, left=-1, right=-1; + + if (centroids[0].isAbove(centroids[1], camcentric)) { + top = 0; + } + else { + top = 1; + } + if (nblobs > 2 && centroids[2].isAbove(centroids[top], camcentric)) { + top = 2; + } + + if ((top != 0 && centroids[0].isLeftOf(centroids[1], camcentric)) || top == 1) { + left = 0; + } + else { + left = 1; + } + if (nblobs>2 && top != 2 && centroids[2].isLeftOf(centroids[left], camcentric)) { + left = 2; + } + + if (nblobs > 2) { + if (top != 0 && left != 0) { + right = 0; + } + else if (top != 1 && left != 1) { + right = 1; + } + else { + right = 2; + } + } + + if (top == left || top == right || left == right){ + std::cout<<"ERROR: Brick side misclassification!"< > boundaries; + NEW_SKETCH_N(topEdist, uchar, visops::edist(blobs[top]->getRendering())); + NEW_SKETCH_N(leftEdist, uchar, visops::edist(blobs[left]->getRendering())); + NEW_SKETCH(tlsmall, bool, topEdistmax()){ + return ShapeRootType(invalid,BrickData); + } + NEW_SKETCH(tl, bool, topEdistsetColor(green_rgb); + boundaries.push_back(tl); + + if (nblobs>2) { + NEW_SKETCH_N(rightEdist, uchar, visops::edist(blobs[right]->getRendering())); + NEW_SKETCH(trsmall, bool, topEdistmax()){ + return ShapeRootType(invalid,BrickData); + } + NEW_SKETCH(tr, bool, topEdistsetColor(green_rgb); + boundaries.push_back(tr); + + NEW_SKETCH(lrsmall, bool, leftEdistmax()){ + return ShapeRootType(invalid,BrickData); + } + NEW_SKETCH(lr, bool, leftEdistsetColor(green_rgb); + boundaries.push_back(lr); + } + + + + // Begin gathering evidence for each point + // This should probably be augmented by an extra vector of confidences + std::vector > points(8); + + // Arbitrary corner / face assignemt (in cam space) + // + // 5------6 + // / T /| + // /(4) / 7 + // 1------2 / <-- R + // | L |/ + // 0------3 + // + + + // Construct the interior lines + // Add their guesses to the appropriate points on the brick + NEW_SHAPE(tlline, LineData, LineData::extractLine(boundaries[0])); + Shape trline, lrline; + + if (right!=-1) { + trline = LineData::extractLine(boundaries[1]); + lrline = LineData::extractLine(boundaries[2]); + + // shorten interior lines based on their intersections + bool on1=false, on2=false; + if (tlline.isValid() && trline.isValid()) { + Point isectPt = tlline->intersectionWithLine(trline, on1, on2); + if (on1) { + tlline->rightPt().setCoords(isectPt); + } + if (on2) { + trline->leftPt().setCoords(isectPt); + } + } + + if (tlline.isValid() && lrline.isValid()) { + Point isectPt = tlline->intersectionWithLine(lrline, on1, on2); + if (on1) { + tlline->rightPt().setCoords(isectPt); + } + if (on2) { + lrline->topPt().setCoords(isectPt); + } + } + + if (trline.isValid() && lrline.isValid()) { + Point isectPt = trline->intersectionWithLine(lrline, on1, on2); + if (on1) { + trline->leftPt().setCoords(isectPt); + } + if (on2) { + lrline->topPt().setCoords(isectPt); + } + } + + + if (trline.isValid()) { + points[2].push_back(trline->leftPt()); + points[6].push_back(trline->rightPt()); + } + + if (lrline.isValid()) { + points[2].push_back(lrline->topPt()); + points[3].push_back(lrline->bottomPt()); + } + } + + if (tlline.isValid()) { + points[1].push_back(tlline->leftPt()); + points[2].push_back(tlline->rightPt()); + } + + + + // Extract the corners of the blob using the derivative approach + // + // corners are coming out of findCorners() in counter-clock-wise order around the blob + // with unknown starting position + // + // the findCorners function in BlobData is the other important part of the algorithm + // Several different methods of corner extraction can be connected there. + std::vector candidates; + + // top face + + // Compute an orthogonal bounding box from the top-left line + if (tlline.isValid()) { + if (trline.isValid() && trline->getLength() > tlline->getLength()) { + candidates = findOrthogonalBoundingBox(space, blobs[top], centroids[top], trline); + } + else { + candidates = findOrthogonalBoundingBox(space, blobs[top], centroids[top], tlline); + } + } + else if (trline.isValid()) { + candidates = findOrthogonalBoundingBox(space, blobs[top], centroids[top], trline); + } + + float cornerValue; + + std::vector tcorners = blobs[top]->findCorners(4, candidates, cornerValue); + + if (tcorners.size() == 4) { + + // Sort out which corner is which + // if there's a clear left-most corner, its corner 1 + // if two corners are close in left-ness, then they should be 1 and 5 + unsigned int leftCorner = 0; + for (unsigned int i=1; i<4; i++){ + if (tcorners[i].isLeftOf(tcorners[leftCorner], camcentric)) { + leftCorner = i; + } + } + int closeCorner = -1; + float mostLeft = tcorners[leftCorner].coordX(); + const int MAX_CLOSE_DIST = 5; + for (unsigned int i=0; i<4; i++) { + if (i != leftCorner && tcorners[i].coordX() - mostLeft < MAX_CLOSE_DIST) { + closeCorner = i; + } + } + + if (closeCorner != -1) { + // If 5 was actually left-most, switch leftCorner to 1 + if ((unsigned int)closeCorner == (leftCorner+1)%4) { + leftCorner = closeCorner; + } + else if ((unsigned int)closeCorner != (leftCorner+3)%4) { + std::cout<<"WARNING, opposite corners are close together!"<getLength() > tlline->getLength()) { + candidates = findOrthogonalBoundingBox(space, blobs[left], centroids[left], lrline); + } + else { + candidates = findOrthogonalBoundingBox(space, blobs[left], centroids[left], tlline); + } + } + else if (trline.isValid()) { + candidates = findOrthogonalBoundingBox(space, blobs[left], centroids[left], lrline); + } + + for (unsigned int i=0; isetParentId(blobs[left]->getViewableId()); + } + std::vector lcorners = blobs[left]->findCorners(4, candidates, cornerValue); + if (lcorners.size() == 4) { + + // if there's a clear right-most corner, its corner 3 + // if two corners are close in right-ness, then they should 3 and 2 + unsigned int rightCorner = 0; + for (unsigned int i=1; i<4; i++){ + if (lcorners[i].isRightOf(lcorners[rightCorner], camcentric)) { + rightCorner = i; + } + } + int closeCorner = -1; + float mostRight = lcorners[rightCorner].coordX(); + const int MAX_CLOSE_DIST = 5; + for (unsigned int i=0; i<4; i++) { + if (i != rightCorner && mostRight - lcorners[i].coordX() < MAX_CLOSE_DIST) { + closeCorner = i; + } + } + + if (closeCorner != -1) { + // If 2 was actually right-most, switch rightCorner to 3 + if ((unsigned int)closeCorner == (rightCorner+3)%4) { + rightCorner = closeCorner; + } + else if ((unsigned int)closeCorner != (rightCorner+1)%4) { + std::cout<<"WARNING, opposite corners are close together!"<getLength() > trline->getLength()) { + candidates = findOrthogonalBoundingBox(space, blobs[right], centroids[right], lrline); + } + else { + candidates = findOrthogonalBoundingBox(space, blobs[right], centroids[right], trline); + } + } + else if (lrline.isValid()) { + candidates = findOrthogonalBoundingBox(space, blobs[right], centroids[right], lrline); + } + + std::vector rcorners = blobs[right]->findCorners(4, candidates, cornerValue); + if (rcorners.size() == 4) { + + // if there's a clear bottom-most corner, its corner 3 + // if two corners are close in bottom-ness, then they should 3 and 7 + unsigned int bottomCorner = 0; + for (unsigned int i=1; i<4; i++){ + if (rcorners[i].isBelow(rcorners[bottomCorner], camcentric)) { + bottomCorner = i; + } + } + int closeCorner = -1; + float mostBottom = rcorners[bottomCorner].coordY(); + const int MAX_CLOSE_DIST = 5; + for (unsigned int i=0; i<4; i++) { + if (i != bottomCorner && mostBottom - rcorners[i].coordY() < MAX_CLOSE_DIST) { + closeCorner = i; + } + } + + if (closeCorner != -1) { + // If 7 was actually bottom-most, switch bottomCorner to 3 + if ((unsigned int)closeCorner == (bottomCorner+3)%4) { + bottomCorner = closeCorner; + } + else if ((unsigned int)closeCorner != (bottomCorner+1)%4) { + std::cout<<"WARNING, opposite corners are close together!"<setName(name); + } + }*/ + + + // Now merge all our candidate points to get the final brick + + vector finalCorners(8); + + // for now just average corners together to get the last corners + // should at least weight the average based on confidence + // could also do statistics + // produce a final confidence based on the std. deviation + + // if we didn't get the center point of the brick then we don't have a shot + // top-left line also is pretty critical, + // though I probably should re-work below to work in the case where top and right only work well + if (points[2].size()==0 || points[1].size()==0){ + return ShapeRootType(invalid,BrickData); + } + + Point empty(0,0); + + + // Lots of cases for which corners are visible and extrapolating the others + + for (unsigned int i=0; i<8; i++) { + finalCorners[i] = empty; + for (unsigned int j=0; j 0) { + finalCorners[i]/=points[i].size(); + } + } + + if (points[3].size()==0){ + // These should be easier cases + return ShapeRootType(invalid,BrickData); + } + + if (points[6].size() == 0) { + // If we have the top left line but not corner 6, infer corner 6 from the thickness of the top side + if (tlline.isValid()) { + NEW_SKETCH_N(topRendering, bool, blobs[top]->getRendering()); + Point topPt = (Region::extractRegion(topRendering)).mostDistantPtFrom(tlline.getData()); + NEW_SHAPE(intersectLine, LineData, LineData(space, topPt, tlline->getThetaNorm())); + Point intersectPoint = intersectLine->intersectionWithLine(tlline); + // lower confidence result + finalCorners[6] = topPt+finalCorners[2]-intersectPoint; + } + // can also use right left line and the right side, but the top side is more likely to be good + else { + return ShapeRootType(invalid,BrickData); + } + } + + if (points[0].size() == 0) { + finalCorners[0] = finalCorners[3] + finalCorners[1]-finalCorners[2]; + } + + if (points[5].size() == 0) { + finalCorners[5] = finalCorners[6] + finalCorners[1]-finalCorners[2]; + } + + if (points[7].size() == 0) { + finalCorners[7] = finalCorners[3] + finalCorners[6]-finalCorners[2]; + } + + finalCorners[4] = finalCorners[5] + finalCorners[7] - finalCorners[6] + + finalCorners[5] + finalCorners[0] - finalCorners[1] + + finalCorners[0] + finalCorners[7] - finalCorners[3]; + finalCorners[4]/=3.0; + + // left == front for generalized brick + NEW_SHAPE(returnbrick, BrickData, new BrickData(space, + finalCorners[0], finalCorners[3], + finalCorners[4], finalCorners[7], + finalCorners[1], finalCorners[2], + finalCorners[5], finalCorners[6])); + return returnbrick; + +} + + + + // Generates a wide bounding box around the blob from a parallel line along one edge + // This bounding box will be used as a starting point for the quadrilateral-fitting algorithm + // + // Generates 4 points based on the line, centroid of the blob, and parameters for extending the bounds + // Then sorts the points into counter-clockwise order to satisfy the ordering constraints to make other + // extrapolations possible. +vector BrickData::findOrthogonalBoundingBox(ShapeSpace& space, Shape blob, Point centroid, Shape parallel) +{ + const float PARALLEL_EXTEND_FACTOR = .6, ORTHO_EXTEND_FACTOR = .6; + + vector candidates; + LineData candidate1(space, parallel->end1Pt(), parallel->end2Pt()); + + //candidate1.setEndPts(parallel->end1Pt(), parallel->end2Pt()); + Point parallelExtend(candidate1.end2Pt() - candidate1.end1Pt()); + parallelExtend *= PARALLEL_EXTEND_FACTOR; + LineData ortho(space, centroid, candidate1.getThetaNorm()); + Point orthoIsect = candidate1.intersectionWithLine(ortho); + Point candidate2Offset = (centroid - orthoIsect) * 2.0; + Point orthoExtend = candidate2Offset * ORTHO_EXTEND_FACTOR; + + LineData candidate2(space, candidate1.end1Pt() + candidate2Offset + orthoExtend - parallelExtend, + candidate1.end2Pt() + candidate2Offset + orthoExtend + parallelExtend); + candidate1.setEndPts(candidate1.end1Pt() - orthoExtend - parallelExtend, + candidate1.end2Pt() - orthoExtend + parallelExtend); + candidates.push_back(candidate1.leftPt()); + candidates.push_back(candidate1.rightPt()); + candidates.push_back(candidate2.rightPt()); + candidates.push_back(candidate2.leftPt()); + + // debug output + for (unsigned int i=0; isetParentId(blob->getViewableId()); + } + + // order counter-clockwise around the blob before returning + Point centerPoint = (candidates[0] + candidates[1] + candidates[2] + candidates[3]) / 4.0; + vector centerAngles; + for (unsigned int i=0; isetParentId(blob->getViewableId()); + centerAngles.push_back(atan2(centerLine->end2Pt().coordY() - centerLine->end1Pt().coordY(), + centerLine->end2Pt().coordX() - centerLine->end1Pt().coordX())); + } + + + vector orderedPoints; + + int maxi = 0, mini = 0; + float maxAng = centerAngles[0]; + float minAng = maxAng; + + for (unsigned int i=1; i maxAng) { + maxAng = centerAngles[i]; + maxi = i; + } + if (centerAngles[i] < minAng) { + minAng = centerAngles[i]; + mini = i; + } + + } + + orderedPoints.push_back(candidates[maxi]); + + float lastMax = maxAng; + for (unsigned int i=1; i maxAng && curAng < lastMax) { + maxi = j; + maxAng = curAng; + } + } + orderedPoints.push_back(candidates[maxi]); + lastMax = maxAng; + } + + // debug output + for (unsigned int i=0; isetParentId(blob->getViewableId()); + } + + return orderedPoints; +} + + + +} // namespace diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/BrickData.h ./DualCoding/BrickData.h --- ../Tekkotsu_2.4.1/DualCoding/BrickData.h 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/BrickData.h 2006-07-21 13:57:45.000000000 -0400 @@ -0,0 +1,115 @@ +//-*-c++-*- +#ifndef _BRICKDATA_H_ +#define _BRICKDATA_H_ + +#include +#include +#include + +#include "Shared/newmat/newmat.h" + +#include "BaseData.h" // superclass +#include "Point.h" // Point data members +#include "ShapeTypes.h" // brickDataType + +#include "LineData.h" +#include "ShapeLine.h" +#include "ShapeBlob.h" + +namespace DualCoding { + +class ShapeRoot; +class SketchSpace; +template class Sketch; + +class BrickData : public BaseData { +private: + // T=Top, G=Ground, F=front, B=back, L=left, R=right + EndPoint GFL; + EndPoint GFR; + EndPoint GBL; + EndPoint GBR; + EndPoint TFL; + EndPoint TFR; + EndPoint TBL; + EndPoint TBR; + + Point centroid; + +public: + + //! Constructor + BrickData(ShapeSpace& _space, + const EndPoint &GFL, const EndPoint &GFR, const EndPoint &GBL, const EndPoint &GBR, + const EndPoint &TFL, const EndPoint &TFR, const EndPoint &TBL, const EndPoint &TBR); + + //! Copy constructor + //BrickData(BrickData& otherBrick); + + static ShapeType_t getStaticType() { return brickDataType; } + DATASTUFF_H(BrickData); + + //! Centroid. (Virtual in BaseData.) + Point getCentroid() const { return centroid; } + + EndPoint getGFL() {return GFL;} + EndPoint getGFR() {return GFR;} + EndPoint getGBL() {return GBL;} + EndPoint getGBR() {return GBR;} + EndPoint getTFL() {return TFL;} + EndPoint getTFR() {return TFR;} + EndPoint getTBL() {return TBL;} + EndPoint getTBR() {return TBR;} + + //! Match bricks based on their parameters. (Virtual in BaseData.) + virtual bool isMatchFor(const ShapeRoot& other) const; + + virtual void mergeWith(const ShapeRoot& other); + + virtual bool isAdmissible() const { return true; } + + virtual bool updateParams(const ShapeRoot& other, bool force=false); + + //! Print information about this shape. (Virtual in BaseData.) + virtual void printParams() const; + + //! Transformations. (Virtual in BaseData.) + void applyTransform(const NEWMAT::Matrix& Tmat); + + //! Project to ground + virtual void projectToGround(const NEWMAT::Matrix& camToBase, + const NEWMAT::ColumnVector& groundplane); + + virtual unsigned short getDimension() const { return 3; } + + //! Extraction. + static std::vector > findBricks(ShapeSpace& ShS, std::vector > lines); + + static std::vector > findBricksFromBlobs(ShapeSpace& ShS, + std::vector > blobs1, + std::vector > blobs2, + std::vector > blobs3); + + static Shape extractBrick(ShapeSpace& space, vector > &blobs); + + static vector findOrthogonalBoundingBox(ShapeSpace& space, Shape blob, Point centroid, Shape parallel); +private: + //! Render into a sketch space and return reference. (Private.) + Sketch* render() const; + + + static int addBrickWithTwoSides(ShapeSpace& ShS, + std::vector& corners1, + std::vector& corners2, + std::vector >& blobs3, + std::vector >& result, + float distanceThresh); + + //@} + + BrickData& operator=(const BrickData&); //!< don't call +}; + +} // namespace + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/BrickOps.cc ./DualCoding/BrickOps.cc --- ../Tekkotsu_2.4.1/DualCoding/BrickOps.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/BrickOps.cc 2006-08-10 23:19:53.000000000 -0400 @@ -0,0 +1,418 @@ +#include "BrickOps.h" +#include "visops.h" + +namespace DualCoding { + + /* Begin functionality for distance-from-center methods of finding the corners of a blob*/ + + + /* Find the point at which the given line hits the edge of the rendered region */ + Point findEdgePoint(Point start, Point end, Sketch& rendering) + { + int gap_tolerance=2; + + float startx = start.coordX(); + float starty = start.coordY(); + float dx = end.coordX() - startx; + float dy = end.coordY() - starty; + + float dist = sqrt(dx*dx + dy*dy); + dx = dx/dist; + dy = dy/dist; + int maxi=0, gap=0; + int x,y; + for (int i=0; igap_tolerance) + break; + } + float fx = startx+maxi*dx; + float fy = starty+maxi*dy; + Point result(fx,fy); + return result; + } + + + /* + * Finds the edge points of the region by drawing lines out from the center to the edges + * Points and distances returned are indexed by angle, so some overlap is possible. + * The return value is the index of the most distant point. + */ + int findRadialDistancesFromPoint(Point center, float radius, + Sketch& rendering, + float distances[], + Point points[]) + { + NEW_SKETCH(circle, bool, visops::zeros(rendering)); + + int maxi = 0, origmaxi = 0; + float max = -1; + bool stillmax = false; + Point edge; + for (int i=0; i<2*M_PI*radius; i++){ + float x = center.coordX()+radius*cos(i/radius); + float y = center.coordY()+radius*sin(i/radius); + if (x<0) x = 0; + else if (x>=rendering->getWidth()) x = rendering->getWidth()-1; + if (y<0) y = 0; + else if (y>=rendering->getHeight()) y = rendering->getHeight()-1; + edge.setCoords(x,y,0); + circle->at((int)(edge.coordX()), (int)(edge.coordY())) = 1; + points[i] = findEdgePoint(center,edge, rendering); + distances[i] = points[i].distanceFrom(center); + if (distances[i] > max) { + max = distances[i]; + maxi = i; + origmaxi = i; + stillmax = true; + } + else if (distances[i] == max && stillmax) { + maxi = (origmaxi + i) / 2; + } + else { + stillmax = false; + } + + } + + return maxi; + } + + + /* + * Takes the derivative of the x array, returned in dx + */ + void takeDerivative(float x[], float dx[], int len) + { + for (int i=0; i& parent) + { + float xscale = 1; + if (len > parent->getWidth() - 20) xscale = (parent->getWidth()-20)/(1.0*len); + float ymax = 0; + for (unsigned int i=0; i maxheight) ? maxheight/ymax : 1.0; + // Draw a histogram + NEW_SKETCH(hist,bool, visops::zeros(parent)); + for(unsigned int i=0; i 0) { + for (unsigned int j=0; jat((int)(i*xscale+10), (int)(maxheight - j*yshrink + 5)) = 1; + } + } + else { + for (int j=-1; j>yscale*distances[i]; j--) { + hist->at((int)(i*xscale+10), (int)(maxheight - j*yshrink + 5)) = 1; + } + } + } + + hist->at(5,(int)(maxheight*(1-yshrink)+5)) = 1; + hist->at(5,(int)(maxheight*(1+yshrink)+5)) = 1; + hist->at(203,(int)(maxheight*(1-yshrink)+5)) = 1; + hist->at(203,(int)(maxheight*(1+yshrink)+5)) = 1; + + } + + + /* Helper functions for finding corners by fitting a quadrilateral to a blob */ + + + /* + * Gets the score of the given set of corners as a fit to the blob. + * + * The score is a combination of the area of the quadrilateral and the number of edge pixels + * that its lines lie on. + * + * A lower score is better, scores can go negative + */ + float getBoundingQuadrilateralScore(BlobData &blob, vector& corners, + Sketch edgeImage, + int& borderCount, ShapeSpace &space) + { + const float EDGE_SCALAR = 50; + + borderCount = countBorderPixelFit(blob, corners, edgeImage, space); + return getQuadrilateralArea(corners) - EDGE_SCALAR*borderCount; + } + + + + /* + * assuming a convex quadrilateral + * splits the quadrilateral into n-2 triangles and uses the + * edge-length method to find the area of each triangle: + * s = perimeter / 2 + * area = sqrt(s*(s-a)(s-b)(s-c)) + */ + float getQuadrilateralArea(vector& corners) + { + float totalArea = 0; + for (unsigned int i=2; i& corners, + ShapeSpace &space) + { + int ncorners = corners.size(); + + vector lines; + for (int i=0; i::const_iterator it = blob.runvec.begin(); + it != blob.runvec.end(); it++ ) { + const BlobData::run &r = *it; + int const xstop = r.x + r.width; + + totalPixelArea += r.width; + + Point p1(r.x,r.y); + Point p2(xstop, r.y); + + // Check if the endpoints are inside the polygon + bool pt1Inside = true, pt2Inside = true; + for (int i=0; ir.x; x--) { + Point p(x,r.y); + bool ptInside = true; + for (int i=0; i=xstart; x--) { + Point p(x,r.y); + bool ptInside = true; + for (int i=0; i &corners, + Sketch edges, ShapeSpace &space) + { + int ncorners = corners.size(); + + vector lines; + vector dx, dy; + for (int i=0; i scores, float weight) + { + unsigned int i; + float min = scores[0], max = scores[0]; + for (i=1; i max) + max = scores[i]; + } + + max -= min; + for (i=0; i 0) { + scores[i]*=weight/max; + } + } + + vector exps; + float expsum = 0; + for (i=0; i& rendering); + + int findRadialDistancesFromPoint(Point center, float radius, + Sketch& rendering, + float distances[], Point points[]); + + void takeDerivative(float x[], float dx[], int len) ; + + void drawHist(float distances[], unsigned int len, Sketch& parent) ; + + void applyGaussian(float x[], float gx[], int len); + + float getBoundingQuadrilateralScore(BlobData &blob, vector& corners, Sketch edgeImage, + int& borderCount, ShapeSpace &space); + + float getBoundingQuadrilateralInteriorPointRatio(BlobData &blob, + vector& corners, + ShapeSpace &space); + + float getQuadrilateralArea(vector& corners); + + int countBorderPixelFit(BlobData &blob, const vector &corners, + Sketch edges, ShapeSpace &space); + + int pickMove(vector scores, float weight); +} + +#endif /* _BRICK_OPS_H_ */ diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/DualCoding.h ./DualCoding/DualCoding.h --- ../Tekkotsu_2.4.1/DualCoding/DualCoding.h 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/DualCoding.h 2006-07-21 13:57:47.000000000 -0400 @@ -0,0 +1,75 @@ +//-*-c++-*- +// Master include file list to be used by user behaviors. + +#ifndef LOADED_DualCoding_h_ +#define LOADED_DualCoding_h_ + +//! Dual coding vision representations (Sketches and Shapes) +namespace DualCoding {} + +#include "Measures.h" +#include "ShapeTypes.h" +#include "SketchTypes.h" + +#include "SketchSpace.h" +#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 "ViewerConnection.h" +#include "VisualRoutinesStateNode.h" +#include "VisualRoutinesBehavior.h" // this must preceed mapbuilders, pilot and lookout +#include "VRmixin.h" + +#include "LookoutRequests.h" +#include "Lookout.h" +#include "MapBuilderRequests.h" +#include "MapBuilder.h" +#include "Pilot.h" + +#include "Particle.h" +#include "ParticleShapes.h" +#include "ParticleFilter.h" + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/EllipseData.cc ./DualCoding/EllipseData.cc --- ../Tekkotsu_2.4.1/DualCoding/EllipseData.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/EllipseData.cc 2006-08-02 17:27:13.000000000 -0400 @@ -0,0 +1,217 @@ +//-*-c++-*- +#include +#include +#include +#include + +#include "BaseData.h" // superclass +#include "Point.h" // Point data member +#include "Measures.h" // coordinate_t; AngPi data member +#include "ShapeTypes.h" // ellipseDataType + +#include "SketchSpace.h" +#include "Sketch.h" +#include "Region.h" +#include "visops.h" + +#include "ShapeSpace.h" // required by DATASTUFF_CC +#include "ShapeRoot.h" // required by DATASTUFF_CC + +#include "EllipseData.h" +#include "ShapeEllipse.h" + +namespace DualCoding { + +inline int round(float x) { return (int) ceil((double)x-0.5f); } + +EllipseData::EllipseData(ShapeSpace& _space, const Point &c) + : BaseData(_space, getStaticType()), + center_pt(c), semimajor(0), semiminor(0), orientation(0) +{ center_pt.setRefFrameType(getRefFrameType()); + mobile = ELLIPSE_DATA_MOBILE; } + +DATASTUFF_CC(EllipseData); + +BoundingBox EllipseData::getBoundingBox() const { + float o_sin = sin(orientation); + float o_cos = cos(orientation); + Point major_tip(semimajor*o_cos, semimajor*o_sin); + Point minor_tip(-semiminor*o_sin, semiminor*o_cos); + return BoundingBox(BoundingBox(BoundingBox(center_pt+major_tip), + BoundingBox(center_pt-major_tip)), + BoundingBox(BoundingBox(center_pt+minor_tip), + BoundingBox(center_pt-minor_tip))); +} + +bool EllipseData::isMatchFor(const ShapeRoot& other) const { + if (!(isSameTypeAs(other) && isSameColorAs(other))) + return false; + const Shape& other_ellipse = ShapeRootTypeConst(other,EllipseData); + float dist = center_pt.distanceFrom(other_ellipse->centerPt()); + return dist < 2*max(semimajor,other_ellipse->semimajor); // *** DST hack +} + +bool EllipseData::updateParams(const ShapeRoot& other, bool) { + const Shape& other_ellipse = ShapeRootTypeConst(other,EllipseData); + if (other_ellipse->confidence <= 0) + return false; + const int other_conf = other_ellipse->confidence; + center_pt = (center_pt*confidence + other_ellipse->centerPt()*other_conf) / (confidence+other_conf); + semimajor = (semimajor*confidence + other_ellipse->getSemimajor()*other_conf) / (confidence+other_conf); + semiminor = (semiminor*confidence + other_ellipse->getSemiminor()*other_conf) / (confidence+other_conf); + orientation = orientation*((orientation_t)confidence/(confidence+other_conf)) + + other_ellipse->getOrientation()*((orientation_t)confidence/(confidence+other_conf)); + return true; +} + +//! 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; +} + +pair EllipseData::findFeaturePoints() const { + AngPi theta = getOrientation(); + NEWMAT::ColumnVector from_center(4); + + float dl = getSemimajor(); + from_center << dl*cos(theta) << dl*sin(theta) << 0 << 0; + Point majorPt(from_center); + majorPt += center_pt; + + dl = getSemiminor(); + theta = theta + (AngPi) (M_PI/2); + from_center << dl*sin(theta) << dl*cos(theta) << 0 << 0; + Point minorPt(from_center); + minorPt += center_pt; + return pair(majorPt,minorPt); +} + +//! Transformations. (Virtual in BaseData.) +void EllipseData::applyTransform(const NEWMAT::Matrix& Tmat) { + pair featurePts = findFeaturePoints(); + center_pt.applyTransform(Tmat); + // orientation = orientation - (AngTwoPi)atan2(Tmat(1,2),Tmat(1,1)); + featurePts.first.applyTransform(Tmat); + featurePts.second.applyTransform(Tmat); + updateProperties(featurePts.first, featurePts.second); +} + +void EllipseData::updateProperties(const Point& majorPt, const Point& minorPt) { + setSemiminor(minorPt.xyDistanceFrom(center_pt)); + setSemimajor(majorPt.xyDistanceFrom(center_pt)); + setOrientation(atan2(majorPt.coords(2)-center_pt.coords(2),majorPt.coords(1)-center_pt.coords(1))); +} + +void EllipseData::projectToGround(const NEWMAT::Matrix& camToBase, + const NEWMAT::ColumnVector& groundplane) { + pair featurePts = findFeaturePoints(); + center_pt.projectToGround(camToBase,groundplane); + featurePts.first.projectToGround(camToBase,groundplane); + featurePts.second.projectToGround(camToBase,groundplane); + updateProperties(featurePts.first, featurePts.second); +} + +//! Functions to set properties. +//{ +void EllipseData::setOrientation(const AngPi _orientation) { + orientation = AngPi(_orientation); + deleteRendering(); +} + +void EllipseData::setSemimajor(float _semimajor) { + semimajor = _semimajor; + deleteRendering(); +} + +void EllipseData::setSemiminor(float _semiminor) { + semiminor = _semiminor; + deleteRendering(); +} +//} + + +// ================================================== +// BEGIN SKETCH MANIPULATION AND LINE EXTRACTION CODE +// ================================================== + + +//! Ellipse extraction. + +std::vector > EllipseData::extractEllipses(const Sketch& sketch) +{ + const float AREA_TOLERANCE = 0.5; + const int REGION_THRESH = 25; + NEW_SKETCH_N(labels,usint,visops::oldlabelcc(sketch,visops::EightWayConnect)); + list regionlist = Region::extractRegions(labels,REGION_THRESH); + std::vector > ellipses; + + if(regionlist.empty()) + return ellipses; + + typedef list::iterator R_IT; + for (R_IT it = regionlist.begin(); it != regionlist.end(); ++it) { + float ratio = it->findSemiMajorAxisLength()/(float)(it->findSemiMinorAxisLength()); + if((ratio < 2.0) && (ratio > 1.0/(float)2.0) + && (it->findArea() > M_PI*2.0*(it->findSemiMajorAxisLength()) + *2.0*(it->findSemiMinorAxisLength())*AREA_TOLERANCE/4.0)) { + Shape temp_ellipse(*it); + temp_ellipse->setParentId(sketch->getViewableId()); + temp_ellipse->setColor(sketch->getColor()); + ellipses.push_back(Shape(temp_ellipse)); + }; + } + return ellipses; +} + + +//! Render into a sketch space and return reference. (Private.) +Sketch* EllipseData::render() const { + SketchSpace &SkS = space->getDualSpace(); + NEWMAT::ColumnVector ctr(centerPt().getCoords()); + SkS.applyTmat(ctr); + const float &cx = ctr(1); + const float &cy = ctr(2); + const NEWMAT::Matrix &Tmat = SkS.getTmat(); + NEWMAT::ColumnVector ori(2); + ori << cos(orientation) << sin(orientation); + NEWMAT::Matrix rot(2,2); + rot(1,1) = Tmat(1,1); + rot(1,2) = Tmat(1,2); + rot(2,1) = Tmat(2,1); + rot(2,2) = Tmat(2,2); + ori = rot * ori; + const float &cosT = ori(1); + const float &sinT = ori(2); + const float xRange = semimajor / Tmat(4,4); + const float majorSq = xRange*xRange; + const float mnrDevMjr = semiminor/semimajor; + Sketch result(SkS, "render("+getName()+")"); + result = 0; + for (float xDist = -xRange; xDist <= xRange; xDist+=0.2) { + const float yRange = sqrt(max((float)0, majorSq - xDist*xDist)) * mnrDevMjr; + for (float yDist = -yRange; yDist <= yRange; yDist+=0.2) { + int const px = round(cx+xDist*cosT-yDist*sinT); + int const py = round(cy+yDist*cosT+xDist*sinT); + if ( px >= 0 && px < result.width && + py >= 0 && py < result.height ) + result(px,py) = true; + } + } + return new Sketch(result); +} + +} // namespace diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/EllipseData.h ./DualCoding/EllipseData.h --- ../Tekkotsu_2.4.1/DualCoding/EllipseData.h 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/EllipseData.h 2006-05-09 18:37:55.000000000 -0400 @@ -0,0 +1,105 @@ +//-*-c++-*- +#ifndef _ELLIPSEDATA_H_ +#define _ELLIPSEDATA_H_ + +#include +#include + +#include "Shared/newmat/newmat.h" + +#include "BaseData.h" // superclass +#include "Point.h" // Point data member +#include "Measures.h" // coordinate_t; AngPi data member + +namespace DualCoding { + +class ShapeRoot; +class SketchSpace; +template class Sketch; + +#define ELLIPSE_DATA_MOBILE false + +class EllipseData : public BaseData { +private: + Point center_pt; + float semimajor; + float semiminor; + AngPi orientation; + +public: + + //! Constructor + EllipseData(ShapeSpace& _space, const Point &c); + + static ShapeType_t getStaticType() { return ellipseDataType; } + + DATASTUFF_H(EllipseData); + + //! Centroid. (Virtual in BaseData.) + Point getCentroid() const { return center_pt; } + void setCentroidPt(const Point& other) { center_pt.setCoords(other); } + + //! finds points where semiminor or semimajor axis touchs the circumference of ellipse + pair findFeaturePoints() const; + + //! updates major/minor axis and orientation from feature points + void updateProperties(const Point& minorPt, const Point& majorPt); + + BoundingBox getBoundingBox() const; + + //! Match ellipses based on their parameters. (Virtual in BaseData.) + virtual bool isMatchFor(const ShapeRoot& other) const; + + virtual bool isAdmissible() const { + return (semimajor >= 9.0 ); // **DST Hack** minimum size for an ellipse to be added to local map + } + + virtual bool updateParams(const ShapeRoot& other, bool force=false); + + //! Print information about this shape. (Virtual in BaseData.) + virtual void printParams() const; + + //! Transformations. (Virtual in BaseData.) + void applyTransform(const NEWMAT::Matrix& Tmat); + + //! Project to ground + virtual void projectToGround(const NEWMAT::Matrix& camToBase, + const NEWMAT::ColumnVector& groundplane); + + //! Center point access function. + const Point& centerPt() const { return center_pt; } + + virtual unsigned short getDimension() const { return 2; } + + //! Properties functions. + //@{ + AngPi getOrientation() const { return orientation; } + float getSemimajor() const { return semimajor; } + float getSemiminor() const { return semiminor; } + //@} + + + //! Set properties. + //@{ + void setOrientation(const AngPi _orientation); + void setSemimajor(float _semimajor); + void setSemiminor(float _semiminor); + //@} + + //! Extraction. + static std::vector > extractEllipses(const Sketch& sketch); + + +private: + //! Render into a sketch space and return reference. (Private.) + virtual Sketch* render() const; + //@} + + EllipseData& operator=(const EllipseData&); //!< don't call + +}; + + +} // namespace + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/EndPoint.cc ./DualCoding/EndPoint.cc --- ../Tekkotsu_2.4.1/DualCoding/EndPoint.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/EndPoint.cc 2006-02-12 00:32:01.000000000 -0500 @@ -0,0 +1,28 @@ +#include "EndPoint.h" + +namespace DualCoding { + +void EndPoint::checkValidity(int width, int height, int edge_thresh) { + const bool v = ( coordX() > edge_thresh && + coordX() < (width-1-edge_thresh) && + coordY() > edge_thresh && + coordY() < (height-1-edge_thresh) ); + setValid(v); +} + +void EndPoint::updateParams(const EndPoint& other){ + ++nsamples; + // cout << "conf (this,other): " << nsamples << " " << other.nsamples << endl; + // cout << "old coords: " << coordX() << " " << coordY() << endl; + // cout << "other coords: " << other.coordX() << " " << other.coordY() << endl; + coords = (coords*nsamples + other.coords*other.nsamples)/(nsamples+other.nsamples); + rendering_valid = false; + // cout << "new coords: " << coordX() << " " << coordY() << endl; +} + +void EndPoint::updateParams(const EndPoint& other, unsigned int num_updates){ + nsamples += num_updates-1; + updateParams(other); +} + +} // namespace diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/EndPoint.h ./DualCoding/EndPoint.h --- ../Tekkotsu_2.4.1/DualCoding/EndPoint.h 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/EndPoint.h 2006-02-12 00:32:01.000000000 -0500 @@ -0,0 +1,72 @@ +//-*-c++-*- +#ifndef _ENDPOINT_H_ +#define _ENDPOINT_H_ + +#include "Measures.h" +#include "Point.h" + +namespace DualCoding { + +class EndPoint : public Point { +private: + //! @a valid indicates whether or not this is known to be the final end-point of the line. + /*! If false, this was the point at which the line went off-camera, not the true end point. */ + bool valid; + + //! @a active indicates whether or not the line is to terminate at this point. + /*! If false, the line will continue infinitely as a ray beyond this point. */ + bool active; + + //! @a rendering_valid is set when the line is rendered and cleared if an endpoint is modified + bool rendering_valid; + + //! How many measurements have gone into the estimate of this point. + int nsamples; + +public: + friend class LineData; + friend class BoundaryDetector; + friend class PolygonData; + + EndPoint() + : Point(), valid(true), active(true), rendering_valid(false), nsamples(1) {}; + + EndPoint(coordinate_t const &xp, coordinate_t const &yp) + : Point(xp,yp), valid(true), active(true), rendering_valid(false), nsamples(1) {}; + + EndPoint(const Point& otherPt) + : Point(otherPt), valid(true), active(true), rendering_valid(false), nsamples(1) {}; + + bool isValid() const { return valid; } + void setValid(bool _valid) { valid = _valid; } + + bool isActive() const { return active; } + void setActive(bool _active) { + if ( active != _active ) rendering_valid = false; + active = _active; + } + + bool isMatchFor(const EndPoint& other, float dist_thresh) const {return distanceFrom(other) <= dist_thresh; } + bool operator==(const EndPoint& other) const { return isMatchFor(other,5); } + + + //! Checks if endpoint comes to close to edge of camera frame and if so, + //! marks it as invalid. + void checkValidity(int width, int height, int edge_thresh); + + void updateParams(const EndPoint& other); + void updateParams(const EndPoint& other, unsigned int num_updates); + +private: + void clearNumUpdates(void) { nsamples=0; }; + void setNumUpdates(long _nsamples) { nsamples = _nsamples; }; + +public: + void incrementNumUpdates(void) { nsamples++; }; + void decrementNumUpdates(void) { nsamples--; }; + int numUpdates(void) const { return nsamples; }; +}; + +} // namespace + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/LineData.cc ./DualCoding/LineData.cc --- ../Tekkotsu_2.4.1/DualCoding/LineData.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/LineData.cc 2006-10-03 19:24:05.000000000 -0400 @@ -0,0 +1,1494 @@ +//-*-c++-*- +#include +#include +#include +#include + +#include "Macrodefs.h" + +#include "SketchSpace.h" +#include "Sketch.h" +#include "Region.h" +#include "visops.h" + +#include "ShapeSpace.h" +#include "ShapeRoot.h" + +#include "PointData.h" +#include "LineData.h" +#include "ShapeLine.h" + +namespace DualCoding { + +DATASTUFF_CC(LineData); + +const Point LineData::origin_pt = Point(0,0); + +LineData::LineData(ShapeSpace& _space, const Point &p1, orientation_t orient) + : BaseData(_space,getStaticType()), end1_pt(p1), end2_pt(), + rho_norm(0), theta_norm(0), orientation(0), length(0) { + int const width = space->getDualSpace().getWidth(); + int const height = space->getDualSpace().getHeight(); + // Use a large offset from p1 to p2 because SketchGUI must calculate + // the line slope from p1/p2 coords transmitted as strings; we don't + // transmit the orientation. But don't use an offset so large that the line + // goes off-screen. + float p2x=0, p2y=0; + if ( fabs(orient-M_PI/2) < 0.001 ) { + p2x = p1.coordX(); + p2y = p1.coordY() > height/2 ? 0 : height-1; + } else { + float slope = tan(orient); + float intcpt = p1.coordY() - p1.coordX()*slope; + p2x = p1.coordX() >= width/2 ? 0.0 : width-1; + p2y = p2x * slope + intcpt; + if ( p2y > height-1 ) { + p2x = (height - intcpt) / slope; + p2y = height; + } else if ( p2y < 0 ) { + p2x = -intcpt / slope; + p2y = 0; + } + } + end2_pt = Point(p2x,p2y); + end1_pt.setValid(false); + end1_pt.setActive(false); + end2_pt.setValid(false); + end2_pt.setActive(false); + update_derived_properties(); +} + +Point LineData::getCentroid() const { return (end1Pt()+end2Pt())*0.5; } + +void LineData::setInfinite(bool value) { + end1_pt.setActive(!value); + end2_pt.setActive(!value); +} + +#define NORMPOINT_MATCH_DISTSQ 3500 +#define LINE_MATCH_OVERLAP -10 +#define LINE_ORIENTATION_MATCH M_PI/6 + +bool LineData::isMatchFor(const ShapeRoot& other) const { + if (!(isSameTypeAs(other) && isSameColorAs(other))) + return false; + else { + const Shape& other_ln = ShapeRootTypeConst(other,LineData); + return isMatchFor(other_ln.getData()); + } +} + +bool LineData::isMatchFor(const LineData& other_line) const { + float const px = rho_norm*cos(theta_norm), + py = rho_norm*sin(theta_norm); + float const qx = other_line.rho_norm*cos(other_line.theta_norm), + qy = other_line.rho_norm*sin(other_line.theta_norm); + AngPi theta_diff = float(theta_norm) - float(other_line.theta_norm); + if ( theta_diff > M_PI/2 ) + theta_diff = M_PI - theta_diff; + float normpointdistsq = (px-qx)*(px-qx) + (py-qy)*(py-qy); + // cout << "px=" << px << " py=" << py << " qx=" << qx << " qy=" << qy << " normpointdistsq=" << normpointdistsq + // << " theta_diff=" << theta_diff + // << " perpdist=" << perpendicularDistanceFrom(other_line.getCentroid()) + // << " isoverlapped=" << isOverlappedWith(other_line,LINE_MATCH_OVERLAP) << endl; + return normpointdistsq < NORMPOINT_MATCH_DISTSQ // *** DST hack + && theta_diff < LINE_ORIENTATION_MATCH + && perpendicularDistanceFrom(other_line.getCentroid()) < 100 + && isOverlappedWith(other_line,LINE_MATCH_OVERLAP); +} + + bool LineData::isOverlappedWith(const LineData& otherline, int amount) const { + if ( firstPtCoord() <= otherline.firstPtCoord() ) + return secondPtCoord()-amount >= otherline.firstPtCoord(); + else + return firstPtCoord()+amount <= otherline.secondPtCoord(); +} + + +void LineData::mergeWith(const ShapeRoot& other) { + const Shape& other_line = ShapeRootTypeConst(other,LineData); + if (other_line->confidence <= 0) + return; + const int other_conf = other_line->confidence; + confidence += other_conf; + end1_pt = (end1_pt*confidence + other_line->end1Pt()*other_conf) / (confidence+other_conf); + end2_pt = (end2_pt*confidence + other_line->end2Pt()*other_conf) / (confidence+other_conf); + update_derived_properties(); +} + + +bool LineData::isValidUpdate(coordinate_t c1_cur, coordinate_t c2_cur, coordinate_t c1_new, coordinate_t c2_new) { + const float c1_noise = 10.0 + fabs(c1_cur+c1_new) / 20.0; // allow larger error for shapes far from the robot + const float c2_noise = 10.0 + fabs(c2_cur+c2_new) / 20.0; + return (c1_new-c1_noise < c1_cur && c2_cur < c2_new+c2_noise); +} + + +//! Update a line in the local map with info from a matching line in the ground space. +bool LineData::updateParams(const ShapeRoot& ground_root, bool force) { + const Shape& ground_line = ShapeRootTypeConst(ground_root,LineData); + // cout << "updating local Line " << getId() << " with data from ground line " << ground_line->getId() << ":" << endl; + // ground_line->printEnds(); + // cout << "Update from " << endl; + // printEnds(); + + const coordinate_t c1_cur = firstPtCoord(); + const coordinate_t c2_cur = secondPtCoord(); + + Point _end1_pt = firstPt(); + Point _end2_pt = secondPt(); + + updateLinePt(firstPt(), firstPtCoord(), firstPt(ground_line), firstPtCoord(ground_line), -1); + updateLinePt(secondPt(), secondPtCoord(), secondPt(ground_line), secondPtCoord(ground_line), +1); + // cout << "to" << endl; + // printEnds(); + + const coordinate_t c1_new = firstPtCoord(); + const coordinate_t c2_new = secondPtCoord(); + + if (isValidUpdate(c1_cur, c2_cur, c1_new, c2_new) || force){ + // cout << "was accepted, line updated" << endl; + // ++nsamples; + update_derived_properties(); + return true; + } + + // cout << "was denied, line not updated" << endl; + setEndPts(_end1_pt, _end2_pt); + return false; +} + +void LineData::updateLinePt(EndPoint& localPt, coordinate_t local_coord, + const EndPoint& groundPt, coordinate_t ground_coord, + int sign) { + if ( groundPt.isValid() ) { + if ( localPt.isValid() ) + localPt.updateParams(groundPt); + else + localPt = groundPt; + } + else if ( (ground_coord - local_coord)*sign > 0 ) + localPt = groundPt; +} + +bool LineData::isAdmissible() const { + if (end1Pt().isValid() && end2Pt().isValid()) + return length >= 70.0; // *** DST hack: lines should be at least 70 mm long + else + return length >= 40.0; +} + +//! 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 << "end2{" << end2Pt().coordX() << ", " << end2Pt().coordY() << "}"; + cout << " active=" << end2Pt().isActive(); + cout << " valid=" << end2Pt().isValid() << std::endl; + + cout << "rho_norm=" << rho_norm << " theta_norm=" << theta_norm; + cout << " orientation=" << getOrientation() + << " length=" << getLength() << endl; + + printf("color = %d %d %d\n",getColor().red,getColor().green,getColor().blue); + + cout << " mobile=" << isMobile() << endl; + cout << " viewable=" << isViewable() << endl; + + vector abc = lineEquation_abc(); + printf("equ = %f %f %f\n",abc[0],abc[1],abc[2]); +} + +void LineData::printEnds() const { + cout << " end1{" << end1Pt().coordX() << ", " << end1Pt().coordY() << "}"; + cout << " active=" << end1Pt().isActive() << ", valid=" << end1Pt().isValid() << endl; + cout << " end2{" << end2Pt().coordX() << ", " << end2Pt().coordY() << "}"; + cout << " active=" << end2Pt().isActive() << ", valid=" << end2Pt().isValid() << endl; + // cout << endl; +} + + + +//! Transformations. +//@{ +//! Apply a transformation to this shape. +void LineData::applyTransform(const NEWMAT::Matrix& Tmat) { + end1Pt().applyTransform(Tmat); + end2Pt().applyTransform(Tmat); + update_derived_properties(); +} + +void LineData::projectToGround(const NEWMAT::Matrix& camToBase, + const NEWMAT::ColumnVector& groundplane) { + end1Pt().projectToGround(camToBase,groundplane); + end2Pt().projectToGround(camToBase,groundplane); + update_derived_properties(); +} + +//! Logical endpoints +//@{ + +EndPoint& LineData::leftPt() { return end1Pt().isLeftOf(end2Pt()) ? end1_pt : end2_pt; } +EndPoint& LineData::rightPt() { return end1Pt().isLeftOf(end2Pt()) ? end2_pt : end1_pt; } +EndPoint& LineData::topPt() { return end1Pt().isAbove(end2Pt()) ? end1_pt : end2_pt; } +EndPoint& LineData::bottomPt() { return end1Pt().isAbove(end2Pt()) ? end2_pt : end1_pt; } + +Shape LineData::leftPtShape() { + Shape result(new PointData(*space, leftPt())); + result->setName("leftPt"); + result->inheritFrom(*this); + result->setViewable(false); + return result; +} + +Shape LineData::rightPtShape() { + Shape result(new PointData(*space, leftPt())); + result->setName("rightPt"); + result->inheritFrom(*this); + result->setViewable(false); + return result; +} + +Shape LineData::topPtShape() { + Shape result(new PointData(*space, leftPt())); + result->setName("topPt"); + result->inheritFrom(*this); + result->setViewable(false); + return result; +} + +Shape LineData::bottomPtShape() { + Shape result(new PointData(*space, leftPt())); + result->setName("bottomPt"); + result->inheritFrom(*this); + result->setViewable(false); + return result; +} + +EndPoint& LineData::firstPt() { + if ( isNotVertical() ) + if ( end1Pt().coordX() < end2Pt().coordX() ) + return end1Pt(); + else return end2Pt(); + else + if ( end1Pt().coordY() < end2Pt().coordY() ) + return end1Pt(); + else return end2Pt(); +} + +EndPoint& LineData::firstPt(const Shape &otherline) const { + if ( isNotVertical() ) + if ( otherline->end1Pt().coordX() < otherline->end2Pt().coordX() ) + return otherline->end1Pt(); + else return otherline->end2Pt(); + else + if ( otherline->end1Pt().coordY() < otherline->end2Pt().coordY() ) + return otherline->end1Pt(); + else return otherline->end2Pt(); +} + +EndPoint& LineData::secondPt() { + if ( isNotVertical() ) + if ( end1Pt().coordX() > end2Pt().coordX() ) + return end1Pt(); + else return end2Pt(); + else + if ( end1Pt().coordY() > end2Pt().coordY() ) + return end1Pt(); + else return end2Pt(); +} + +EndPoint& LineData::secondPt(const Shape &otherline) const { + if ( isNotVertical() ) + if ( otherline->end1Pt().coordX() > otherline->end2Pt().coordX() ) + return otherline->end1Pt(); + else return otherline->end2Pt(); + else + if ( otherline->end1Pt().coordY() > otherline->end2Pt().coordY() ) + return otherline->end1Pt(); + else return otherline->end2Pt(); +} + +coordinate_t LineData::firstPtCoord() const { + return isNotVertical() ? + const_cast(this)->firstPt().coordX() : + const_cast(this)->firstPt().coordY(); +} + +coordinate_t LineData::firstPtCoord(const Shape &otherline) const { + return isNotVertical() ? + firstPt(otherline).coordX() : + firstPt(otherline).coordY(); +} + +coordinate_t LineData::secondPtCoord() const { + return isNotVertical() ? + const_cast(this)->secondPt().coordX() : + const_cast(this)->secondPt().coordY(); +} + +coordinate_t LineData::secondPtCoord(const Shape &otherline) const { + return isNotVertical() ? + secondPt(otherline).coordX() : + secondPt(otherline).coordY(); +} + +//@} + +//! Functions to set endpoints. + +void LineData::setEndPts(const EndPoint& _end1_pt, const EndPoint& _end2_pt) { + end1_pt.setCoords(_end1_pt); + end1_pt.setActive(_end1_pt.isActive()); + end1_pt.setValid(_end1_pt.isValid()); + end1_pt.setNumUpdates(_end1_pt.numUpdates()); + + end2_pt.setCoords(_end2_pt); + end2_pt.setActive(_end2_pt.isActive()); + end2_pt.setValid(_end2_pt.isValid()); + end2_pt.setNumUpdates(_end2_pt.numUpdates()); + + update_derived_properties(); +} + + +//! Properties functions. +//@{ + +std::pair LineData::lineEquation_mb() const { + float m; + if ((fabs(end2Pt().coordX() - end1Pt().coordX()) * BIG_SLOPE) + <= fabs(end2Pt().coordY() - end2Pt().coordY())) + m = BIG_SLOPE; + else + m = (end2Pt().coordY() - end1Pt().coordY())/(end2Pt().coordX() - end1Pt().coordX()); + float b = end1Pt().coordY() - m * end1Pt().coordX(); + return pair(m,b); +} + + +//! Determine parameters a, b, c, d satisfying the equation ax + bz = c. +std::vector LineData::lineEquation_abc_xz() const { + float dx = end2Pt().coordX() - end1Pt().coordX(); + float dz = end2Pt().coordZ() - end1Pt().coordZ(); + + std::vector abc; + abc.resize(3, 1.0); + float& a = abc[0]; + float& b = abc[1]; + float& c = abc[2]; + + // If vertical...b = 0 + if((dx == 0.0) + || (dz/dx > BIG_SLOPE)) { + a = 1.0; + b = 0.0; + c = end1Pt().coordX(); + } + + // If horizontal...a = 0 + else if((dz == 0.0) + || (dx/dz > BIG_SLOPE)) { + a = 0.0; + b = 1.0; + c = end1Pt().coordZ(); + } + + // If not horizontal or vertical...a = 1.0 + else { + a = 1.0; + b = (end1Pt().coordX() - end2Pt().coordX()) + / (end2Pt().coordZ() - end1Pt().coordZ()); + c = end1Pt().coordX() + b*end1Pt().coordZ(); + } + + return(abc); + +} +//@} + + //! 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(); + + std::vector abc; + abc.resize(3, 1.0); + float& a = abc[0]; + float& b = abc[1]; + float& c = abc[2]; + + // If vertical...b = 0 + if( fabs(dx) < 1.0e-6 || dy/dx > BIG_SLOPE) { + a = 1.0; + b = 0.0; + c = end1Pt().coordX(); + } + + // If horizontal...a = 0 + else if ( fabs(dy) < 1.0e-6 || dx/dy > BIG_SLOPE ) { + a = 0.0; + b = 1.0; + c = end1Pt().coordY(); + } + + // If not horizontal or vertical...a = 1.0 + else { + a = 1.0; + // b = (end1Pt().coordX() - end2Pt().coordX()) + // / (end2Pt().coordY() - end1Pt().coordY()); + b = -dx / dy; + c = end1Pt().coordX() + b*end1Pt().coordY(); + } + + return(abc); +} +//@} + + +//! Functions to set values dealing with orientation. +//@{ +void LineData::update_derived_properties() { + rho_norm = perpendicularDistanceFrom(origin_pt); + const vector abc = lineEquation_abc(); + const float& a1 = abc[0]; + const float& b1 = abc[1]; + const float& c1 = abc[2]; + const float c1sign = (c1 >= 0) ? 1.0 : -1.0; + theta_norm = atan2(b1*c1sign, a1*c1sign); + orientation = theta_norm + AngPi(M_PI/2); + length = end1Pt().distanceFrom(end2Pt()); + const ReferenceFrameType_t ref = getRefFrameType(); + end1_pt.setRefFrameType(ref); + end2_pt.setRefFrameType(ref); + deleteRendering(); +} + +bool LineData::isNotVertical() const { + const AngPi threshold = M_PI / 3; + const AngPi orient = getOrientation(); + return (orient <= threshold) || (orient >= M_PI - threshold); +} + +/* +bool LineData::isRoughlyPerpendicularTo(Shape& other) { + AngPi threshold = M_PI_4; + AngPi orientation_diff = getOrientation() - other->getOrientation(); + if((orientation_diff >= threshold) && (orientation_diff < (M_PI-threshold))) + return true; + else + return false; +} + +bool LineData::isExactlyPerpendicularTo(Shape& other) { + AngPi orientation_diff = getOrientation() - other->getOrientation(); + return (orientation_diff == M_PI_2); +} + +*/ + + +//! These functions are true based on line length. +//@{ +bool LineData::isLongerThan(const Shape& other) const { + return length > other->length; } + +bool LineData::isLongerThan(float ref_length) const { + return length > ref_length; } + +bool LineData::isShorterThan(const Shape& other) const { + return length < other->length; } + +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 + float dl = perpendicularDistanceFrom(other.end1Pt()); // distance between the lines + return (perpendicularDistanceFrom(p) <= dl && other.perpendicularDistanceFrom(p) <= dl); + } + else { + bool b; + const LineData p_line (*space, p, // line from intersection of this and other to p + intersectionWithLine(other, b, b)); + const AngPi theta_pline = p_line.getOrientation(); + const AngPi theta_greater = + (getOrientation() > other.getOrientation()) ? getOrientation() : other.getOrientation(); + const AngPi theta_smaller = + (getOrientation() < other.getOrientation()) ? getOrientation() : other.getOrientation(); + if (theta_greater - theta_smaller > M_PI/2) + return (theta_pline >= theta_greater || theta_pline <= theta_smaller); + else + return (theta_pline <= theta_greater && theta_pline >= theta_smaller); + } +} + +//! Check intersection. +//{ +bool +LineData::intersectsLine(const Shape& other) const { + return intersectsLine(other.getData()); +} + +bool +LineData::intersectsLine(const LineData& other) const { + // Calculate F(x,y) = 0 for this line (x1,y1) to (x2,y2). + pair F = lineEquation_mb(); + + // Calculate G(x,y) = 0 for L (x3,y3) to (x4,y4). + pair G = other.lineEquation_mb(); + + // NOTE: These tests are assumed to be taking place in the space of + // "this" line. Therefore, the limits of line extent (for lines + // with inactive endpoints) are calculated in the space of "this" + // line. + + // JJW *** YOU NEED TO TAKE ACCOUNT OF END POINTS BEING TURNED OFF. + + // float end1x, end1y, end2x, end2y, other_end1x, other_end1y, other_end2x, other_end2y; + // if(end1Pt().isActive()) { + // end1x = end1Pt().coordX(); + // end1y = end1Pt().coordY(); + // } else { + // end1x = + + + // TEST 1 + + // Calculate r3 = F(x3,y3) + float r3 = F.first * other.end1Pt().coordX() + F.second - other.end1Pt().coordY(); + + // Calculate r4 = F(x4,y4) + float r4 = F.first * other.end2Pt().coordX() + F.second - other.end2Pt().coordY(); + + // If r3 != 0... + // ...AND r4 != 0... + // ...AND SGN(r3) == SGN(r4)... + // ...THEN the lines do not intersect. + + if((r3 != 0) + && (r4 != 0) + && (SGN(r3) == SGN(r4)) + ) + return false; + + + // TEST 2 + + // Calculate r1 = G(x1,y1) + float r1 = G.first * end1Pt().coordX() + G.second - end1Pt().coordY(); + + // Calculate r2 = G(x2,y2) + float r2 = G.first * end2Pt().coordX() + G.second - end2Pt().coordY(); + + // If r1 != 0... + // ...AND r2 != 0... + // ...AND SGN(r1) == SGN(r2)... + // ...THEN the lines do not intersect. + + if((r1 != 0) + && (r2 != 0) + && (SGN(r1) == SGN(r2)) + ) + return false; + + // Otherwise, the lines DO intersect. + return true; +} + + +Point LineData::intersectionWithLine(const Shape& other, + bool& intersection_on_this, + bool& intersection_on_other) const { + return intersectionWithLine(other.getData(), intersection_on_this,intersection_on_other); +} + +Point +LineData::intersectionWithLine(const LineData& other, + bool& intersection_on_this, bool& intersection_on_other) const { + + // Based upon algorithm written by Paul Bourke, April 1989. + // http://astronomy.swin.edu/~pbourke/geometry/lineline2d/ + // Accessed July 20th 2004 + + // Points 1 and 2 are on "this" line. Points 3 and 4 define the "other" line. + // float x1, x2, x3, x4, y1, y2, y3, y4; + float x1, x2, x3, x4, y1, y2, y3, y4; + x1 = end1Pt().coordX(); + x2 = end2Pt().coordX(); + x3 = other.end1Pt().coordX(); + x4 = other.end2Pt().coordX(); + // x3 = other->end1Pt().coordX(); + // x4 = other->end2Pt().coordX(); + y1 = end1Pt().coordY(); + y2 = end2Pt().coordY(); + y3 = other.end1Pt().coordY(); + y4 = other.end2Pt().coordY(); + // y3 = other->end1Pt().coordY(); + // y4 = other->end2Pt().coordY(); + + // x1 + u_a(x2-x1) = x3 + u_b(x4-x3) + // y1 + u_a(y2-y1) = y3 + u_b(y4-y3) + + // u_a = (x4-x3)(y1-y3) - (y4-y3)(x1-x3) + // ------------------------------- + // (y4-y3)(x2-x1) - (x4-x3)(y2-y1) + + // u_b = (x2-x1)(y1-y3) - (y2-y1)(x1-x3) + // ------------------------------- + // (y4-y3)(x2-x1) - (x4-x3)(y2-y1) + + float denom = ((y4-y3)*(x2-x1) - (x4-x3)*(y2-y1)); + float u_a_numerator = ((x4-x3)*(y1-y3) - (y4-y3)*(x1-x3)); + float u_b_numerator = ((x2-x1)*(y1-y3) - (y2-y1)*(x1-x3)); + + // If denominators of u_a and u_b are zero, then lines are parallel. + if(denom == 0.0) { + if (u_a_numerator == 0.0 && u_b_numerator == 0.0) { + PRINTF("intersectionWithLine: lines are coincident!\n"); + return(end1Pt()); + } + else { + cout << x1 << " " << x2 << " " << x3 << " " << x4 << " " + << y1 << " " << y2 << " " << y3 << " " << y4 << endl; + cout << "this theta; " << getOrientation() << ", other theta: " << other.getOrientation() << endl; + PRINTF("ERROR in intersectionWithLine: lines are parallel!\n"); + return(Point(-9999.0,-99999.0)); + } + } + + else { + float u_a = u_a_numerator / denom; + float u_b = u_b_numerator / denom; + + // If 0 <= u_a <=1 then intersection point is on segment a. + if(0.0 <= u_a <=1.0) + intersection_on_this = true; + else + intersection_on_this = false; + + // If 0 <= u_b <=1 then intersection point is on segment b. + if(0.0 <= u_b <=1.0) + intersection_on_other = true; + else + intersection_on_other = false; + + return(Point((x1+u_a*(x2-x1)), + (y1+u_a*(y2-y1)))); + } +} + +//! Distance. +//{ + +float LineData::perpendicularDistanceFrom(const Point& otherPt) const { + // NOTE that this formula is rather slow, as it involves the + // calculation of the line equation in addition to a square root here. + + // Using the formula from http://www.tpub.com/math2/8.htm + vector abc = lineEquation_abc(); + float& a = abc[0]; + float& b = abc[1]; + float& c = abc[2]; + + // Distance... + // (x*A + y*B - C ) + // abs(------------- ) + // (sqrt(a^2 + b^2)) + float d = fabs((a * otherPt.coordX() + b * otherPt.coordY() - c)/sqrt(a*a + b*b)); + // cout << "abcd: " << a << ", " << b << ", " << c << ", " << d << endl; + return(d); +} + +//} + + +// ================================================== +// BEGIN LINE EXTRACTION CODE +// ================================================== + +//!@name Line Extraction. +//@{ + + +#define BEG_DIST_THRESH 2 +#define END_DIST_THRESH 2 + +Shape LineData::extractLine(Sketch& sketch) { + NEW_SKETCH_N(fatmask,bool,visops::fillin(sketch,1,2,8)); + NEW_SKETCH_N(skel,bool,visops::skel(fatmask)); + return extractLine(skel, sketch); +} + +Shape LineData::extractLine(Sketch& skeleton, + const Sketch& occlusions) { + const int width = skeleton->getWidth(), height = skeleton->getHeight(); + SketchSpace &SkS = skeleton->getSpace(); + ShapeSpace &ShS = SkS.getDualSpace(); + + // approximate largest skel region with line + NEW_SKETCH_N(labels,usint,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 + return ShapeRootType(invalid,LineData); + } + Region skelchunk = regions.front(); // regions not empty, so use the largest.... + AngPi orientation(skelchunk.findPrincipalAxisOrientation()); + Point centroid(skelchunk.findCentroid()); + // cout << " orientation=" << orientation << " centroid=" << centroid.getCoords() << endl; + if (! skelchunk.isContained(centroid, 3) ) { // region does not look like a straight line + Shape result(splitLine(ShS, skelchunk, skeleton, occlusions)); + if ( result.isValid() ) + return result; + } + + // Create a symbolic line object. + Shape extracted_line(ShS, centroid, orientation); + extracted_line->setColor(skeleton->getColor()); + extracted_line->setParentId(skeleton->getViewableId()); + + // rho and theta describe the normal line. + // normpoint is where the normal line intersects with this line. + // normpoint may actually be off-image. + float x_normpoint = extracted_line->rho_norm*cos(extracted_line->theta_norm); + float y_normpoint = extracted_line->rho_norm*sin(extracted_line->theta_norm); + + // m is slope of this line (not the normal line) + // float m = (y_normpoint != 0) ? -(x_normpoint/y_normpoint) : BIG_SLOPE; + float m = max(min(tan(extracted_line->theta_norm+AngPi(M_PI/2)), (float)BIG_SLOPE), (float)(-BIG_SLOPE)); + float b = y_normpoint - m*x_normpoint; + // cout << " x_normpoint=" << x_normpoint << " y_normpoint=" << y_normpoint + // << " m=" << m << " b=" << b <scanHorizForEndPts(skelDist,occlusions,m,b); + else + extracted_line->scanVertForEndPts(skelDist,occlusions,m,b); + + // Check to see if any endpoints are near any edge of the screen. + // If they are, invalidate them, assuming that line continues beyond screen. + extracted_line->end1_pt.checkValidity(width,height,BEG_DIST_THRESH); + extracted_line->end2_pt.checkValidity(width,height,BEG_DIST_THRESH); + + // Transform from SketchSpace coordinates to ShapeSpace coordinates + SkS.applyTmatinv(extracted_line->end1_pt.getCoords()); + SkS.applyTmatinv(extracted_line->end2_pt.getCoords()); + extracted_line->update_derived_properties(); + return extracted_line; +} + + + // hack to split a non-straight region by kei +Shape LineData::splitLine(ShapeSpace &ShS, Region &skelchunk, + Sketch &skeleton, const Sketch &occlusions) { + // cout << "this region is not straight, needs be split into two regions" << endl; + Point bounds[4] = {skelchunk.findTopBoundPoint(), skelchunk.findLeftBoundPoint(), + skelchunk.findRightBoundPoint(), skelchunk.findBotBoundPoint()}; + // cout << "top(0): " << bounds[0] << endl; + // cout << "left(1): " << bounds[1] << endl; + // cout << "right(2): " << bounds[2] << endl; + // cout << "bottom(3): " << bounds[3] << endl; + for (int i = 0; i < 4; i++) { + for (int j = i+1; j < 4; j++) { + if (bounds[i].distanceFrom(bounds[j]) > 20 && ! skelchunk.isContained((bounds[i]+bounds[j])/2.0, 3)) { + // cout << "[" << i << "," << j << "] form a line from which most distant point is where the region should split " << endl; + LineData ln(ShS,bounds[i],bounds[j]); + 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; + NEW_SKETCH_N(not_too_close, bool, visops::edist(point_rendering) >= clear_dist); + skeleton &= not_too_close; + return extractLine(skeleton, occlusions); + } + } + } + ShapeRoot invalid; // need to define a named variable to avoid warning on next line + return ShapeRootType(invalid,LineData); +} + +void LineData::scanHorizForEndPts(const Sketch& skelDist, + const Sketch& occlusions, + float m, float b) { + // Scan along the infinite line, looking for segments in the image. + bool on_line = false; // true if tracing a line skeleton + int beg_dist_thresh = BEG_DIST_THRESH; // skel has to be <= this close to start segment + int end_dist_thresh = END_DIST_THRESH; // how far line should extend from skel + int curxstart = -1; // start of current segment we're examining + int possxstop = -1; // end of current segment unless we bridge a gap + int bestxstart = -1; // start of best (longest) segment seen so far + int bestxstop = -1; // end of best segment seen so far + int bestlength = -1; // length of best segment seen so far + for (int x = 0, y, dist=0; x < skelDist.width; x++) { + y = int(m*x+b); + + // If y is on-screen... + if (y >= 0 && y < skelDist.height) { + dist = skelDist(x,y); + + if (on_line == false) { // not currently on a line segment + if (dist <= beg_dist_thresh) { + // start a new segment + on_line = true; + curxstart = x - dist; + // if new segment begins at an occluder, back up over the occluder + int curystart; + bool backupflag = false; + while ( curxstart >= 0 && (curystart=int(m*curxstart+b)) >= 0 && curystart < skelDist.height ) + if ( occlusions(curxstart,curystart) || skelDist(curxstart,curystart) == 0 ) { + --curxstart; + backupflag = true; + } else { // if we backed up past the occluder, go one step forward + curxstart += backupflag; + break; + } + if ( curxstart < 0) // occluder extended to left edge of image + curxstart = 0; + } + } + else { // on_line == true: currently on a line segment + const bool occ = occlusions(x,y); + // cout << "x=" << x << " dist=" << dist <<" occ=" << occ; + if ( dist <= end_dist_thresh || occ ) { + if ( occ ) + possxstop = x; + else + possxstop = x - dist; + // cout << " possxstop=" << possxstop; + if ( possxstop-curxstart > bestlength ) { + bestxstart = curxstart; + bestxstop = possxstop; + bestlength = bestxstop-bestxstart; + // cout << " bestxstop=" << bestxstop << " bestlength=" << bestlength; + } + } + else if ( dist > extractorGapTolerance ) { + // we're traversing a gap, and it just got too long + on_line = false; + // cout << " on_line=" << on_line << " dist=" << dist; + }; + // cout << endl; + } + } + } + + // cout << "xstart:" << bestxstart << "xstop:" << bestxstop << endl; + float y1 = m*bestxstart + b; + float y2 = m*bestxstop + b; + setEndPts(Point(bestxstart,y1), Point(bestxstop,y2)); + // cout << "m = " << m << " b = " << b << endl; + balanceEndPointHoriz(end1_pt,occlusions,m,b); + balanceEndPointHoriz(end2_pt,occlusions,m,b); +} + +void LineData::scanVertForEndPts(const Sketch& skelDist, + const Sketch& occlusions, + float m, float b) { + // Scan along the infinite line, looking for segments in the image. + bool on_line = false; // true if tracing a line skeleton + int beg_dist_thresh = BEG_DIST_THRESH; // skel has to be <= this close to start segment + int end_dist_thresh = END_DIST_THRESH; // how far line should extend from skel + int curystart = -1; // start of current segment we're examining + int possystop = -1; // end of current segment unless we bridge a gap + int bestystart = -1; // start of best (longest) segment seen so far + int bestystop = -1; // end of best segment seen so far + int bestlength = -1; // length of best segment seen so far + for (int x, y = 0, dist=0; y < skelDist.height; y++) { + x = int((y-b)/m); + + // If x is on-screen... + if (x >= 0 && x < skelDist.width) { + dist = int(skelDist(x,y)); + + if (on_line == false) { // not currently on a line segment + if (dist <= beg_dist_thresh) { + // start a new segment + on_line = true; + curystart = y - dist; + // if new segment begins at an occluder, back up over the occluder + int curxstart; + bool backupflag = false; + while ( curystart >= 0 && (curxstart=int((curystart-b)/m)) >= 0 && curxstart < skelDist.width ) + if ( occlusions(curxstart,curystart) || skelDist(curxstart,curystart) == 0 ){ + --curystart; + backupflag = true; + } else { // if we backed up past the occluder, go one step forward + curystart += backupflag; + break; + } + if ( curystart < 0) // occluder extended to top edge of image + curystart = 0; + } + } + else { // on_line == true: currently on a line segment + const bool occ = occlusions(x,y); + // cout << "y=" << y << " dist=" << dist <<" occ=" << occ; + if ( dist <= end_dist_thresh || occ ) { + if ( occ ) + possystop = y; + else + possystop = y - dist; + // cout << " possystop=" << possystop; + if ( possystop-curystart > bestlength ) { + bestystart = curystart; + bestystop = possystop; + bestlength = bestystop-bestystart; + // cout << " bestystop=" << bestystop << " bestlength=" << bestlength; + } + } + else if ( dist > extractorGapTolerance ) { + // we're traversing a gap, and it just got too long + on_line = false; + // cout << " on_line=" << on_line; + }; + // cout << endl; + } + } + } + + float x1 = (bestystart-b)/m; + float x2 = (bestystop-b)/m; + // cout << "x1=" << x1 << ", ystart=" << bestystart << " x2=" << x2 << ", ystop=" << bestystop << endl; + setEndPts(Point(x1,bestystart), Point(x2,bestystop)); + // cout << "m = " << m << " b = " << b << endl; + balanceEndPointVert(end1_pt,occlusions,m,b); + balanceEndPointVert(end2_pt,occlusions,m,b); +} + +void LineData::balanceEndPointHoriz(EndPoint &, Sketch const &, float, float) { + /* + int xstep = ( pt.coordX() < max(end1_pt.coordX(),end2_pt.coordX()) ? +1 : -1 ); + int toppix = 0, bottompix = 0; + int const balance_rows = 5; // check this many rows from the line endpoint inward + int const row_samples = 10; // for each row, check this many pixels each side of the line + */ + return; +} + +void LineData::balanceEndPointVert(EndPoint &pt, Sketch const &occluders, float m, float b) { + int ystep = ( pt.coordY() < max(end1_pt.coordY(),end2_pt.coordY()) ? +1 : -1 ); + int leftpix = 0, rightpix = 0; + int const balance_rows = 8; // check this many rows from the line endpoint inward + int const row_samples = 10; // for each row, check this many pixels each side of the line + // cout << endl << " ystep=" << ystep << " coords= ( " << pt.coordX() << " , " << pt.coordY() << ")" << endl; + for ( int y = (int)pt.coordY(), ycnt=balance_rows ; + y>= 0 && y 0 ; y+=ystep ) { + int const xstart = (int) ((y-b)/m); + for ( int x = xstart-row_samples; x < xstart; x++ ) + if ( x >= 0 ) + leftpix += occluders(x,y); + for ( int x = xstart+row_samples; x > xstart; x-- ) + if ( x < occluders.width ) + rightpix += occluders(x,y); + // cout << " " << xstart << "/" << y << " = " << leftpix << " " << rightpix << endl;; + } + float const new_x = pt.coordX() + (rightpix-leftpix)/(2*balance_rows); + // cout << " leftpix=" << leftpix <<" rightpix=" << rightpix << endl; + pt.setCoords(new_x, pt.coordY()); +} + +vector > LineData::extractLines(Sketch const& sketch, + const int num_lines) { + NEW_SKETCH_N(fatmask,bool,visops::fillin(sketch,1,2,8)); + NEW_SKETCH_N(skel,bool,visops::skel(fatmask)); + return extractLines(skel, sketch, num_lines); +} + +vector > LineData::extractLines(Sketch const& skel, + Sketch const& occluders, + const int num_lines) { + vector > lines_vec; + NEW_SKETCH_N(temp,bool,visops::copy(skel)) + for (int gloop = 0; gloop newline(extractLine(temp,occluders)); + if ( !newline.isValid() ) break; + newline->clearLine(temp); + if (newline->isLongerThan(DEFAULT_MIN_LENGTH)) { + lines_vec.push_back(newline); + } + } + // std::cout << "extractLines returning " << lines_vec.size() << " lines." << std::endl; + + return lines_vec; +} + +//! 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; + Sketch not_too_close = (visops::edist(line_rendering) >= clear_dist); + sketch &= not_too_close; +} + +//@} + + +// ================================================== +// BEGIN LINE RENDERING CODE +// ================================================== + +Sketch& LineData::getRendering() { + if ( ! end1Pt().rendering_valid || ! end2Pt().rendering_valid ) + deleteRendering(); + if ( rendering_sketch == NULL ) + rendering_sketch = render(); + return *rendering_sketch; +} + +//! Render line to SketchSpace and return reference. +//! This function does not link the Sketch* in the shape to the sketch returned. +Sketch* LineData::render() const { + SketchSpace &renderspace = space->getDualSpace(); + int const width = renderspace.getWidth(); + int const height = renderspace.getHeight(); + float x1,y1,x2,y2; + setDrawCoords(x1, y1, x2, y2, width, height); + Sketch* draw_result = + new Sketch(renderspace, "render("+getName()+")"); + (*draw_result)->setParentId(getViewableId()); + (*draw_result)->setColor(getColor()); + *draw_result = 0; + drawline2d(*draw_result, (int)x1, (int)y1, (int)x2, (int)y2); + const_cast(this)->end1Pt().rendering_valid = true; + const_cast(this)->end2Pt().rendering_valid = true; + return draw_result; +} + + +// This function will be called by both LineData and PolygonData renderers +void LineData::setDrawCoords(float& x1,float& y1, float& x2, float& y2, + const int width, const int height) const { + EndPoint e1(end1Pt()); + EndPoint e2(end2Pt()); + space->getDualSpace().applyTmat(e1.getCoords()); + space->getDualSpace().applyTmat(e2.getCoords()); + const EndPoint &left_point = e1.coordX() <= e2.coordX() ? e1 : e2; + const EndPoint &right_point = e1.coordX() <= e2.coordX() ? e2 : e1; + + // Check if horizontal + if((int)left_point.coordY() == (int)right_point.coordY()) { + if (!left_point.isActive()) + x1 = 0; + else x1 = max(0,(int)left_point.coordX()); + + if (!right_point.isActive()) + x2 = width-1; + else x2 = min(width-1, (int)right_point.coordX()); + + y1 = (int)left_point.coordY(); + y2 = y1; + } + else // Check if vertical... + if ((int)left_point.coordX() == (int)right_point.coordX()) { + + x1 = (int)left_point.coordX(); + x2 = x1; + + const EndPoint &top_point = end1Pt().coordY() <= end2Pt().coordY() ? end1Pt() : end2Pt(); + const EndPoint &bottom_point = end1Pt().coordY() <= end2Pt().coordY() ? end2Pt() : end1Pt(); + + if(!top_point.isActive()) + y1 = 0; + else y1 = max(0,(int)top_point.coordY()); + + if (!bottom_point.isActive()) + y2 = height-1; + else y2 = min(height-1,(int)bottom_point.coordY()); + } + + else { // Neither horizontal nor vertical... + float m = (right_point.coordY()-left_point.coordY())/(right_point.coordX()-left_point.coordX()); + float b = left_point.coordY() - m*left_point.coordX(); + + // find image edge intersections + int i0x = (int)((0-b)/m); + int ihx = (int)(((height-1)-b)/m); + int i0y = (int)(m*0+b); + int iwy = (int)(m*(width-1)+b); + + // If left point is active, set starting x and y accordingly. + if(left_point.isActive()) { + x1 = (int)left_point.coordX(); + y1 = (int)left_point.coordY(); + } + + // If endpoint 1 extends past image edge... + else { + + // intersects left edge + if(i0y >= 0 && i0y < height) { + x1 = 0; + y1 = i0y; + } + + // intersects top or bottom edge + else { + + // intersects top first + if (i0x < ihx) { + x1 = i0x; + y1 = 0; + } + + // intersects bottom first + else { + x1 = ihx; + y1 = height-1; + } + } + } + + // If right point is active, set starting x and y accordingly. + if(right_point.isActive()) { + x2 = (int)right_point.coordX(); + y2 = (int)right_point.coordY(); + } + else { // endpoint extends to image edge + if(iwy >= 0 && iwy < height) { // intersects right edge + x2 = width-1; + y2 = iwy; + } + else { // intersects top or bottom edge + if (i0x > ihx) { // intersects top last + x2 = i0x; + y2 = 0; + } + else { // intersects bottom last + x2 = ihx; + y2 = height-1; + } + } + } + } +} + +void LineData::drawline2d(Sketch& canvas, int x0, int y0, int x1, int y1) { + int width = canvas->getWidth(); + int height = canvas->getHeight(); + + // code below from free Graphics Gems repository, graphicsgems.org + int d, x, y, ax, ay, sx, sy, dx, dy; + dx = x1-x0; ax = abs(dx)<<1; sx = SGN(dx); + dy = y1-y0; ay = abs(dy)<<1; sy = SGN(dy); + + x = x0; + y = y0; + if ( ax > ay ) { /* x dominant */ + d = ay-(ax>>1); + for (;;) { + if (x >= 0 && y >= 0 && x < width && y < height) + canvas(x,y) = true; + + if (x==x1) + return; + + if ( d >= 0 ) { + y += sy; + d -= ax; + } + + x += sx; + d += ay; + } + } + else { /* y dominant */ + d = ax-(ay>>1); + for (;;) { + if (x >= 0 && y >= 0 && x < width && y < height) + canvas(x,y) = true; + + if ( y == y1 ) + return; + + if ( d >= 0 ) { + x += sx; + d -= ay; + } + + y += sy; + d += ax; + } + } +} +//} + + +std::vector > LineData::houghTransform(const Sketch& fat, + const Sketch& skinny, + const Sketch& occlusions, + const size_t num_lines, int minLength) +{ + std::vector > result; + ShapeSpace& ShS = fat->getSpace().getDualSpace(); + + const int width = fat->getWidth(), height = fat->getHeight(); + const int numR = 120, numTheta = 120; + const int numRf = 40, numThetaf = 40; + int hsize = numR*numTheta, hsizef = numRf * numThetaf; + int htab[hsize], hfine[hsizef]; // Hough accumulator table + float minTheta = 0.0, maxTheta = 2*M_PI; //0.9999*M_PI; + float minR = 0.000, maxR = sqrt((float)width*width+(float)height*height); //240.0; + float thetaSpread = maxTheta - minTheta; + float rSpread = maxR - minR; + //float bestR = 0, bestTheta = 0; + + for (int i = 0; i < hsize; i++) + htab[i] = 0; // zero accumulator table + + // build accumulator table + float theta, r; + int ridx; + for (int x = 0; x < width; x++) { + for (int y = 0; y < height; y++) { + if (fat(x,y) == true) { + for (int tidx = 0; tidx < numTheta ; tidx++) { + theta = minTheta + tidx*thetaSpread/numTheta; + r = (float)x*cos(theta)+(float)y*sin(theta); + ridx = (int)((r-minR)*(float)numR/rSpread); // Is this right calc? + if (ridx >= 0 && ridx < numR) // check ridx bounds + htab[ridx+tidx*numR]++; + } + } + } + } + + + /*float lineLen = 500; + for (int i = 0; i < numR*numTheta; i++) { + if (htab[i] >= minCount) + { + const float curR = (i%numR)*rSpread/numR + minR; + const float curTheta = (i/numR)*thetaSpread/numTheta + minTheta; + const float x1 = curR*cos(curTheta), y1 = curR*sin(curTheta); + const float x2 = x1+lineLen*cos(curTheta+M_PI/2), y2 = y1+lineLen*sin(curTheta+M_PI/2); + const float x3 = x1-lineLen*cos(curTheta+M_PI/2), y3 = y1-lineLen*sin(curTheta+M_PI/2); + + Point e1(x3,y3), e2(x2,y2); + Shape line(ShS,e1,e2); + line->end1Pt().setValid(false); + line->end2Pt().setValid(false); + result.push_back(line); + } + }*/ + + // Finely explore the first n best lines in the rough table + int max_val = -1, max_i = 0; + while(result.size() < num_lines) { + + // find maximum value in accumulator table + max_val = -1; + max_i = 0; + for (int i = 0; i < numR*numTheta; i++) { + if (max_val < htab[i]) { + max_val = htab[i]; + max_i = i; + } + } + + // Give up if the lines start getting too small + if (max_val < minLength) + break; + + + float bestR = (max_i%numR)*rSpread/numR + minR; + float bestTheta = (max_i/numR)*thetaSpread/numTheta + minTheta; + + // determine parameters for the fine hough iteration + const float fthetaSpread = M_PI/40.0; + const float frSpread = maxR/40.0; + float fminTheta = bestTheta-fthetaSpread/2.0; + float fminR = bestR-frSpread/2.0; + + for (int i = 0; i < hsizef; i++) + hfine[i] = 0; // zero fine table + + // build fine table + float thetaf, rf; + int ridxf; + for (int x = 0; x < width; x++) { + for (int y = 0; y < height; y++) { + if (skinny(x,y) == true) { + for (int tidx = 0; tidx < numThetaf ; tidx++) { + thetaf = fminTheta + tidx*fthetaSpread/numThetaf; + rf = (float)x*cos(thetaf)+(float)y*sin(thetaf); + ridxf = (int)((rf-fminR)*(float)numRf/frSpread); // Is this right calc? + if (ridxf >= 0 && ridxf < numRf) // check ridx bounds + hfine[ridxf+tidx*numRf]++; + } + } + } + } + + + // Pull the best line out of the fine table + int max_val_f = -1, max_i_f = 0; + for (int i = 0; i < numRf*numThetaf; i++) { + if (max_val_f < hfine[i]) { + max_val_f = hfine[i]; + max_i_f = i; + } + } + float bestRf = (max_i_f%numRf)*frSpread/numRf + fminR; + float bestThetaf = (max_i_f/numRf)*fthetaSpread/numThetaf + fminTheta; + + // Add viable segments to the vector + // Step along the line, looking for segments. + float lineLen = 500; + // Can be simpler + const float x1 = bestRf*cos(bestThetaf), y1 = bestRf*sin(bestThetaf); + const float x2 = x1+lineLen*cos(bestThetaf+M_PI/2), y2 = y1+lineLen*sin(bestThetaf+M_PI/2); + const float x3 = x1-lineLen*cos(bestThetaf+M_PI/2), y3 = y1-lineLen*sin(bestThetaf+M_PI/2); + + Point e1(x3,y3), e2(x2,y2); + Shape line(ShS,e1,e2); + + + line->setColor(skinny->getColor()); + line->setParentId(skinny->getViewableId()); + + // m is slope of this line (not the normal line) + float m = (y1 != 0) ? -(x1/y1) : BIG_SLOPE; + float b = y1 - m*x1; + + NEW_SKETCH_N(skelDist,usint,visops::edist(skinny)); + if ( abs(m) <= 1 ) // when |slope| <= 1, scan along x, else scan along y + line->scanHorizForEndPts(skelDist,occlusions,m,b); + else + line->scanVertForEndPts(skelDist,occlusions,m,b); + + //! Check to see if any endpoints are near any edge of the screen. + //! If they are, invalidate them, assuming that line continues beyond screen. + line->end1_pt.checkValidity(width,height,BEG_DIST_THRESH); + line->end2_pt.checkValidity(width,height,BEG_DIST_THRESH); + + + // Experimental: check how much of the line actually lies along edge pixels + // only include lines that are at least 50% above edge pixels. + + float len = line->getLength(); + EndPoint start = line->end1_pt; + EndPoint dx = (line->end2_pt - start)/len; + float onCount = 0; + for (float i=0; iisLongerThan(minLength) && (onCount / len) > .5 ) + { + std::cout<<" accepted"; + result.push_back(line); + } + std::cout< l1, Shapel2) +{ + const float maxDTheta = .15, minDThetaR = 10.0; + float dTheta = l1->getOrientation() - l2->getOrientation(); + if (dTheta > M_PI_2) + dTheta = dTheta - M_PI; + if (abs(dTheta) < maxDTheta) + { + if (abs (100*dTheta + (l1->rho_norm - l2->rho_norm)) > minDThetaR) + return true; + } + return false; +} + +LineData& LineData::operator=(const LineData& other) { + if (&other == this) + return *this; + BaseData::operator=(other); + end1_pt = other.end1_pt; + end2_pt = other.end2_pt; + rho_norm = other.rho_norm; + theta_norm = other.theta_norm; + orientation = other.orientation; + length = other.length; + return *this; +} + + +// Compute if two points are on the same side of the line +bool LineData::pointsOnSameSide(const Point& p1, const Point& p2) +{ + float dx = end2_pt.coordX() - end1_pt.coordX(); + float dy = end2_pt.coordY() - end1_pt.coordY(); + + float p1val = (p1.coordY() - end1_pt.coordY())*dx - (p1.coordX() - end1_pt.coordX())*dy; + float p2val = (p2.coordY() - end1_pt.coordY())*dx - (p2.coordX() - end1_pt.coordX())*dy; + + return (p1val>0) == (p2val>0); +} + +// Checks if the distance from the line to the point is less than 1 pixel, +// and checks that the point is within the end points of the line. +bool LineData::pointOnLine(const Point& p) +{ + const float BOUNDS_EXTEND = 1.0; + float dx = end2_pt.coordX() - end1_pt.coordX(); + float dy = end2_pt.coordY() - end1_pt.coordY(); + float val = (p.coordY() - end1_pt.coordY())*dx - (p.coordX() - end1_pt.coordX())*dy; + val /= length; + bool inBounds = (p.coordX() >= (leftPt().coordX() - BOUNDS_EXTEND)) && + (p.coordX() <= (rightPt().coordX() + BOUNDS_EXTEND)) && + (p.coordY() >= (topPt().coordY() - BOUNDS_EXTEND)) && + (p.coordY() <= (bottomPt().coordY() + BOUNDS_EXTEND)); + return (val > -1) && (val < 1) && inBounds; +} + + +// Comparison predicates + +bool LineData::LengthLessThan::operator() (const Shape &line1, const Shape &line2) const { + return line1->getLength() < line2->getLength(); +} + +bool LineData::ParallelTest::operator() (const Shape &line1, const Shape &line2) const { + return angdist(line1->getOrientation(),line2->getOrientation()) <= tolerance; +} + +bool LineData::PerpendicularTest::operator() (const Shape &line1, const Shape &line2) const { + return angdist(angdist(line1->getOrientation(),line2->getOrientation()), M_PI/2) <= tolerance; +} + +bool LineData::ColinearTest::operator() (const Shape &line1, const Shape &line2) const { + return ParallelTest(ang_tol)(line1,line2) && + abs( line1->getRhoNorm() - line2->getRhoNorm() ) <= dist_tol; +} + +bool LineData::IsHorizontal::operator() (const Shape &line) { + const AngPi orient = line->getOrientation(); + return (orient <= threshold) || (orient >= M_PI - threshold); + } + +} // namespace diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/LineData.h ./DualCoding/LineData.h --- ../Tekkotsu_2.4.1/DualCoding/LineData.h 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/LineData.h 2006-09-23 23:45:20.000000000 -0400 @@ -0,0 +1,316 @@ +//-*-c++-*- +#ifndef _LINEDATA_H_ +#define _LINEDATA_H_ + +#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 "Shared/mathutils.h" // deg2rad + +namespace DualCoding { + +class Region; + +#define EXTRACT_LINE_MIN_AREA 20 +#define DEFAULT_MIN_LENGTH 4.0 + +class LineData : public BaseData { +private: + EndPoint end1_pt; + EndPoint end2_pt; + float rho_norm; + AngTwoPi theta_norm; + AngPi orientation; + float length; + + static const Point origin_pt; + + friend class Shape; // grant access to line endpoints + friend class PolygonData; + friend class BlobData; + +public: + + //! Constructor + LineData(ShapeSpace& _space, const Point &p1, const Point &p2) + : BaseData(_space,getStaticType()), + end1_pt(p1), end2_pt(p2), rho_norm(0), theta_norm(0), orientation(0), length(0) + { update_derived_properties(); } + + //! Constructor + LineData(ShapeSpace& _space, const Point &pt, orientation_t orient); + + //! Copy constructor + LineData(const LineData& other) + : BaseData(other), + end1_pt(other.end1_pt), end2_pt(other.end2_pt), + rho_norm(other.rho_norm), theta_norm(other.theta_norm), + orientation(other.orientation), length(other.length) {} + + static ShapeType_t getStaticType() { return lineDataType; } + + DATASTUFF_H(LineData); + + //! Updates norm parameters (rho and theta) + void update_derived_properties(); + + //! Centroid. (Virtual in BaseData.) + virtual Point getCentroid() const; + + //! Makes endpoints inactive if value = true + 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())); + } + + //! Match lines based on their parameters. (Virtual in BaseData.) + virtual bool isMatchFor(const ShapeRoot& other) const; + bool isMatchFor(const LineData& other) const; + + //! Lines are admissible to the local map if they're long enough to not be noise. + virtual bool isAdmissible() const ; + + virtual bool updateParams(const ShapeRoot& other, bool force=false); + static void updateLinePt(EndPoint& localPt, coordinate_t local_coord, + const EndPoint& groundPt, coordinate_t ground_coord, + int sign); + virtual void mergeWith(const ShapeRoot& other); + + LineData& operator=(const LineData&); + + //checks if update of endpoints from (p1,p2) to (p3,p4) is acceptable + bool isValidUpdate(coordinate_t p1, coordinate_t p2, coordinate_t p3, coordinate_t p4); + + //! Print information about this shape. (Virtual in BaseData.) + virtual void printParams() const; + + //! Transformations. (Virtual in BaseData.) + virtual void applyTransform(const NEWMAT::Matrix& Tmat); + + //! Project to ground + virtual void projectToGround(const NEWMAT::Matrix& camToBase, + const NEWMAT::ColumnVector& groundplane); + + virtual unsigned short getDimension() const { return 1; } + + //! Point Access functions. + //@{ + EndPoint& end1Pt() { return end1_pt; } + EndPoint& end2Pt() { return end2_pt; } + const EndPoint& end1Pt() const { return end1_pt; } + const EndPoint& end2Pt() const { return end2_pt; } + + EndPoint& leftPt(); + EndPoint& rightPt(); + EndPoint& topPt(); + EndPoint& bottomPt(); + + Shape leftPtShape(); + Shape rightPtShape(); + 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; + + 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. + //@{ + 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; + //@} + +public: + + 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); + // bool isExactlyPerpendicularTo(Shape &other); + //@} + + + //! Predicates based on line length. + //@{ + bool isLongerThan(const Shape& other) const; + bool isLongerThan(float ref_length) const; + bool isShorterThan(const Shape& other) const; + bool isShorterThan(float ref_length) const; + //@} + + //! Check if point falls between the two lines + bool isBetween(const Point &p, const LineData &other) const; + + //! Check intersection. + //@{ + bool intersectsLine(const Shape& other) const; + bool intersectsLine(const LineData& other) const; + + Point intersectionWithLine(const Shape& other) const + { bool b; return intersectionWithLine(other,b,b); }; + Point intersectionWithLine(const Shape& other, + bool& intersection_on_this, + bool& intersection_on_other) const; + Point intersectionWithLine(const LineData& other) const + { bool b; return intersectionWithLine(other, b,b); }; + Point intersectionWithLine(const LineData& other, + bool& intersection_on_this, + bool& intersection_on_other) const; + + //@} + + bool pointsOnSameSide(const Point& p1, const Point& p2); + bool pointOnLine(const Point& p); + + //! Distance. + //@{ + float perpendicularDistanceFrom(const Point& other) const; + //@} + + + // ================================================== + // BEGIN SKETCH MANIPULATION AND LINE EXTRACTION CODE + // ================================================== + + //! Extraction. + //@{ + + //! Extracts most prominent line from a skeletonized image. + static Shape extractLine(Sketch& sketch); + + //! Extracts most prominent line from a skeletonized image. + //! 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(). + //@{ + static Shape splitLine(ShapeSpace &ShS, Region &skelchunk, + Sketch &skeleton, const Sketch &occlusions); + + //! Clears a line from a sketch. + void clearLine(Sketch& sketch); + + void scanHorizForEndPts(const Sketch& skelDist, const Sketch& occlusions, + float m, float b); + 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 vector > extractLines(Sketch const& skel, + Sketch const& occluders, + int const num_lines=10); + + static std::vector > houghTransform(const Sketch& fat, + const Sketch& skinny, + const Sketch& occlusions, + const size_t num_lines, + int minLength=DEFAULT_MIN_LENGTH); + + static bool linesParallel(Shape l1, Shapel2); + + //@} + + //! Comparison predicates used by shape functions + //@{ + + class LengthLessThan : public BinaryShapePred { + public: + bool operator() (const Shape &ln1, const Shape &ln2) const; + }; + + class ParallelTest : public BinaryShapePred { + public: + AngPi tolerance; + ParallelTest(AngPi _tol=mathutils::deg2rad(20.0)) : tolerance(_tol) {} + bool operator() (const Shape &line1, const Shape &line2) const; + }; + + + class PerpendicularTest : public BinaryShapePred { + public: + AngPi tolerance; + PerpendicularTest(AngPi _tol=mathutils::deg2rad(20.0)) : tolerance(_tol) {} + bool operator() (const Shape &line1, const Shape &line2) const; + }; + + + class ColinearTest : public BinaryShapePred { + public: + AngPi ang_tol; + coordinate_t dist_tol; + ColinearTest(AngPi _ang_tol=mathutils::deg2rad(20.0), coordinate_t _dist_tol=10) : + ang_tol(_ang_tol), dist_tol(_dist_tol) {} + bool operator() (const Shape &line1, const Shape &ln2) const; + }; + + class IsHorizontal : public UnaryShapePred { + public: + IsHorizontal(AngPi thresh=M_PI/6) : UnaryShapePred(), threshold(thresh) {} + bool operator() (const Shape &line); + private: + AngPi threshold; + }; + + + //@} + + + virtual Sketch& getRendering(); + +private: + static const int extractorGapTolerance = 8; + + //! Rendering. + //@{ + + //! Render into a sketch space and return reference. + Sketch* render() const; + + //! returns a Sketch which is true where the specified line is + //! end0_stop and end1_stop specify whether rendering should stop at endpoints + // Sketch& drawline2d(SketchSpace &renderspace, + // int x0, int y0, int x1, int y1) const; + void setDrawCoords(float& x1,float& y1, float& x2, float& y2, const int width, const int height) const; + static void drawline2d(Sketch& canvas, int x0, int y0, int x1, int y1); + //@} +}; + +} // namespace + +#endif + diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/Lookout.cc ./DualCoding/Lookout.cc --- ../Tekkotsu_2.4.1/DualCoding/Lookout.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/Lookout.cc 2006-10-02 14:49:16.000000000 -0400 @@ -0,0 +1,609 @@ +//-*-c++-*- + +#include "Events/EventRouter.h" +#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/WorldState.h" +#include "Vision/RegionGenerator.h" + +#include "VRmixin.h" +#include "LookoutRequests.h" +#include "Lookout.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 +} + + +Lookout::Lookout() + : BehaviorBase("Lookout"), + pointer_id(MotionManager::invalid_MC_ID), + sequence_id(MotionManager::invalid_MC_ID), + requests(), curReq(NULL), landmarkInView(true), + idCounter(0) +{} + +void Lookout::DoStart() { + BehaviorBase::DoStart(); +} + +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(sequence_id); + sequence_id = MotionManager::invalid_MC_ID; + BehaviorBase::DoStop(); +} + /* + +vector Lookout::getVisionObjectData() { + +} + +vector Lookout::getIRData() { + +} + */ + +void Lookout::processScan(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); + 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); + } + } + else if (event.getSourceID() == sequence_id) { + sequence_id = MotionManager::invalid_MC_ID; + 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); + storeVisionRegionDataTo(vrt->data,vrt->index,vrt->minArea); + } + else if (task->getTaskType() == ScanRequest::Task::visObj) { + erouter->addListener(this, EventBase::visSegmentEGID, + ProjectInterface::visSegmentSID,EventBase::statusETID); + listeningObjEGID = true; + } + else if (task->getTaskType() == ScanRequest::Task::ir) + storeIRDataTo(task->data); + } + 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(); + it != curScanReq->tasks.end(); it++) + if ((*it)->getTaskType() == ScanRequest::Task::visObj) { + ScanRequest::VisionTask& vTask = *dynamic_cast(*it); + if (vTask.index.find(event.getSourceID()) != vTask.index.end()) { + vTask.data.push_back(findLocationFor(&voe)); + cout << "VisionObject at " << vTask.data.back() << endl; + break; + } + } + } + break; + default: + cout << "Lookout::processScan: unknown event " << event.getName() << endl; + break; + }; +} + +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::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; +} + +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; + } + modeRequest.image[i] = maxChar; + colorCount[i].clear(); + } + return true; + } + else { + for (size_t i = 0; iaddTimer(this, dur_timer, dynamic_cast(curReq)->duration, false); + } + 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; + }; + 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(); + } + break; + default: + cout << "Lookout::processPointAt: unknown event " << event.getName() << endl; + break; + }; +} + /* +void Lookout::processTrackVision(float normX, float normY, bool isCurrentlyVisible) { + if (!isCurrentlyVisible && landmarkInView) { // landmark just lost + cout << "landmark just lost" << endl; + erouter->addTimer(this,start_pan,500,false); // wait 0.5 sec before starting to look for landmark + erouter->removeTimer(this,reset_pan); + } + else if (!landmarkInView && isCurrentlyVisible) { // landmark just found + cout << "found landmark" << endl; + erouter->removeTimer(this,start_pan); + erouter->addTimer(this,reset_pan,1000,false); + } + else if (isCurrentlyVisible) { // continue tracking landmark + trackObjectAt(normX,normY); + } + landmarkInView = isCurrentlyVisible; + lm_location = findLocationFor(normX,normY); + erouter->postEvent(EventBase::lookoutEGID, curReq->getRequestID(), EventBase::statusETID,0); +} + +void Lookout::processTrack(const EventBase& event) { + switch (event.getGeneratorID()) { + case EventBase::visObjEGID: + if (event.getTypeID()==EventBase::statusETID) { + const VisionObjectEvent *voe = (static_cast(&event)); + const float area = voe->getBoundaryArea(); + processTrackVision(voe->getCenterX(), voe->getCenterY(), area > 0.03); + } + break; + case EventBase::visSegmentEGID: + if (event.getTypeID() == EventBase::statusETID) { + vector pts = getRegionData(); + if (pts.empty()) processTrackVision(0,0,false); + else processTrackVision(pts.front().coordX(),pts.front().coordY(),true); + } + break; + case EventBase::timerEGID: + switch (event.getSourceID()) { + case start_pan: //! enough time passed after landmark lost + cout << "landmark not seen for 0.5 sec\n"; + if (curReq->isSticky()) + lookForLandmark(); + else + requestComplete(); + break; + case reset_pan: + cout << "reset pan motion" << endl; + { MMAccessor (sequence_id)->setTime(0); } + break; + default: + cout << "unknown source timer evenet" << endl; + break; + } + case EventBase::motmanEGID: + if (event.getSourceID() == sequence_id && curReq->isSticky()) { // lookForLandmark() failed + cout << "Lookout::processTrack: track failed\n"; + requestComplete(); + } + break; + default: + cout << "Lookout::processTrack: unknown event" << endl; + break; + }; +} + */ +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); +// findLandmarkLocation(voe); +// erouter->postEvent(EventBase::lookoutEGID, curReq->getRequestID(), EventBase::statusETID,0); + double tilt=state->outputs[HeadOffset+TiltOffset]-vert*M_PI/7.5; + double pan=state->outputs[HeadOffset+PanOffset]-horiz*M_PI/6; + if(tiltmathutils::deg2rad(40.0)) + tilt=mathutils::deg2rad(40.0); + if(pan>mathutils::deg2rad(80.0)) + pan=mathutils::deg2rad(80.0); + if(panoutputs[HeadOffset+TiltOffset] << ", nod: " << state->outputs[HeadOffset+NodOffset] << endl; + if (tilt < -0.5) + MMAccessor (pointer_id)->setJoints(tilt,pan,outputRanges[HeadOffset+NodOffset][MinRange]); + else { + const float act_tilt = state->outputs[HeadOffset+TiltOffset]; + const float nod_fact = act_tilt*act_tilt*4.0; + MMAccessor (pointer_id)->setJoints(tilt,pan,outputRanges[HeadOffset+NodOffset][MinRange]*nod_fact); + } +#else + MMAccessor (pointer_id)->setJoints(tilt,pan,0); +#endif +} + */ + +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); +} + +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); + } +} + +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::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); + + // 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 + + 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); + + 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; + + 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); + } + 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); +} + +} // namespace diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/Lookout.h ./DualCoding/Lookout.h --- ../Tekkotsu_2.4.1/DualCoding/Lookout.h 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/Lookout.h 2006-08-10 23:13:15.000000000 -0400 @@ -0,0 +1,116 @@ +//-*-c++-*- +#ifndef INCLUDED_Lookout_h_ +#define INCLUDED_Lookout_h_ + +#include +#include "Behaviors/BehaviorBase.h" +#include "Motion/MotionManager.h" +#include "Motion/MotionSequenceMC.h" +#include "Events/VisionObjectEvent.h" +#include "LookoutRequests.h" + +namespace DualCoding { + +class LookoutRequest; +class StoreModeImageRequest; +class ModeImageRequest; + +/*! + 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. + +*/ + +class Lookout : public BehaviorBase { +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"; } + 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 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; + +protected: + virtual void executeRequest(); + virtual void requestComplete(); + template + void pushRequest(const LookoutRequest& req) { + requests.push(new T(*dynamic_cast(&req))); + } + + + //! PointAtReqeust + void processPointAt(const EventBase& event); + bool findPixelModes(StoreModeImageRequest& modeRequest); + void getData(); + /* + //! 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); + } + + 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; + unsigned int idCounter; + + enum LookoutTimerSourceId_t { + start_pan, + pan_complete, + reset_pan, + no_event, + dur_timer, + scan_timer, //scan_timer has to be at the end of enum list + }; +}; + +} //namespace + +#endif diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/LookoutRequests.cc ./DualCoding/LookoutRequests.cc --- ../Tekkotsu_2.4.1/DualCoding/LookoutRequests.cc 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/LookoutRequests.cc 2006-08-11 18:21:33.000000000 -0400 @@ -0,0 +1,5 @@ +#include "LookoutRequests.h" + +namespace DualCoding { +const float ScanRequest::defSpd = 0.0015; +} diff -urdN --exclude=CVS -I'\$[^$]*:[^$]*\$' ../Tekkotsu_2.4.1/DualCoding/LookoutRequests.h ./DualCoding/LookoutRequests.h --- ../Tekkotsu_2.4.1/DualCoding/LookoutRequests.h 1969-12-31 19:00:00.000000000 -0500 +++ ./DualCoding/LookoutRequests.h 2006-08-11 18:21:33.000000000 -0400 @@ -0,0 +1,317 @@ +//-*-c++-*- +#ifndef INCLUDED_LookoutRequests_h_ +#define INCLUDED_LookoutRequests_h_ + +#include "Shared/newmat/newmat.h" +#include "Shared/ProjectInterface.h" +#include "Shared/WorldState.h" +#include "DualCoding.h" +#include "VRmixin.h" + +namespace DualCoding { + +class LookoutRequest { +public: + enum HeadMotionType_t { + none, + pointAt, + scan, + track + }; + 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) {} + + virtual PointAtRequestType_t getPointAtType() const { return none; } + virtual HeadMotionType_t getHeadMotionType() const { return pointAt; } + Point gazePt; //!< point to look at in base frame coordinates +}; + +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) {} + + 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) {} + + 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) {}; + + //! 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 +}; + +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) {} + + virtual PointAtRequestType_t getPointAtType() const { return storeImage; } + virtual HeadMotionType_t getHeadMotionType() const { return pointAt; } +private: + StoreModeImageAtRequest& operator=(const StoreModeImageAtRequest&); +}; + +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; +}; + +class MeasureDistanceAtReq