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